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