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   * Rotates the current pose around a point in 3D space.
276   *
277   * @param point The point in 3D space to rotate around.
278   * @param rot The rotation to rotate the pose by.
279   * @return The new rotated pose.
280   */
281  public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
282    return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
283  }
284
285  /**
286   * Obtain a new Pose3d from a (constant curvature) velocity.
287   *
288   * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
289   * update. When the user runs exp() on the previous known field-relative pose with the argument
290   * being the twist, the user will receive the new field-relative pose.
291   *
292   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
293   * pose forward in time.
294   *
295   * @param twist The change in pose in the robot's coordinate frame since the previous pose update.
296   *     For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
297   *     degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
298   *     Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
299   * @return The new pose of the robot.
300   */
301  public Pose3d exp(Twist3d twist) {
302    var quaternion = this.getRotation().getQuaternion();
303    double[] resultArray =
304        Pose3dJNI.exp(
305            this.getX(),
306            this.getY(),
307            this.getZ(),
308            quaternion.getW(),
309            quaternion.getX(),
310            quaternion.getY(),
311            quaternion.getZ(),
312            twist.dx,
313            twist.dy,
314            twist.dz,
315            twist.rx,
316            twist.ry,
317            twist.rz);
318    return new Pose3d(
319        resultArray[0],
320        resultArray[1],
321        resultArray[2],
322        new Rotation3d(
323            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
324  }
325
326  /**
327   * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
328   * then {@code a.Exp(c)} would yield b.
329   *
330   * @param end The end pose for the transformation.
331   * @return The twist that maps this to end.
332   */
333  public Twist3d log(Pose3d end) {
334    var thisQuaternion = this.getRotation().getQuaternion();
335    var endQuaternion = end.getRotation().getQuaternion();
336    double[] resultArray =
337        Pose3dJNI.log(
338            this.getX(),
339            this.getY(),
340            this.getZ(),
341            thisQuaternion.getW(),
342            thisQuaternion.getX(),
343            thisQuaternion.getY(),
344            thisQuaternion.getZ(),
345            end.getX(),
346            end.getY(),
347            end.getZ(),
348            endQuaternion.getW(),
349            endQuaternion.getX(),
350            endQuaternion.getY(),
351            endQuaternion.getZ());
352    return new Twist3d(
353        resultArray[0],
354        resultArray[1],
355        resultArray[2],
356        resultArray[3],
357        resultArray[4],
358        resultArray[5]);
359  }
360
361  /**
362   * Returns an affine transformation matrix representation of this pose.
363   *
364   * @return An affine transformation matrix representation of this pose.
365   */
366  public Matrix<N4, N4> toMatrix() {
367    var vec = m_translation.toVector();
368    var mat = m_rotation.toMatrix();
369    return MatBuilder.fill(
370        Nat.N4(),
371        Nat.N4(),
372        mat.get(0, 0),
373        mat.get(0, 1),
374        mat.get(0, 2),
375        vec.get(0),
376        mat.get(1, 0),
377        mat.get(1, 1),
378        mat.get(1, 2),
379        vec.get(1),
380        mat.get(2, 0),
381        mat.get(2, 1),
382        mat.get(2, 2),
383        vec.get(2),
384        0.0,
385        0.0,
386        0.0,
387        1.0);
388  }
389
390  /**
391   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
392   *
393   * @return A Pose2d representing this Pose3d projected into the X-Y plane.
394   */
395  public Pose2d toPose2d() {
396    return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
397  }
398
399  @Override
400  public String toString() {
401    return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
402  }
403
404  /**
405   * Checks equality between this Pose3d and another object.
406   *
407   * @param obj The other object.
408   * @return Whether the two objects are equal or not.
409   */
410  @Override
411  public boolean equals(Object obj) {
412    return obj instanceof Pose3d pose
413        && m_translation.equals(pose.m_translation)
414        && m_rotation.equals(pose.m_rotation);
415  }
416
417  @Override
418  public int hashCode() {
419    return Objects.hash(m_translation, m_rotation);
420  }
421
422  @Override
423  public Pose3d interpolate(Pose3d endValue, double t) {
424    if (t < 0) {
425      return this;
426    } else if (t >= 1) {
427      return endValue;
428    } else {
429      var twist = this.log(endValue);
430      var scaledTwist =
431          new Twist3d(
432              twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
433      return this.exp(scaledTwist);
434    }
435  }
436
437  /** Pose3d protobuf for serialization. */
438  public static final Pose3dProto proto = new Pose3dProto();
439
440  /** Pose3d struct for serialization. */
441  public static final Pose3dStruct struct = new Pose3dStruct();
442}