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