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.VecBuilder;
009import edu.wpi.first.math.numbers.N1;
010import edu.wpi.first.math.numbers.N2;
011import edu.wpi.first.math.system.LinearSystem;
012import edu.wpi.first.math.system.NumericalIntegration;
013import edu.wpi.first.math.system.plant.DCMotor;
014import edu.wpi.first.math.system.plant.LinearSystemId;
015
016/** Represents a simulated single jointed arm mechanism. */
017public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
018  // The gearbox for the arm.
019  private final DCMotor m_gearbox;
020
021  // The gearing between the motors and the output.
022  private final double m_gearing;
023
024  // The length of the arm.
025  @SuppressWarnings("MemberName")
026  private final double m_r;
027
028  // The minimum angle that the arm is capable of.
029  private final double m_minAngle;
030
031  // The maximum angle that the arm is capable of.
032  private final double m_maxAngle;
033
034  // The mass of the arm.
035  private final double m_armMass;
036
037  // Whether the simulator should simulate gravity.
038  private final boolean m_simulateGravity;
039
040  /**
041   * Creates a simulated arm mechanism.
042   *
043   * @param plant The linear system that represents the arm.
044   * @param gearbox The type of and number of motors in the arm gearbox.
045   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
046   * @param armLengthMeters The length of the arm.
047   * @param minAngleRads The minimum angle that the arm is capable of.
048   * @param maxAngleRads The maximum angle that the arm is capable of.
049   * @param armMassKg The mass of the arm.
050   * @param simulateGravity Whether gravity should be simulated or not.
051   */
052  public SingleJointedArmSim(
053      LinearSystem<N2, N1, N1> plant,
054      DCMotor gearbox,
055      double gearing,
056      double armLengthMeters,
057      double minAngleRads,
058      double maxAngleRads,
059      double armMassKg,
060      boolean simulateGravity) {
061    this(
062        plant,
063        gearbox,
064        gearing,
065        armLengthMeters,
066        minAngleRads,
067        maxAngleRads,
068        armMassKg,
069        simulateGravity,
070        null);
071  }
072
073  /**
074   * Creates a simulated arm mechanism.
075   *
076   * @param plant The linear system that represents the arm.
077   * @param gearbox The type of and number of motors in the arm gearbox.
078   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
079   * @param armLengthMeters The length of the arm.
080   * @param minAngleRads The minimum angle that the arm is capable of.
081   * @param maxAngleRads The maximum angle that the arm is capable of.
082   * @param armMassKg The mass of the arm.
083   * @param simulateGravity Whether gravity should be simulated or not.
084   * @param measurementStdDevs The standard deviations of the measurements.
085   */
086  public SingleJointedArmSim(
087      LinearSystem<N2, N1, N1> plant,
088      DCMotor gearbox,
089      double gearing,
090      double armLengthMeters,
091      double minAngleRads,
092      double maxAngleRads,
093      double armMassKg,
094      boolean simulateGravity,
095      Matrix<N1, N1> measurementStdDevs) {
096    super(plant, measurementStdDevs);
097    m_gearbox = gearbox;
098    m_gearing = gearing;
099    m_r = armLengthMeters;
100    m_minAngle = minAngleRads;
101    m_maxAngle = maxAngleRads;
102    m_armMass = armMassKg;
103    m_simulateGravity = simulateGravity;
104  }
105
106  /**
107   * Creates a simulated arm mechanism.
108   *
109   * @param gearbox The type of and number of motors in the arm gearbox.
110   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
111   * @param jKgMetersSquared The moment of inertia of the arm, can be calculated from CAD software.
112   * @param armLengthMeters The length of the arm.
113   * @param minAngleRads The minimum angle that the arm is capable of.
114   * @param maxAngleRads The maximum angle that the arm is capable of.
115   * @param armMassKg The mass of the arm.
116   * @param simulateGravity Whether gravity should be simulated or not.
117   */
118  @SuppressWarnings("ParameterName")
119  public SingleJointedArmSim(
120      DCMotor gearbox,
121      double gearing,
122      double jKgMetersSquared,
123      double armLengthMeters,
124      double minAngleRads,
125      double maxAngleRads,
126      double armMassKg,
127      boolean simulateGravity) {
128    this(
129        gearbox,
130        gearing,
131        jKgMetersSquared,
132        armLengthMeters,
133        minAngleRads,
134        maxAngleRads,
135        armMassKg,
136        simulateGravity,
137        null);
138  }
139
140  /**
141   * Creates a simulated arm mechanism.
142   *
143   * @param gearbox The type of and number of motors in the arm gearbox.
144   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
145   * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
146   * @param armLengthMeters The length of the arm.
147   * @param minAngleRads The minimum angle that the arm is capable of.
148   * @param maxAngleRads The maximum angle that the arm is capable of.
149   * @param armMassKg The mass of the arm.
150   * @param simulateGravity Whether gravity should be simulated or not.
151   * @param measurementStdDevs The standard deviations of the measurements.
152   */
153  @SuppressWarnings("ParameterName")
154  public SingleJointedArmSim(
155      DCMotor gearbox,
156      double gearing,
157      double jKgMetersSquared,
158      double armLengthMeters,
159      double minAngleRads,
160      double maxAngleRads,
161      double armMassKg,
162      boolean simulateGravity,
163      Matrix<N1, N1> measurementStdDevs) {
164    super(
165        LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
166        measurementStdDevs);
167    m_gearbox = gearbox;
168    m_gearing = gearing;
169    m_r = armLengthMeters;
170    m_minAngle = minAngleRads;
171    m_maxAngle = maxAngleRads;
172    m_armMass = armMassKg;
173    m_simulateGravity = simulateGravity;
174  }
175
176  /**
177   * Returns whether the arm would hit the lower limit.
178   *
179   * @param currentAngleRads The current arm height.
180   * @return Whether the arm would hit the lower limit.
181   */
182  public boolean wouldHitLowerLimit(double currentAngleRads) {
183    return currentAngleRads < this.m_minAngle;
184  }
185
186  /**
187   * Returns whether the arm would hit the upper limit.
188   *
189   * @param currentAngleRads The current arm height.
190   * @return Whether the arm would hit the upper limit.
191   */
192  public boolean wouldHitUpperLimit(double currentAngleRads) {
193    return currentAngleRads > this.m_maxAngle;
194  }
195
196  /**
197   * Returns whether the arm has hit the lower limit.
198   *
199   * @return Whether the arm has hit the lower limit.
200   */
201  public boolean hasHitLowerLimit() {
202    return wouldHitLowerLimit(getAngleRads());
203  }
204
205  /**
206   * Returns whether the arm has hit the upper limit.
207   *
208   * @return Whether the arm has hit the upper limit.
209   */
210  public boolean hasHitUpperLimit() {
211    return wouldHitUpperLimit(getAngleRads());
212  }
213
214  /**
215   * Returns the current arm angle.
216   *
217   * @return The current arm angle.
218   */
219  public double getAngleRads() {
220    return m_y.get(0, 0);
221  }
222
223  /**
224   * Returns the current arm velocity.
225   *
226   * @return The current arm velocity.
227   */
228  public double getVelocityRadPerSec() {
229    return m_x.get(1, 0);
230  }
231
232  /**
233   * Returns the arm current draw.
234   *
235   * @return The aram current draw.
236   */
237  @Override
238  public double getCurrentDrawAmps() {
239    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
240    // spinning 10x faster than the output
241    var motorVelocity = m_x.get(1, 0) * m_gearing;
242    return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
243  }
244
245  /**
246   * Sets the input voltage for the arm.
247   *
248   * @param volts The input voltage.
249   */
250  public void setInputVoltage(double volts) {
251    setInput(volts);
252  }
253
254  /**
255   * Calculates a rough estimate of the moment of inertia of an arm given its length and mass.
256   *
257   * @param lengthMeters The length of the arm.
258   * @param massKg The mass of the arm.
259   * @return The calculated moment of inertia.
260   */
261  public static double estimateMOI(double lengthMeters, double massKg) {
262    return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters;
263  }
264
265  /**
266   * Updates the state of the arm.
267   *
268   * @param currentXhat The current state estimate.
269   * @param u The system inputs (voltage).
270   * @param dtSeconds The time difference between controller updates.
271   */
272  @Override
273  @SuppressWarnings({"ParameterName", "LambdaParameterName"})
274  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
275    // Horizontal case:
276    // Torque = F * r = I * alpha
277    // alpha = F * r / I
278    // Since F = mg,
279    // alpha = m * g * r / I
280    // Finally, multiply RHS by cos(theta) to account for the arm angle
281    // This acceleration is added to the linear system dynamics x-dot = Ax + Bu
282    // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
283    // cos(theta)]]
284    Matrix<N2, N1> updatedXhat =
285        NumericalIntegration.rkdp(
286            (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
287              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
288              if (m_simulateGravity) {
289                xdot =
290                    xdot.plus(
291                        VecBuilder.fill(
292                            0,
293                            m_armMass
294                                * m_r
295                                * -9.8
296                                * 3.0
297                                / (m_armMass * m_r * m_r)
298                                * Math.cos(x.get(0, 0))));
299              }
300              return xdot;
301            },
302            currentXhat,
303            u,
304            dtSeconds);
305
306    // We check for collision after updating xhat
307    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
308      return VecBuilder.fill(m_minAngle, 0);
309    }
310    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
311      return VecBuilder.fill(m_maxAngle, 0);
312    }
313    return updatedXhat;
314  }
315}