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