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 static edu.wpi.first.units.Units.Meters; 008 009import com.fasterxml.jackson.annotation.JsonAutoDetect; 010import com.fasterxml.jackson.annotation.JsonCreator; 011import com.fasterxml.jackson.annotation.JsonIgnoreProperties; 012import com.fasterxml.jackson.annotation.JsonProperty; 013import edu.wpi.first.math.MatBuilder; 014import edu.wpi.first.math.Matrix; 015import edu.wpi.first.math.Nat; 016import edu.wpi.first.math.geometry.proto.Pose3dProto; 017import edu.wpi.first.math.geometry.struct.Pose3dStruct; 018import edu.wpi.first.math.interpolation.Interpolatable; 019import edu.wpi.first.math.jni.Pose3dJNI; 020import edu.wpi.first.math.numbers.N4; 021import edu.wpi.first.units.measure.Distance; 022import edu.wpi.first.util.protobuf.ProtobufSerializable; 023import edu.wpi.first.util.struct.StructSerializable; 024import java.util.Objects; 025 026/** Represents a 3D pose containing translational and rotational elements. */ 027@JsonIgnoreProperties(ignoreUnknown = true) 028@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) 029public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable { 030 /** 031 * A preallocated Pose3d representing the origin. 032 * 033 * <p>This exists to avoid allocations for common poses. 034 */ 035 public static final Pose3d kZero = new Pose3d(); 036 037 private final Translation3d m_translation; 038 private final Rotation3d m_rotation; 039 040 /** Constructs a pose at the origin facing toward the positive X axis. */ 041 public Pose3d() { 042 m_translation = Translation3d.kZero; 043 m_rotation = Rotation3d.kZero; 044 } 045 046 /** 047 * Constructs a pose with the specified translation and rotation. 048 * 049 * @param translation The translational component of the pose. 050 * @param rotation The rotational component of the pose. 051 */ 052 @JsonCreator 053 public Pose3d( 054 @JsonProperty(required = true, value = "translation") Translation3d translation, 055 @JsonProperty(required = true, value = "rotation") Rotation3d rotation) { 056 m_translation = translation; 057 m_rotation = rotation; 058 } 059 060 /** 061 * Constructs a pose with x, y, and z translations instead of a separate Translation3d. 062 * 063 * @param x The x component of the translational component of the pose. 064 * @param y The y component of the translational component of the pose. 065 * @param z The z component of the translational component of the pose. 066 * @param rotation The rotational component of the pose. 067 */ 068 public Pose3d(double x, double y, double z, Rotation3d rotation) { 069 m_translation = new Translation3d(x, y, z); 070 m_rotation = rotation; 071 } 072 073 /** 074 * Constructs a pose with x, y, and z translations instead of a separate Translation3d. The X, Y, 075 * and Z translations will be converted to and tracked as meters. 076 * 077 * @param x The x component of the translational component of the pose. 078 * @param y The y component of the translational component of the pose. 079 * @param z The z component of the translational component of the pose. 080 * @param rotation The rotational component of the pose. 081 */ 082 public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) { 083 this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); 084 } 085 086 /** 087 * Constructs a pose with the specified affine transformation matrix. 088 * 089 * @param matrix The affine transformation matrix. 090 * @throws IllegalArgumentException if the affine transformation matrix is invalid. 091 */ 092 public Pose3d(Matrix<N4, N4> matrix) { 093 m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); 094 m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); 095 if (matrix.get(3, 0) != 0.0 096 || matrix.get(3, 1) != 0.0 097 || matrix.get(3, 2) != 0.0 098 || matrix.get(3, 3) != 1.0) { 099 throw new IllegalArgumentException("Affine transformation matrix is invalid"); 100 } 101 } 102 103 /** 104 * Constructs a 3D pose from a 2D pose in the X-Y plane. 105 * 106 * @param pose The 2D pose. 107 * @see Rotation3d#Rotation3d(Rotation2d) 108 * @see Translation3d#Translation3d(Translation2d) 109 */ 110 public Pose3d(Pose2d pose) { 111 m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0); 112 m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians()); 113 } 114 115 /** 116 * Transforms the pose by the given transformation and returns the new transformed pose. The 117 * transform is applied relative to the pose's frame. Note that this differs from {@link 118 * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the 119 * origin. 120 * 121 * @param other The transform to transform the pose by. 122 * @return The transformed pose. 123 */ 124 public Pose3d plus(Transform3d other) { 125 return transformBy(other); 126 } 127 128 /** 129 * Returns the Transform3d that maps the one pose to another. 130 * 131 * @param other The initial pose of the transformation. 132 * @return The transform that maps the other pose to the current pose. 133 */ 134 public Transform3d minus(Pose3d other) { 135 final var pose = this.relativeTo(other); 136 return new Transform3d(pose.getTranslation(), pose.getRotation()); 137 } 138 139 /** 140 * Returns the translation component of the transformation. 141 * 142 * @return The translational component of the pose. 143 */ 144 @JsonProperty 145 public Translation3d getTranslation() { 146 return m_translation; 147 } 148 149 /** 150 * Returns the X component of the pose's translation. 151 * 152 * @return The x component of the pose's translation. 153 */ 154 public double getX() { 155 return m_translation.getX(); 156 } 157 158 /** 159 * Returns the Y component of the pose's translation. 160 * 161 * @return The y component of the pose's translation. 162 */ 163 public double getY() { 164 return m_translation.getY(); 165 } 166 167 /** 168 * Returns the Z component of the pose's translation. 169 * 170 * @return The z component of the pose's translation. 171 */ 172 public double getZ() { 173 return m_translation.getZ(); 174 } 175 176 /** 177 * Returns the X component of the pose's translation in a measure. 178 * 179 * @return The x component of the pose's translation in a measure. 180 */ 181 public Distance getMeasureX() { 182 return m_translation.getMeasureX(); 183 } 184 185 /** 186 * Returns the Y component of the pose's translation in a measure. 187 * 188 * @return The y component of the pose's translation in a measure. 189 */ 190 public Distance getMeasureY() { 191 return m_translation.getMeasureY(); 192 } 193 194 /** 195 * Returns the Z component of the pose's translation in a measure. 196 * 197 * @return The z component of the pose's translation in a measure. 198 */ 199 public Distance getMeasureZ() { 200 return m_translation.getMeasureZ(); 201 } 202 203 /** 204 * Returns the rotational component of the transformation. 205 * 206 * @return The rotational component of the pose. 207 */ 208 @JsonProperty 209 public Rotation3d getRotation() { 210 return m_rotation; 211 } 212 213 /** 214 * Multiplies the current pose by a scalar. 215 * 216 * @param scalar The scalar. 217 * @return The new scaled Pose3d. 218 */ 219 public Pose3d times(double scalar) { 220 return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar)); 221 } 222 223 /** 224 * Divides the current pose by a scalar. 225 * 226 * @param scalar The scalar. 227 * @return The new scaled Pose3d. 228 */ 229 public Pose3d div(double scalar) { 230 return times(1.0 / scalar); 231 } 232 233 /** 234 * Rotates the pose around the origin and returns the new pose. 235 * 236 * @param other The rotation to transform the pose by, which is applied extrinsically (from the 237 * global frame). 238 * @return The rotated pose. 239 */ 240 public Pose3d rotateBy(Rotation3d other) { 241 return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other)); 242 } 243 244 /** 245 * Transforms the pose by the given transformation and returns the new transformed pose. The 246 * transform is applied relative to the pose's frame. Note that this differs from {@link 247 * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the 248 * origin. 249 * 250 * @param other The transform to transform the pose by. 251 * @return The transformed pose. 252 */ 253 public Pose3d transformBy(Transform3d other) { 254 return new Pose3d( 255 m_translation.plus(other.getTranslation().rotateBy(m_rotation)), 256 other.getRotation().plus(m_rotation)); 257 } 258 259 /** 260 * Returns the current pose relative to the given pose. 261 * 262 * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to 263 * get the error between the reference and the current pose. 264 * 265 * @param other The pose that is the origin of the new coordinate frame that the current pose will 266 * be converted into. 267 * @return The current pose relative to the new origin pose. 268 */ 269 public Pose3d relativeTo(Pose3d other) { 270 var transform = new Transform3d(other, this); 271 return new Pose3d(transform.getTranslation(), transform.getRotation()); 272 } 273 274 /** 275 * Obtain a new Pose3d from a (constant curvature) velocity. 276 * 277 * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose 278 * update. When the user runs exp() on the previous known field-relative pose with the argument 279 * being the twist, the user will receive the new field-relative pose. 280 * 281 * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the 282 * pose forward in time. 283 * 284 * @param twist The change in pose in the robot's coordinate frame since the previous pose update. 285 * For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5 286 * degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new 287 * Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))). 288 * @return The new pose of the robot. 289 */ 290 public Pose3d exp(Twist3d twist) { 291 var quaternion = this.getRotation().getQuaternion(); 292 double[] resultArray = 293 Pose3dJNI.exp( 294 this.getX(), 295 this.getY(), 296 this.getZ(), 297 quaternion.getW(), 298 quaternion.getX(), 299 quaternion.getY(), 300 quaternion.getZ(), 301 twist.dx, 302 twist.dy, 303 twist.dz, 304 twist.rx, 305 twist.ry, 306 twist.rz); 307 return new Pose3d( 308 resultArray[0], 309 resultArray[1], 310 resultArray[2], 311 new Rotation3d( 312 new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6]))); 313 } 314 315 /** 316 * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)}, 317 * then {@code a.Exp(c)} would yield b. 318 * 319 * @param end The end pose for the transformation. 320 * @return The twist that maps this to end. 321 */ 322 public Twist3d log(Pose3d end) { 323 var thisQuaternion = this.getRotation().getQuaternion(); 324 var endQuaternion = end.getRotation().getQuaternion(); 325 double[] resultArray = 326 Pose3dJNI.log( 327 this.getX(), 328 this.getY(), 329 this.getZ(), 330 thisQuaternion.getW(), 331 thisQuaternion.getX(), 332 thisQuaternion.getY(), 333 thisQuaternion.getZ(), 334 end.getX(), 335 end.getY(), 336 end.getZ(), 337 endQuaternion.getW(), 338 endQuaternion.getX(), 339 endQuaternion.getY(), 340 endQuaternion.getZ()); 341 return new Twist3d( 342 resultArray[0], 343 resultArray[1], 344 resultArray[2], 345 resultArray[3], 346 resultArray[4], 347 resultArray[5]); 348 } 349 350 /** 351 * Returns an affine transformation matrix representation of this pose. 352 * 353 * @return An affine transformation matrix representation of this pose. 354 */ 355 public Matrix<N4, N4> toMatrix() { 356 var vec = m_translation.toVector(); 357 var mat = m_rotation.toMatrix(); 358 return MatBuilder.fill( 359 Nat.N4(), 360 Nat.N4(), 361 mat.get(0, 0), 362 mat.get(0, 1), 363 mat.get(0, 2), 364 vec.get(0), 365 mat.get(1, 0), 366 mat.get(1, 1), 367 mat.get(1, 2), 368 vec.get(1), 369 mat.get(2, 0), 370 mat.get(2, 1), 371 mat.get(2, 2), 372 vec.get(2), 373 0.0, 374 0.0, 375 0.0, 376 1.0); 377 } 378 379 /** 380 * Returns a Pose2d representing this Pose3d projected into the X-Y plane. 381 * 382 * @return A Pose2d representing this Pose3d projected into the X-Y plane. 383 */ 384 public Pose2d toPose2d() { 385 return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d()); 386 } 387 388 @Override 389 public String toString() { 390 return String.format("Pose3d(%s, %s)", m_translation, m_rotation); 391 } 392 393 /** 394 * Checks equality between this Pose3d and another object. 395 * 396 * @param obj The other object. 397 * @return Whether the two objects are equal or not. 398 */ 399 @Override 400 public boolean equals(Object obj) { 401 return obj instanceof Pose3d pose 402 && m_translation.equals(pose.m_translation) 403 && m_rotation.equals(pose.m_rotation); 404 } 405 406 @Override 407 public int hashCode() { 408 return Objects.hash(m_translation, m_rotation); 409 } 410 411 @Override 412 public Pose3d interpolate(Pose3d endValue, double t) { 413 if (t < 0) { 414 return this; 415 } else if (t >= 1) { 416 return endValue; 417 } else { 418 var twist = this.log(endValue); 419 var scaledTwist = 420 new Twist3d( 421 twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t); 422 return this.exp(scaledTwist); 423 } 424 } 425 426 /** Pose3d protobuf for serialization. */ 427 public static final Pose3dProto proto = new Pose3dProto(); 428 429 /** Pose3d struct for serialization. */ 430 public static final Pose3dStruct struct = new Pose3dStruct(); 431}