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.kinematics;
006
007import static edu.wpi.first.units.Units.MetersPerSecond;
008import static edu.wpi.first.units.Units.RadiansPerSecond;
009import static edu.wpi.first.units.Units.Seconds;
010
011import edu.wpi.first.math.geometry.Pose2d;
012import edu.wpi.first.math.geometry.Rotation2d;
013import edu.wpi.first.math.geometry.Translation2d;
014import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
015import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
016import edu.wpi.first.units.Angle;
017import edu.wpi.first.units.Distance;
018import edu.wpi.first.units.Measure;
019import edu.wpi.first.units.Time;
020import edu.wpi.first.units.Velocity;
021
022/**
023 * Represents the speed of a robot chassis. Although this class contains similar members compared to
024 * a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
025 * w.r.t to the robot frame of reference, a ChassisSpeeds object represents a robot's velocity.
026 *
027 * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
028 * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
029 * will often have all three components.
030 */
031public class ChassisSpeeds {
032  /** Velocity along the x-axis. (Fwd is +) */
033  public double vxMetersPerSecond;
034
035  /** Velocity along the y-axis. (Left is +) */
036  public double vyMetersPerSecond;
037
038  /** Represents the angular velocity of the robot frame. (CCW is +) */
039  public double omegaRadiansPerSecond;
040
041  public static final ChassisSpeedsProto proto = new ChassisSpeedsProto();
042  public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct();
043
044  /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
045  public ChassisSpeeds() {}
046
047  /**
048   * Constructs a ChassisSpeeds object.
049   *
050   * @param vxMetersPerSecond Forward velocity.
051   * @param vyMetersPerSecond Sideways velocity.
052   * @param omegaRadiansPerSecond Angular velocity.
053   */
054  public ChassisSpeeds(
055      double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
056    this.vxMetersPerSecond = vxMetersPerSecond;
057    this.vyMetersPerSecond = vyMetersPerSecond;
058    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
059  }
060
061  /**
062   * Constructs a ChassisSpeeds object.
063   *
064   * @param vx Forward velocity.
065   * @param vy Sideways velocity.
066   * @param omega Angular velocity.
067   */
068  public ChassisSpeeds(
069      Measure<Velocity<Distance>> vx,
070      Measure<Velocity<Distance>> vy,
071      Measure<Velocity<Angle>> omega) {
072    this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
073  }
074
075  /**
076   * Discretizes a continuous-time chassis speed.
077   *
078   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
079   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
080   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
081   * along the y-axis, and omega * dt around the z-axis).
082   *
083   * <p>This is useful for compensating for translational skew when translating and rotating a
084   * swerve drivetrain.
085   *
086   * @param vxMetersPerSecond Forward velocity.
087   * @param vyMetersPerSecond Sideways velocity.
088   * @param omegaRadiansPerSecond Angular velocity.
089   * @param dtSeconds The duration of the timestep the speeds should be applied for.
090   * @return Discretized ChassisSpeeds.
091   */
092  public static ChassisSpeeds discretize(
093      double vxMetersPerSecond,
094      double vyMetersPerSecond,
095      double omegaRadiansPerSecond,
096      double dtSeconds) {
097    var desiredDeltaPose =
098        new Pose2d(
099            vxMetersPerSecond * dtSeconds,
100            vyMetersPerSecond * dtSeconds,
101            new Rotation2d(omegaRadiansPerSecond * dtSeconds));
102    var twist = new Pose2d().log(desiredDeltaPose);
103    return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
104  }
105
106  /**
107   * Discretizes a continuous-time chassis speed.
108   *
109   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
110   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
111   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
112   * along the y-axis, and omega * dt around the z-axis).
113   *
114   * <p>This is useful for compensating for translational skew when translating and rotating a
115   * swerve drivetrain.
116   *
117   * @param vx Forward velocity.
118   * @param vy Sideways velocity.
119   * @param omega Angular velocity.
120   * @param dt The duration of the timestep the speeds should be applied for.
121   * @return Discretized ChassisSpeeds.
122   */
123  public static ChassisSpeeds discretize(
124      Measure<Velocity<Distance>> vx,
125      Measure<Velocity<Distance>> vy,
126      Measure<Velocity<Angle>> omega,
127      Measure<Time> dt) {
128    return discretize(
129        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds));
130  }
131
132  /**
133   * Discretizes a continuous-time chassis speed.
134   *
135   * <p>This function converts a continous-time chassis speed into a discrete-time one such that
136   * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
137   * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
138   * along the y-axis, and omega * dt around the z-axis).
139   *
140   * <p>This is useful for compensating for translational skew when translating and rotating a
141   * swerve drivetrain.
142   *
143   * @param continuousSpeeds The continuous speeds.
144   * @param dtSeconds The duration of the timestep the speeds should be applied for.
145   * @return Discretized ChassisSpeeds.
146   */
147  public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
148    return discretize(
149        continuousSpeeds.vxMetersPerSecond,
150        continuousSpeeds.vyMetersPerSecond,
151        continuousSpeeds.omegaRadiansPerSecond,
152        dtSeconds);
153  }
154
155  /**
156   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
157   * object.
158   *
159   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
160   *     Positive x is away from your alliance wall.
161   * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
162   *     Positive y is to your left when standing behind the alliance wall.
163   * @param omegaRadiansPerSecond The angular rate of the robot.
164   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
165   *     considered to be zero when it is facing directly away from your alliance station wall.
166   *     Remember that this should be CCW positive.
167   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
168   */
169  public static ChassisSpeeds fromFieldRelativeSpeeds(
170      double vxMetersPerSecond,
171      double vyMetersPerSecond,
172      double omegaRadiansPerSecond,
173      Rotation2d robotAngle) {
174    // CW rotation into chassis frame
175    var rotated =
176        new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
177    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
178  }
179
180  /**
181   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
182   * object.
183   *
184   * @param vx The component of speed in the x direction relative to the field. Positive x is away
185   *     from your alliance wall.
186   * @param vy The component of speed in the y direction relative to the field. Positive y is to
187   *     your left when standing behind the alliance wall.
188   * @param omega The angular rate of the robot.
189   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
190   *     considered to be zero when it is facing directly away from your alliance station wall.
191   *     Remember that this should be CCW positive.
192   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
193   */
194  public static ChassisSpeeds fromFieldRelativeSpeeds(
195      Measure<Velocity<Distance>> vx,
196      Measure<Velocity<Distance>> vy,
197      Measure<Velocity<Angle>> omega,
198      Rotation2d robotAngle) {
199    return fromFieldRelativeSpeeds(
200        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
201  }
202
203  /**
204   * Converts a user provided field-relative ChassisSpeeds object into a robot-relative
205   * ChassisSpeeds object.
206   *
207   * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
208   *     of reference. Positive x is away from your alliance wall. Positive y is to your left when
209   *     standing behind the alliance wall.
210   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
211   *     considered to be zero when it is facing directly away from your alliance station wall.
212   *     Remember that this should be CCW positive.
213   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
214   */
215  public static ChassisSpeeds fromFieldRelativeSpeeds(
216      ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
217    return fromFieldRelativeSpeeds(
218        fieldRelativeSpeeds.vxMetersPerSecond,
219        fieldRelativeSpeeds.vyMetersPerSecond,
220        fieldRelativeSpeeds.omegaRadiansPerSecond,
221        robotAngle);
222  }
223
224  /**
225   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
226   * object.
227   *
228   * @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
229   *     Positive x is towards the robot's front.
230   * @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
231   *     Positive y is towards the robot's left.
232   * @param omegaRadiansPerSecond The angular rate of the robot.
233   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
234   *     considered to be zero when it is facing directly away from your alliance station wall.
235   *     Remember that this should be CCW positive.
236   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
237   */
238  public static ChassisSpeeds fromRobotRelativeSpeeds(
239      double vxMetersPerSecond,
240      double vyMetersPerSecond,
241      double omegaRadiansPerSecond,
242      Rotation2d robotAngle) {
243    // CCW rotation out of chassis frame
244    var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
245    return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
246  }
247
248  /**
249   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
250   * object.
251   *
252   * @param vx The component of speed in the x direction relative to the robot. Positive x is
253   *     towards the robot's front.
254   * @param vy The component of speed in the y direction relative to the robot. Positive y is
255   *     towards the robot's left.
256   * @param omega The angular rate of the robot.
257   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
258   *     considered to be zero when it is facing directly away from your alliance station wall.
259   *     Remember that this should be CCW positive.
260   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
261   */
262  public static ChassisSpeeds fromRobotRelativeSpeeds(
263      Measure<Velocity<Distance>> vx,
264      Measure<Velocity<Distance>> vy,
265      Measure<Velocity<Angle>> omega,
266      Rotation2d robotAngle) {
267    return fromRobotRelativeSpeeds(
268        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
269  }
270
271  /**
272   * Converts a user provided robot-relative ChassisSpeeds object into a field-relative
273   * ChassisSpeeds object.
274   *
275   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
276   *     of reference. Positive x is towards the robot's front. Positive y is towards the robot's
277   *     left.
278   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
279   *     considered to be zero when it is facing directly away from your alliance station wall.
280   *     Remember that this should be CCW positive.
281   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
282   */
283  public static ChassisSpeeds fromRobotRelativeSpeeds(
284      ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
285    return fromRobotRelativeSpeeds(
286        robotRelativeSpeeds.vxMetersPerSecond,
287        robotRelativeSpeeds.vyMetersPerSecond,
288        robotRelativeSpeeds.omegaRadiansPerSecond,
289        robotAngle);
290  }
291
292  /**
293   * Adds two ChassisSpeeds and returns the sum.
294   *
295   * <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
296   * ChassisSpeeds{3.0, 2.0, 1.0}
297   *
298   * @param other The ChassisSpeeds to add.
299   * @return The sum of the ChassisSpeeds.
300   */
301  public ChassisSpeeds plus(ChassisSpeeds other) {
302    return new ChassisSpeeds(
303        vxMetersPerSecond + other.vxMetersPerSecond,
304        vyMetersPerSecond + other.vyMetersPerSecond,
305        omegaRadiansPerSecond + other.omegaRadiansPerSecond);
306  }
307
308  /**
309   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
310   *
311   * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} =
312   * ChassisSpeeds{4.0, 2.0, 1.0}
313   *
314   * @param other The ChassisSpeeds to subtract.
315   * @return The difference between the two ChassisSpeeds.
316   */
317  public ChassisSpeeds minus(ChassisSpeeds other) {
318    return new ChassisSpeeds(
319        vxMetersPerSecond - other.vxMetersPerSecond,
320        vyMetersPerSecond - other.vyMetersPerSecond,
321        omegaRadiansPerSecond - other.omegaRadiansPerSecond);
322  }
323
324  /**
325   * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
326   * of the ChassisSpeeds.
327   *
328   * @return The inverse of the current ChassisSpeeds.
329   */
330  public ChassisSpeeds unaryMinus() {
331    return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
332  }
333
334  /**
335   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
336   *
337   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0}
338   *
339   * @param scalar The scalar to multiply by.
340   * @return The scaled ChassisSpeeds.
341   */
342  public ChassisSpeeds times(double scalar) {
343    return new ChassisSpeeds(
344        vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
345  }
346
347  /**
348   * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
349   *
350   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5}
351   *
352   * @param scalar The scalar to divide by.
353   * @return The scaled ChassisSpeeds.
354   */
355  public ChassisSpeeds div(double scalar) {
356    return new ChassisSpeeds(
357        vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
358  }
359
360  @Override
361  public String toString() {
362    return String.format(
363        "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
364        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
365  }
366}