Source code

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 com.fasterxml.jackson.annotation.JsonAutoDetect;
008import com.fasterxml.jackson.annotation.JsonCreator;
009import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
010import com.fasterxml.jackson.annotation.JsonProperty;
011import edu.wpi.first.math.WPIMathJNI;
012import edu.wpi.first.math.geometry.proto.Pose3dProto;
013import edu.wpi.first.math.geometry.struct.Pose3dStruct;
014import edu.wpi.first.math.interpolation.Interpolatable;
015import java.util.Objects;
016
017/** Represents a 3D pose containing translational and rotational elements. */
018@JsonIgnoreProperties(ignoreUnknown = true)
019@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
020public class Pose3d implements Interpolatable<Pose3d> {
021  private final Translation3d m_translation;
022  private final Rotation3d m_rotation;
023
024  /** Constructs a pose at the origin facing toward the positive X axis. */
025  public Pose3d() {
026    m_translation = new Translation3d();
027    m_rotation = new Rotation3d();
028  }
029
030  /**
031   * Constructs a pose with the specified translation and rotation.
032   *
033   * @param translation The translational component of the pose.
034   * @param rotation The rotational component of the pose.
035   */
036  @JsonCreator
037  public Pose3d(
038      @JsonProperty(required = true, value = "translation") Translation3d translation,
039      @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
040    m_translation = translation;
041    m_rotation = rotation;
042  }
043
044  /**
045   * Constructs a pose with x, y, and z translations instead of a separate Translation3d.
046   *
047   * @param x The x component of the translational component of the pose.
048   * @param y The y component of the translational component of the pose.
049   * @param z The z component of the translational component of the pose.
050   * @param rotation The rotational component of the pose.
051   */
052  public Pose3d(double x, double y, double z, Rotation3d rotation) {
053    m_translation = new Translation3d(x, y, z);
054    m_rotation = rotation;
055  }
056
057  /**
058   * Constructs a 3D pose from a 2D pose in the X-Y plane.
059   *
060   * @param pose The 2D pose.
061   */
062  public Pose3d(Pose2d pose) {
063    m_translation = new Translation3d(pose.getX(), pose.getY(), 0.0);
064    m_rotation = new Rotation3d(0.0, 0.0, pose.getRotation().getRadians());
065  }
066
067  /**
068   * Transforms the pose by the given transformation and returns the new transformed pose. The
069   * transform is applied relative to the pose's frame. Note that this differs from {@link
070   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
071   * origin.
072   *
073   * @param other The transform to transform the pose by.
074   * @return The transformed pose.
075   */
076  public Pose3d plus(Transform3d other) {
077    return transformBy(other);
078  }
079
080  /**
081   * Returns the Transform3d that maps the one pose to another.
082   *
083   * @param other The initial pose of the transformation.
084   * @return The transform that maps the other pose to the current pose.
085   */
086  public Transform3d minus(Pose3d other) {
087    final var pose = this.relativeTo(other);
088    return new Transform3d(pose.getTranslation(), pose.getRotation());
089  }
090
091  /**
092   * Returns the translation component of the transformation.
093   *
094   * @return The translational component of the pose.
095   */
096  @JsonProperty
097  public Translation3d getTranslation() {
098    return m_translation;
099  }
100
101  /**
102   * Returns the X component of the pose's translation.
103   *
104   * @return The x component of the pose's translation.
105   */
106  public double getX() {
107    return m_translation.getX();
108  }
109
110  /**
111   * Returns the Y component of the pose's translation.
112   *
113   * @return The y component of the pose's translation.
114   */
115  public double getY() {
116    return m_translation.getY();
117  }
118
119  /**
120   * Returns the Z component of the pose's translation.
121   *
122   * @return The z component of the pose's translation.
123   */
124  public double getZ() {
125    return m_translation.getZ();
126  }
127
128  /**
129   * Returns the rotational component of the transformation.
130   *
131   * @return The rotational component of the pose.
132   */
133  @JsonProperty
134  public Rotation3d getRotation() {
135    return m_rotation;
136  }
137
138  /**
139   * Multiplies the current pose by a scalar.
140   *
141   * @param scalar The scalar.
142   * @return The new scaled Pose3d.
143   */
144  public Pose3d times(double scalar) {
145    return new Pose3d(m_translation.times(scalar), m_rotation.times(scalar));
146  }
147
148  /**
149   * Divides the current pose by a scalar.
150   *
151   * @param scalar The scalar.
152   * @return The new scaled Pose3d.
153   */
154  public Pose3d div(double scalar) {
155    return times(1.0 / scalar);
156  }
157
158  /**
159   * Rotates the pose around the origin and returns the new pose.
160   *
161   * @param other The rotation to transform the pose by, which is applied extrinsically (from the
162   *     global frame).
163   * @return The rotated pose.
164   */
165  public Pose3d rotateBy(Rotation3d other) {
166    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
167  }
168
169  /**
170   * Transforms the pose by the given transformation and returns the new transformed pose. The
171   * transform is applied relative to the pose's frame. Note that this differs from {@link
172   * Pose3d#rotateBy(Rotation3d)}, which is applied relative to the global frame and around the
173   * origin.
174   *
175   * @param other The transform to transform the pose by.
176   * @return The transformed pose.
177   */
178  public Pose3d transformBy(Transform3d other) {
179    return new Pose3d(
180        m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
181        other.getRotation().plus(m_rotation));
182  }
183
184  /**
185   * Returns the current pose relative to the given pose.
186   *
187   * <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
188   * get the error between the reference and the current pose.
189   *
190   * @param other The pose that is the origin of the new coordinate frame that the current pose will
191   *     be converted into.
192   * @return The current pose relative to the new origin pose.
193   */
194  public Pose3d relativeTo(Pose3d other) {
195    var transform = new Transform3d(other, this);
196    return new Pose3d(transform.getTranslation(), transform.getRotation());
197  }
198
199  /**
200   * Obtain a new Pose3d from a (constant curvature) velocity.
201   *
202   * <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
203   * update. When the user runs exp() on the previous known field-relative pose with the argument
204   * being the twist, the user will receive the new field-relative pose.
205   *
206   * <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
207   * pose forward in time.
208   *
209   * @param twist The change in pose in the robot's coordinate frame since the previous pose update.
210   *     For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
211   *     degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
212   *     Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
213   * @return The new pose of the robot.
214   */
215  public Pose3d exp(Twist3d twist) {
216    var quaternion = this.getRotation().getQuaternion();
217    double[] resultArray =
218        WPIMathJNI.expPose3d(
219            this.getX(),
220            this.getY(),
221            this.getZ(),
222            quaternion.getW(),
223            quaternion.getX(),
224            quaternion.getY(),
225            quaternion.getZ(),
226            twist.dx,
227            twist.dy,
228            twist.dz,
229            twist.rx,
230            twist.ry,
231            twist.rz);
232    return new Pose3d(
233        resultArray[0],
234        resultArray[1],
235        resultArray[2],
236        new Rotation3d(
237            new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
238  }
239
240  /**
241   * Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
242   * then {@code a.Exp(c)} would yield b.
243   *
244   * @param end The end pose for the transformation.
245   * @return The twist that maps this to end.
246   */
247  public Twist3d log(Pose3d end) {
248    var thisQuaternion = this.getRotation().getQuaternion();
249    var endQuaternion = end.getRotation().getQuaternion();
250    double[] resultArray =
251        WPIMathJNI.logPose3d(
252            this.getX(),
253            this.getY(),
254            this.getZ(),
255            thisQuaternion.getW(),
256            thisQuaternion.getX(),
257            thisQuaternion.getY(),
258            thisQuaternion.getZ(),
259            end.getX(),
260            end.getY(),
261            end.getZ(),
262            endQuaternion.getW(),
263            endQuaternion.getX(),
264            endQuaternion.getY(),
265            endQuaternion.getZ());
266    return new Twist3d(
267        resultArray[0],
268        resultArray[1],
269        resultArray[2],
270        resultArray[3],
271        resultArray[4],
272        resultArray[5]);
273  }
274
275  /**
276   * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
277   *
278   * @return A Pose2d representing this Pose3d projected into the X-Y plane.
279   */
280  public Pose2d toPose2d() {
281    return new Pose2d(m_translation.toTranslation2d(), m_rotation.toRotation2d());
282  }
283
284  @Override
285  public String toString() {
286    return String.format("Pose3d(%s, %s)", m_translation, m_rotation);
287  }
288
289  /**
290   * Checks equality between this Pose3d and another object.
291   *
292   * @param obj The other object.
293   * @return Whether the two objects are equal or not.
294   */
295  @Override
296  public boolean equals(Object obj) {
297    if (obj instanceof Pose3d) {
298      return ((Pose3d) obj).m_translation.equals(m_translation)
299          && ((Pose3d) obj).m_rotation.equals(m_rotation);
300    }
301    return false;
302  }
303
304  @Override
305  public int hashCode() {
306    return Objects.hash(m_translation, m_rotation);
307  }
308
309  @Override
310  public Pose3d interpolate(Pose3d endValue, double t) {
311    if (t < 0) {
312      return this;
313    } else if (t >= 1) {
314      return endValue;
315    } else {
316      var twist = this.log(endValue);
317      var scaledTwist =
318          new Twist3d(
319              twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
320      return this.exp(scaledTwist);
321    }
322  }
323
324  public static final Pose3dStruct struct = new Pose3dStruct();
325  public static final Pose3dProto proto = new Pose3dProto();
326}