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}