001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.geometry; 006 007import com.fasterxml.jackson.annotation.JsonAutoDetect; 008import com.fasterxml.jackson.annotation.JsonCreator; 009import com.fasterxml.jackson.annotation.JsonIgnoreProperties; 010import com.fasterxml.jackson.annotation.JsonProperty; 011import edu.wpi.first.math.WPIMathJNI; 012import edu.wpi.first.math.geometry.proto.Pose3dProto; 013import edu.wpi.first.math.geometry.struct.Pose3dStruct; 014import edu.wpi.first.math.interpolation.Interpolatable; 015import edu.wpi.first.util.protobuf.ProtobufSerializable; 016import edu.wpi.first.util.struct.StructSerializable; 017import java.util.Objects; 018 019/** Represents a 3D pose containing translational and rotational elements. */ 020@JsonIgnoreProperties(ignoreUnknown = true) 021@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 022public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable { 023 /** 024 * A preallocated Pose3d representing the origin. 025 * 026 * <p>This exists to avoid allocations for common poses. 027 */ 028 public static final Pose3d kZero = new Pose3d(); 029 030 private final Translation3d m_translation; 031 private final Rotation3d m_rotation; 032 033 /** Constructs a pose at the origin facing toward the positive X axis. */ 034 public Pose3d() { 035 m_translation = Translation3d.kZero; 036 m_rotation = Rotation3d.kZero; 037 } 038 039 /** 040 * Constructs a pose with the specified translation and rotation. 041 * 042 * @param translation The translational component of the pose. 043 * @param rotation The rotational component of the pose. 044 */ 045 @JsonCreator 046 public Pose3d( 047 @JsonProperty(required = true, value = "translation") Translation3d translation, 048 @JsonProperty(required = true, value = "rotation") Rotation3d rotation) { 049 m_translation = translation; 050 m_rotation = rotation; 051 } 052 053 /** 054 * Constructs a pose with x, y, and z translations instead of a separate Translation3d. 055 * 056 * @param x The x component of the translational component of the pose. 057 * @param y The y component of the translational component of the pose. 058 * @param z The z component of the translational component of the pose. 059 * @param rotation The rotational component of the pose. 060 */ 061 public Pose3d(double x, double y, double z, Rotation3d rotation) { 062 m_translation = new Translation3d(x, y, z); 063 m_rotation = rotation; 064 } 065 066 /** 067 * Constructs a 3D pose from a 2D pose in the X-Y plane. 068 * 069 * @param pose The 2D pose. 070 */ 071 public Pose3d(Pose2d pose) { 072 m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0); 073 m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians()); 074 } 075 076 /** 077 * Transforms the pose by the given transformation and returns the new transformed pose. The 078 * transform is applied relative to the pose's frame. Note that this differs from {@link 079 * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the 080 * origin. 081 * 082 * @param other The transform to transform the pose by. 083 * @return The transformed pose. 084 */ 085 public Pose3d plus(Transform3d other) { 086 return transformBy(other); 087 } 088 089 /** 090 * Returns the Transform3d that maps the one pose to another. 091 * 092 * @param other The initial pose of the transformation. 093 * @return The transform that maps the other pose to the current pose. 094 */ 095 public Transform3d minus(Pose3d other) { 096 final var pose = this.relativeTo(other); 097 return new Transform3d(pose.getTranslation(), pose.getRotation()); 098 } 099 100 /** 101 * Returns the translation component of the transformation. 102 * 103 * @return The translational component of the pose. 104 */ 105 @JsonProperty 106 public Translation3d getTranslation() { 107 return m_translation; 108 } 109 110 /** 111 * Returns the X component of the pose's translation. 112 * 113 * @return The x component of the pose's translation. 114 */ 115 public double getX() { 116 return m_translation.getX(); 117 } 118 119 /** 120 * Returns the Y component of the pose's translation. 121 * 122 * @return The y component of the pose's translation. 123 */ 124 public double getY() { 125 return m_translation.getY(); 126 } 127 128 /** 129 * Returns the Z component of the pose's translation. 130 * 131 * @return The z component of the pose's translation. 132 */ 133 public double getZ() { 134 return m_translation.getZ(); 135 } 136 137 /** 138 * Returns the rotational component of the transformation. 139 * 140 * @return The rotational component of the pose. 141 */ 142 @JsonProperty 143 public Rotation3d getRotation() { 144 return m_rotation; 145 } 146 147 /** 148 * Multiplies the current pose by a scalar. 149 * 150 * @param scalar The scalar. 151 * @return The new scaled Pose3d. 152 */ 153 public Pose3d times(double scalar) { 154 return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar)); 155 } 156 157 /** 158 * Divides the current pose by a scalar. 159 * 160 * @param scalar The scalar. 161 * @return The new scaled Pose3d. 162 */ 163 public Pose3d div(double scalar) { 164 return times(1.0 / scalar); 165 } 166 167 /** 168 * Rotates the pose around the origin and returns the new pose. 169 * 170 * @param other The rotation to transform the pose by, which is applied extrinsically (from the 171 * global frame). 172 * @return The rotated pose. 173 */ 174 public Pose3d rotateBy(Rotation3d other) { 175 return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other)); 176 } 177 178 /** 179 * Transforms the pose by the given transformation and returns the new transformed pose. The 180 * transform is applied relative to the pose's frame. Note that this differs from {@link 181 * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the 182 * origin. 183 * 184 * @param other The transform to transform the pose by. 185 * @return The transformed pose. 186 */ 187 public Pose3d transformBy(Transform3d other) { 188 return new Pose3d( 189 m_translation.plus(other.getTranslation().rotateBy(m_rotation)), 190 other.getRotation().plus(m_rotation)); 191 } 192 193 /** 194 * Returns the current pose relative to the given pose. 195 * 196 * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to 197 * get the error between the reference and the current pose. 198 * 199 * @param other The pose that is the origin of the new coordinate frame that the current pose will 200 * be converted into. 201 * @return The current pose relative to the new origin pose. 202 */ 203 public Pose3d relativeTo(Pose3d other) { 204 var transform = new Transform3d(other, this); 205 return new Pose3d(transform.getTranslation(), transform.getRotation()); 206 } 207 208 /** 209 * Obtain a new Pose3d from a (constant curvature) velocity. 210 * 211 * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose 212 * update. When the user runs exp() on the previous known field-relative pose with the argument 213 * being the twist, the user will receive the new field-relative pose. 214 * 215 * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the 216 * pose forward in time. 217 * 218 * @param twist The change in pose in the robot's coordinate frame since the previous pose update. 219 * For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5 220 * degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new 221 * Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))). 222 * @return The new pose of the robot. 223 */ 224 public Pose3d exp(Twist3d twist) { 225 var quaternion = this.getRotation().getQuaternion(); 226 double[] resultArray = 227 WPIMathJNI.expPose3d( 228 this.getX(), 229 this.getY(), 230 this.getZ(), 231 quaternion.getW(), 232 quaternion.getX(), 233 quaternion.getY(), 234 quaternion.getZ(), 235 twist.dx, 236 twist.dy, 237 twist.dz, 238 twist.rx, 239 twist.ry, 240 twist.rz); 241 return new Pose3d( 242 resultArray[0], 243 resultArray[1], 244 resultArray[2], 245 new Rotation3d( 246 new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6]))); 247 } 248 249 /** 250 * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)}, 251 * then {@code a.Exp(c)} would yield b. 252 * 253 * @param end The end pose for the transformation. 254 * @return The twist that maps this to end. 255 */ 256 public Twist3d log(Pose3d end) { 257 var thisQuaternion = this.getRotation().getQuaternion(); 258 var endQuaternion = end.getRotation().getQuaternion(); 259 double[] resultArray = 260 WPIMathJNI.logPose3d( 261 this.getX(), 262 this.getY(), 263 this.getZ(), 264 thisQuaternion.getW(), 265 thisQuaternion.getX(), 266 thisQuaternion.getY(), 267 thisQuaternion.getZ(), 268 end.getX(), 269 end.getY(), 270 end.getZ(), 271 endQuaternion.getW(), 272 endQuaternion.getX(), 273 endQuaternion.getY(), 274 endQuaternion.getZ()); 275 return new Twist3d( 276 resultArray[0], 277 resultArray[1], 278 resultArray[2], 279 resultArray[3], 280 resultArray[4], 281 resultArray[5]); 282 } 283 284 /** 285 * Returns a Pose2d representing this Pose3d projected into the X-Y plane. 286 * 287 * @return A Pose2d representing this Pose3d projected into the X-Y plane. 288 */ 289 public Pose2d toPose2d() { 290 return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d()); 291 } 292 293 @Override 294 public String toString() { 295 return String.format("Pose3d(%s, %s)", m_translation, m_rotation); 296 } 297 298 /** 299 * Checks equality between this Pose3d and another object. 300 * 301 * @param obj The other object. 302 * @return Whether the two objects are equal or not. 303 */ 304 @Override 305 public boolean equals(Object obj) { 306 if (obj instanceof Pose3d) { 307 return ((Pose3d) obj).m_translation.equals(m_translation) 308 && ((Pose3d) obj).m_rotation.equals(m_rotation); 309 } 310 return false; 311 } 312 313 @Override 314 public int hashCode() { 315 return Objects.hash(m_translation, m_rotation); 316 } 317 318 @Override 319 public Pose3d interpolate(Pose3d endValue, double t) { 320 if (t < 0) { 321 return this; 322 } else if (t >= 1) { 323 return endValue; 324 } else { 325 var twist = this.log(endValue); 326 var scaledTwist = 327 new Twist3d( 328 twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t); 329 return this.exp(scaledTwist); 330 } 331 } 332 333 /** Pose3d protobuf for serialization. */ 334 public static final Pose3dProto proto = new Pose3dProto(); 335 336 /** Pose3d struct for serialization. */ 337 public static final Pose3dStruct struct = new Pose3dStruct(); 338}