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.wpilibj.simulation; 006 007import edu.wpi.first.math.Matrix; 008import edu.wpi.first.math.Nat; 009import edu.wpi.first.math.StateSpaceUtil; 010import edu.wpi.first.math.VecBuilder; 011import edu.wpi.first.math.geometry.Pose2d; 012import edu.wpi.first.math.geometry.Rotation2d; 013import edu.wpi.first.math.numbers.N1; 014import edu.wpi.first.math.numbers.N2; 015import edu.wpi.first.math.numbers.N7; 016import edu.wpi.first.math.system.LinearSystem; 017import edu.wpi.first.math.system.NumericalIntegration; 018import edu.wpi.first.math.system.plant.DCMotor; 019import edu.wpi.first.math.system.plant.LinearSystemId; 020import edu.wpi.first.math.util.Units; 021import edu.wpi.first.wpilibj.RobotController; 022 023/** 024 * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set 025 * inputs from motors with {@link #setInputs(double, double)}, call {@link #update(double)} to 026 * update the simulation, and set estimated encoder and gyro positions, as well as estimated 027 * odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize 028 * their robot on the Sim GUI's field. 029 * 030 * <p>Our state-space system is: 031 * 032 * <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are 033 * wheel distances.) 034 * 035 * <p>u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a 036 * LTVDiffDriveController. 037 * 038 * <p>y = x 039 */ 040public class DifferentialDrivetrainSim { 041 private final DCMotor m_motor; 042 private final double m_originalGearing; 043 private final Matrix<N7, N1> m_measurementStdDevs; 044 private double m_currentGearing; 045 private final double m_wheelRadius; 046 047 private Matrix<N2, N1> m_u; 048 private Matrix<N7, N1> m_x; 049 private Matrix<N7, N1> m_y; 050 051 private final double m_rb; 052 private final LinearSystem<N2, N2, N2> m_plant; 053 054 /** 055 * Creates a simulated differential drivetrain. 056 * 057 * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain. 058 * @param gearing The gearing ratio between motor and wheel, as output over input. This must be 059 * the same ratio as the ratio used to identify or create the drivetrainPlant. 060 * @param j The moment of inertia of the drivetrain about its center in kg-m². 061 * @param mass The mass of the drivebase in kg. 062 * @param wheelRadius The radius of the wheels on the drivetrain in meters. 063 * @param trackwidth The robot's trackwidth, or distance between left and right wheels, in meters. 064 * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, 065 * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is 066 * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 067 * m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting 068 * point. 069 */ 070 public DifferentialDrivetrainSim( 071 DCMotor driveMotor, 072 double gearing, 073 double j, 074 double mass, 075 double wheelRadius, 076 double trackwidth, 077 Matrix<N7, N1> measurementStdDevs) { 078 this( 079 LinearSystemId.createDrivetrainVelocitySystem( 080 driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing), 081 driveMotor, 082 gearing, 083 trackwidth, 084 wheelRadius, 085 measurementStdDevs); 086 } 087 088 /** 089 * Creates a simulated differential drivetrain. 090 * 091 * @param plant The {@link LinearSystem} representing the robot's drivetrain. This system can be 092 * created with {@link 093 * edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, 094 * double, double, double, double, double)} or {@link 095 * edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, 096 * double, double)}. 097 * @param driveMotor A {@link DCMotor} representing the drivetrain. 098 * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same 099 * ratio as the ratio used to identify or create the drivetrainPlant. 100 * @param trackwidth The distance between the two sides of the drivetrain in meters. Can be found 101 * with SysId. 102 * @param wheelRadius The radius of the wheels on the drivetrain, in meters. 103 * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, 104 * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is 105 * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 106 * m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting 107 * point. 108 */ 109 public DifferentialDrivetrainSim( 110 LinearSystem<N2, N2, N2> plant, 111 DCMotor driveMotor, 112 double gearing, 113 double trackwidth, 114 double wheelRadius, 115 Matrix<N7, N1> measurementStdDevs) { 116 this.m_plant = plant; 117 this.m_rb = trackwidth / 2.0; 118 this.m_motor = driveMotor; 119 this.m_originalGearing = gearing; 120 this.m_measurementStdDevs = measurementStdDevs; 121 m_wheelRadius = wheelRadius; 122 m_currentGearing = m_originalGearing; 123 124 m_x = new Matrix<>(Nat.N7(), Nat.N1()); 125 m_u = VecBuilder.fill(0, 0); 126 m_y = new Matrix<>(Nat.N7(), Nat.N1()); 127 } 128 129 /** 130 * Sets the applied voltage to the drivetrain. Note that positive voltage must make that side of 131 * the drivetrain travel forward (+X). 132 * 133 * @param leftVoltageVolts The left voltage. 134 * @param rightVoltageVolts The right voltage. 135 */ 136 public void setInputs(double leftVoltageVolts, double rightVoltageVolts) { 137 m_u = clampInput(VecBuilder.fill(leftVoltageVolts, rightVoltageVolts)); 138 } 139 140 /** 141 * Update the drivetrain states with the current time difference. 142 * 143 * @param dt the time difference in seconds 144 */ 145 public void update(double dt) { 146 m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt); 147 m_y = m_x; 148 if (m_measurementStdDevs != null) { 149 m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); 150 } 151 } 152 153 /** Returns the full simulated state of the drivetrain. */ 154 Matrix<N7, N1> getState() { 155 return m_x; 156 } 157 158 /** 159 * Get one of the drivetrain states. 160 * 161 * @param state the state to get 162 * @return the state 163 */ 164 double getState(State state) { 165 return m_x.get(state.value, 0); 166 } 167 168 private double getOutput(State output) { 169 return m_y.get(output.value, 0); 170 } 171 172 /** 173 * Returns the direction the robot is pointing. 174 * 175 * <p>Note that this angle is counterclockwise-positive, while most gyros are clockwise positive. 176 * 177 * @return The direction the robot is pointing. 178 */ 179 public Rotation2d getHeading() { 180 return new Rotation2d(getOutput(State.kHeading)); 181 } 182 183 /** 184 * Returns the current pose. 185 * 186 * @return The current pose. 187 */ 188 public Pose2d getPose() { 189 return new Pose2d(getOutput(State.kX), getOutput(State.kY), getHeading()); 190 } 191 192 /** 193 * Get the right encoder position. 194 * 195 * @return The encoder position in meters. 196 */ 197 public double getRightPosition() { 198 return getOutput(State.kRightPosition); 199 } 200 201 /** 202 * Get the right encoder velocity. 203 * 204 * @return The encoder velocity in meters per second. 205 */ 206 public double getRightVelocity() { 207 return getOutput(State.kRightVelocity); 208 } 209 210 /** 211 * Get the left encoder position. 212 * 213 * @return The encoder position in meters. 214 */ 215 public double getLeftPosition() { 216 return getOutput(State.kLeftPosition); 217 } 218 219 /** 220 * Get the left encoder velocity. 221 * 222 * @return The encoder velocity in meters per second. 223 */ 224 public double getLeftVelocity() { 225 return getOutput(State.kLeftVelocity); 226 } 227 228 /** 229 * Get the current draw of the left side of the drivetrain. 230 * 231 * @return the drivetrain's left side current draw, in amps 232 */ 233 public double getLeftCurrentDraw() { 234 return m_motor.getCurrent( 235 getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0)) 236 * Math.signum(m_u.get(0, 0)); 237 } 238 239 /** 240 * Get the current draw of the right side of the drivetrain. 241 * 242 * @return the drivetrain's right side current draw, in amps 243 */ 244 public double getRightCurrentDraw() { 245 return m_motor.getCurrent( 246 getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0)) 247 * Math.signum(m_u.get(1, 0)); 248 } 249 250 /** 251 * Get the current draw of the drivetrain. 252 * 253 * @return the current draw, in amps 254 */ 255 public double getCurrentDraw() { 256 return getLeftCurrentDraw() + getRightCurrentDraw(); 257 } 258 259 /** 260 * Get the drivetrain gearing. 261 * 262 * @return the gearing ration 263 */ 264 public double getCurrentGearing() { 265 return m_currentGearing; 266 } 267 268 /** 269 * Sets the gearing reduction on the drivetrain. This is commonly used for shifting drivetrains. 270 * 271 * @param newGearRatio The new gear ratio, as output over input. 272 */ 273 public void setCurrentGearing(double newGearRatio) { 274 this.m_currentGearing = newGearRatio; 275 } 276 277 /** 278 * Sets the system state. 279 * 280 * @param state The state. 281 */ 282 public void setState(Matrix<N7, N1> state) { 283 m_x = state; 284 } 285 286 /** 287 * Sets the system pose. 288 * 289 * @param pose The pose. 290 */ 291 public void setPose(Pose2d pose) { 292 m_x.set(State.kX.value, 0, pose.getX()); 293 m_x.set(State.kY.value, 0, pose.getY()); 294 m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians()); 295 m_x.set(State.kLeftPosition.value, 0, 0); 296 m_x.set(State.kRightPosition.value, 0, 0); 297 } 298 299 /** 300 * The differential drive dynamics function. 301 * 302 * @param x The state. 303 * @param u The input. 304 * @return The state derivative with respect to time. 305 */ 306 protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) { 307 // Because G can be factored out of B, we can divide by the old ratio and multiply 308 // by the new ratio to get a new drivetrain model. 309 var B = new Matrix<>(Nat.N4(), Nat.N2()); 310 B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing)); 311 312 // Because G² can be factored out of A, we can divide by the old ratio squared and multiply 313 // by the new ratio squared to get a new drivetrain model. 314 var A = new Matrix<>(Nat.N4(), Nat.N4()); 315 A.assignBlock( 316 0, 317 0, 318 m_plant 319 .getA() 320 .times( 321 (this.m_currentGearing * this.m_currentGearing) 322 / (this.m_originalGearing * this.m_originalGearing))); 323 324 A.assignBlock(2, 0, Matrix.eye(Nat.N2())); 325 326 var v = (x.get(State.kLeftVelocity.value, 0) + x.get(State.kRightVelocity.value, 0)) / 2.0; 327 328 var xdot = new Matrix<>(Nat.N7(), Nat.N1()); 329 xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0))); 330 xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0))); 331 xdot.set( 332 2, 333 0, 334 (x.get(State.kRightVelocity.value, 0) - x.get(State.kLeftVelocity.value, 0)) 335 / (2.0 * m_rb)); 336 xdot.assignBlock(3, 0, A.times(x.block(Nat.N4(), Nat.N1(), 3, 0)).plus(B.times(u))); 337 338 return xdot; 339 } 340 341 /** 342 * Clamp the input vector such that no element exceeds the battery voltage. If any does, the 343 * relative magnitudes of the input will be maintained. 344 * 345 * @param u The input vector. 346 * @return The normalized input. 347 */ 348 protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) { 349 return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage()); 350 } 351 352 /** Represents the different states of the drivetrain. */ 353 enum State { 354 kX(0), 355 kY(1), 356 kHeading(2), 357 kLeftVelocity(3), 358 kRightVelocity(4), 359 kLeftPosition(5), 360 kRightPosition(6); 361 362 public final int value; 363 364 State(int i) { 365 this.value = i; 366 } 367 } 368 369 /** 370 * Represents a gearing option of the Toughbox mini. 12.75:1 -- 14:50 and 14:50 10.71:1 -- 14:50 371 * and 16:48 8.45:1 -- 14:50 and 19:45 7.31:1 -- 14:50 and 21:43 5.95:1 -- 14:50 and 24:40 372 */ 373 public enum KitbotGearing { 374 /** Gear ratio of 12.75:1. */ 375 k12p75(12.75), 376 /** Gear ratio of 10.71:1. */ 377 k10p71(10.71), 378 /** Gear ratio of 8.45:1. */ 379 k8p45(8.45), 380 /** Gear ratio of 7.31:1. */ 381 k7p31(7.31), 382 /** Gear ratio of 5.95:1. */ 383 k5p95(5.95); 384 385 /** KitbotGearing value. */ 386 public final double value; 387 388 KitbotGearing(double i) { 389 this.value = i; 390 } 391 } 392 393 /** Represents common motor layouts of the kit drivetrain. */ 394 public enum KitbotMotor { 395 /** One CIM motor per drive side. */ 396 kSingleCIMPerSide(DCMotor.getCIM(1)), 397 /** Two CIM motors per drive side. */ 398 kDualCIMPerSide(DCMotor.getCIM(2)), 399 /** One Mini CIM motor per drive side. */ 400 kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)), 401 /** Two Mini CIM motors per drive side. */ 402 kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)), 403 /** One Falcon 500 motor per drive side. */ 404 kSingleFalcon500PerSide(DCMotor.getFalcon500(1)), 405 /** Two Falcon 500 motors per drive side. */ 406 kDoubleFalcon500PerSide(DCMotor.getFalcon500(2)), 407 /** One NEO motor per drive side. */ 408 kSingleNEOPerSide(DCMotor.getNEO(1)), 409 /** Two NEO motors per drive side. */ 410 kDoubleNEOPerSide(DCMotor.getNEO(2)); 411 412 /** KitbotMotor value. */ 413 public final DCMotor value; 414 415 KitbotMotor(DCMotor i) { 416 this.value = i; 417 } 418 } 419 420 /** Represents common wheel sizes of the kit drivetrain. */ 421 public enum KitbotWheelSize { 422 /** Six inch diameter wheels. */ 423 kSixInch(Units.inchesToMeters(6)), 424 /** Eight inch diameter wheels. */ 425 kEightInch(Units.inchesToMeters(8)), 426 /** Ten inch diameter wheels. */ 427 kTenInch(Units.inchesToMeters(10)); 428 429 /** KitbotWheelSize value. */ 430 public final double value; 431 432 KitbotWheelSize(double i) { 433 this.value = i; 434 } 435 } 436 437 /** 438 * Create a sim for the standard FRC kitbot. 439 * 440 * @param motor The motors installed in the bot. 441 * @param gearing The gearing reduction used. 442 * @param wheelSize The wheel size. 443 * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, 444 * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is 445 * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 446 * m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting 447 * point. 448 * @return A sim for the standard FRC kitbot. 449 */ 450 public static DifferentialDrivetrainSim createKitbotSim( 451 KitbotMotor motor, 452 KitbotGearing gearing, 453 KitbotWheelSize wheelSize, 454 Matrix<N7, N1> measurementStdDevs) { 455 // MOI estimation -- note that I = mr² for point masses 456 var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2); 457 var gearboxMoi = 458 (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */) 459 * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2); 460 461 return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi, measurementStdDevs); 462 } 463 464 /** 465 * Create a sim for the standard FRC kitbot. 466 * 467 * @param motor The motors installed in the bot. 468 * @param gearing The gearing reduction used. 469 * @param wheelSize The wheel size. 470 * @param j The moment of inertia of the drivebase in kg-m². This can be calculated using SysId. 471 * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, 472 * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is 473 * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 474 * m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting 475 * point. 476 * @return A sim for the standard FRC kitbot. 477 */ 478 public static DifferentialDrivetrainSim createKitbotSim( 479 KitbotMotor motor, 480 KitbotGearing gearing, 481 KitbotWheelSize wheelSize, 482 double j, 483 Matrix<N7, N1> measurementStdDevs) { 484 return new DifferentialDrivetrainSim( 485 motor.value, 486 gearing.value, 487 j, 488 Units.lbsToKilograms(60), 489 wheelSize.value / 2.0, 490 Units.inchesToMeters(26), 491 measurementStdDevs); 492 } 493}