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