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}