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}