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.math.system.plant;
006
007import edu.wpi.first.math.MatBuilder;
008import edu.wpi.first.math.Matrix;
009import edu.wpi.first.math.Nat;
010import edu.wpi.first.math.VecBuilder;
011import edu.wpi.first.math.numbers.N1;
012import edu.wpi.first.math.numbers.N2;
013import edu.wpi.first.math.system.LinearSystem;
014
015/** Linear system ID utility functions. */
016public final class LinearSystemId {
017  private LinearSystemId() {
018    // Utility class
019  }
020
021  /**
022   * Create a state-space model of an elevator system. The states of the system are [position,
023   * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
024   *
025   * @param motor The motor (or gearbox) attached to the carriage.
026   * @param mass The mass of the elevator carriage, in kilograms.
027   * @param radius The radius of the elevator's driving drum, in meters.
028   * @param gearing The reduction between motor and drum, as a ratio of output to input.
029   * @return A LinearSystem representing the given characterized constants.
030   * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0.
031   */
032  public static LinearSystem<N2, N1, N2> createElevatorSystem(
033      DCMotor motor, double mass, double radius, double gearing) {
034    if (mass <= 0.0) {
035      throw new IllegalArgumentException("mass must be greater than zero.");
036    }
037    if (radius <= 0.0) {
038      throw new IllegalArgumentException("radius must be greater than zero.");
039    }
040    if (gearing <= 0) {
041      throw new IllegalArgumentException("gearing must be greater than zero.");
042    }
043
044    return new LinearSystem<>(
045        MatBuilder.fill(
046            Nat.N2(),
047            Nat.N2(),
048            0,
049            1,
050            0,
051            -Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)),
052        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)),
053        Matrix.eye(Nat.N2()),
054        new Matrix<>(Nat.N2(), Nat.N1()));
055  }
056
057  /**
058   * Create a state-space model of a flywheel system. The states of the system are [angular
059   * velocity], inputs are [voltage], and outputs are [angular velocity].
060   *
061   * @param motor The motor (or gearbox) attached to the flywheel.
062   * @param J The moment of inertia J of the flywheel in kg-m².
063   * @param gearing The reduction between motor and drum, as a ratio of output to input.
064   * @return A LinearSystem representing the given characterized constants.
065   * @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
066   */
067  public static LinearSystem<N1, N1, N1> createFlywheelSystem(
068      DCMotor motor, double J, double gearing) {
069    if (J <= 0.0) {
070      throw new IllegalArgumentException("J must be greater than zero.");
071    }
072    if (gearing <= 0.0) {
073      throw new IllegalArgumentException("gearing must be greater than zero.");
074    }
075
076    return new LinearSystem<>(
077        VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
078        VecBuilder.fill(gearing * motor.Kt / (motor.R * J)),
079        Matrix.eye(Nat.N1()),
080        new Matrix<>(Nat.N1(), Nat.N1()));
081  }
082
083  /**
084   * Create a state-space model of a DC motor system. The states of the system are [angular
085   * position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
086   * velocity]ᵀ.
087   *
088   * @param motor The motor (or gearbox) attached to system.
089   * @param J The moment of inertia J of the DC motor in kg-m².
090   * @param gearing The reduction between motor and drum, as a ratio of output to input.
091   * @return A LinearSystem representing the given characterized constants.
092   * @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
093   */
094  public static LinearSystem<N2, N1, N2> createDCMotorSystem(
095      DCMotor motor, double J, double gearing) {
096    if (J <= 0.0) {
097      throw new IllegalArgumentException("J must be greater than zero.");
098    }
099    if (gearing <= 0.0) {
100      throw new IllegalArgumentException("gearing must be greater than zero.");
101    }
102
103    return new LinearSystem<>(
104        MatBuilder.fill(
105            Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
106        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
107        Matrix.eye(Nat.N2()),
108        new Matrix<>(Nat.N2(), Nat.N1()));
109  }
110
111  /**
112   * Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
113   * (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
114   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
115   *
116   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
117   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
118   *
119   * <p>The parameters provided by the user are from this feedforward model:
120   *
121   * <p>u = K_v v + K_a a
122   *
123   * @param kV The velocity gain, in volts/(unit/sec)
124   * @param kA The acceleration gain, in volts/(unit/sec²)
125   * @return A LinearSystem representing the given characterized constants.
126   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
127   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
128   */
129  public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
130    if (kV < 0.0) {
131      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
132    }
133    if (kA <= 0.0) {
134      throw new IllegalArgumentException("Ka must be greater than zero.");
135    }
136
137    return new LinearSystem<>(
138        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
139        VecBuilder.fill(0, 1 / kA),
140        Matrix.eye(Nat.N2()),
141        new Matrix<>(Nat.N2(), Nat.N1()));
142  }
143
144  /**
145   * Create a state-space model of a differential drive drivetrain. In this model, the states are
146   * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
147   * [left velocity, right velocity]ᵀ.
148   *
149   * @param motor The motor (or gearbox) driving the drivetrain.
150   * @param mass The mass of the robot in kilograms.
151   * @param r The radius of the wheels in meters.
152   * @param rb The radius of the base (half the trackwidth) in meters.
153   * @param J The moment of inertia of the robot in kg-m².
154   * @param gearing The gearing reduction as output over input.
155   * @return A LinearSystem representing a differential drivetrain.
156   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
157   *     &lt;= 0.
158   */
159  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
160      DCMotor motor, double mass, double r, double rb, double J, double gearing) {
161    if (mass <= 0.0) {
162      throw new IllegalArgumentException("mass must be greater than zero.");
163    }
164    if (r <= 0.0) {
165      throw new IllegalArgumentException("r must be greater than zero.");
166    }
167    if (rb <= 0.0) {
168      throw new IllegalArgumentException("rb must be greater than zero.");
169    }
170    if (J <= 0.0) {
171      throw new IllegalArgumentException("J must be greater than zero.");
172    }
173    if (gearing <= 0.0) {
174      throw new IllegalArgumentException("gearing must be greater than zero.");
175    }
176
177    var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r);
178    var C2 = gearing * motor.Kt / (motor.R * r);
179
180    final double C3 = 1 / mass + rb * rb / J;
181    final double C4 = 1 / mass - rb * rb / J;
182    var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
183    var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
184    var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
185    var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
186
187    return new LinearSystem<>(A, B, C, D);
188  }
189
190  /**
191   * Create a state-space model of a single jointed arm system. The states of the system are [angle,
192   * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
193   *
194   * @param motor The motor (or gearbox) attached to the arm.
195   * @param J The moment of inertia J of the arm in kg-m².
196   * @param gearing The gearing between the motor and arm, in output over input. Most of the time
197   *     this will be greater than 1.
198   * @return A LinearSystem representing the given characterized constants.
199   */
200  public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
201      DCMotor motor, double J, double gearing) {
202    if (J <= 0.0) {
203      throw new IllegalArgumentException("J must be greater than zero.");
204    }
205    if (gearing <= 0.0) {
206      throw new IllegalArgumentException("gearing must be greater than zero.");
207    }
208
209    return new LinearSystem<>(
210        MatBuilder.fill(
211            Nat.N2(),
212            Nat.N2(),
213            0,
214            1,
215            0,
216            -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)),
217        VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
218        Matrix.eye(Nat.N2()),
219        new Matrix<>(Nat.N2(), Nat.N1()));
220  }
221
222  /**
223   * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
224   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
225   * [velocity], inputs are [voltage], and outputs are [velocity].
226   *
227   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
228   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
229   *
230   * <p>The parameters provided by the user are from this feedforward model:
231   *
232   * <p>u = K_v v + K_a a
233   *
234   * @param kV The velocity gain, in volts/(unit/sec)
235   * @param kA The acceleration gain, in volts/(unit/sec²)
236   * @return A LinearSystem representing the given characterized constants.
237   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
238   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
239   */
240  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
241    if (kV < 0.0) {
242      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
243    }
244    if (kA <= 0.0) {
245      throw new IllegalArgumentException("Ka must be greater than zero.");
246    }
247
248    return new LinearSystem<>(
249        VecBuilder.fill(-kV / kA),
250        VecBuilder.fill(1.0 / kA),
251        VecBuilder.fill(1.0),
252        VecBuilder.fill(0.0));
253  }
254
255  /**
256   * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
257   * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
258   * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
259   *
260   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
261   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
262   *
263   * <p>The parameters provided by the user are from this feedforward model:
264   *
265   * <p>u = K_v v + K_a a
266   *
267   * @param kV The velocity gain, in volts/(unit/sec)
268   * @param kA The acceleration gain, in volts/(unit/sec²)
269   * @return A LinearSystem representing the given characterized constants.
270   * @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
271   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
272   */
273  public static LinearSystem<N2, N1, N2> identifyPositionSystem(double kV, double kA) {
274    if (kV < 0.0) {
275      throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
276    }
277    if (kA <= 0.0) {
278      throw new IllegalArgumentException("Ka must be greater than zero.");
279    }
280
281    return new LinearSystem<>(
282        MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
283        VecBuilder.fill(0.0, 1.0 / kA),
284        Matrix.eye(Nat.N2()),
285        new Matrix<>(Nat.N2(), Nat.N1()));
286  }
287
288  /**
289   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
290   * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
291   * (volts/(radian/sec²))} cases. These constants can be found using SysId.
292   *
293   * <p>States: [[left velocity], [right velocity]]<br>
294   * Inputs: [[left voltage], [right voltage]]<br>
295   * Outputs: [[left velocity], [right velocity]]
296   *
297   * @param kVLinear The linear velocity gain in volts per (meters per second).
298   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
299   * @param kVAngular The angular velocity gain in volts per (meters per second).
300   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
301   * @return A LinearSystem representing the given characterized constants.
302   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
303   *     kAAngular &lt;= 0.
304   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
305   */
306  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
307      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
308    if (kVLinear <= 0.0) {
309      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
310    }
311    if (kALinear <= 0.0) {
312      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
313    }
314    if (kVAngular <= 0.0) {
315      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
316    }
317    if (kAAngular <= 0.0) {
318      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
319    }
320
321    final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
322    final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
323    final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
324    final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
325
326    return new LinearSystem<>(
327        MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
328        MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
329        MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
330        MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
331  }
332
333  /**
334   * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
335   * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
336   * (volts/(radian/sec²))} cases. This can be found using SysId.
337   *
338   * <p>States: [[left velocity], [right velocity]]<br>
339   * Inputs: [[left voltage], [right voltage]]<br>
340   * Outputs: [[left velocity], [right velocity]]
341   *
342   * @param kVLinear The linear velocity gain in volts per (meters per second).
343   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
344   * @param kVAngular The angular velocity gain in volts per (radians per second).
345   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
346   * @param trackwidth The distance between the differential drive's left and right wheels, in
347   *     meters.
348   * @return A LinearSystem representing the given characterized constants.
349   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
350   *     kAAngular &lt;= 0, or trackwidth &lt;= 0.
351   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
352   */
353  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
354      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
355    if (kVLinear <= 0.0) {
356      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
357    }
358    if (kALinear <= 0.0) {
359      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
360    }
361    if (kVAngular <= 0.0) {
362      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
363    }
364    if (kAAngular <= 0.0) {
365      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
366    }
367    if (trackwidth <= 0.0) {
368      throw new IllegalArgumentException("trackwidth must be greater than zero.");
369    }
370
371    // We want to find a factor to include in Kv,angular that will convert
372    // `u = Kv,angular omega` to `u = Kv,angular v`.
373    //
374    // v = omega r
375    // omega = v/r
376    // omega = 1/r v
377    // omega = 1/(trackwidth/2) v
378    // omega = 2/trackwidth v
379    //
380    // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
381    // to V/(m/s).
382    return identifyDrivetrainSystem(
383        kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
384  }
385}