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}