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