WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
LinearSystemId.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <concepts>
8#include <stdexcept>
9
10#include <gcem.hpp>
11#include <wpi/SymbolExports.h>
12
15#include "units/acceleration.h"
18#include "units/length.h"
20#include "units/velocity.h"
21#include "units/voltage.h"
22
23namespace frc {
24/**
25 * Linear system ID utility functions.
26 */
28 public:
29 template <typename Distance>
32
33 template <typename Distance>
37
38 /**
39 * Create a state-space model of the elevator system. The states of the system
40 * are [position, velocity], inputs are [voltage], and outputs are [position].
41 *
42 * @param motor The motor (or gearbox) attached to the carriage.
43 * @param mass The mass of the elevator carriage, in kilograms.
44 * @param radius The radius of the elevator's driving drum, in meters.
45 * @param gearing Gear ratio from motor to carriage.
46 * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
47 */
49 units::kilogram_t mass,
50 units::meter_t radius,
51 double gearing) {
52 if (mass <= 0_kg) {
53 throw std::domain_error("mass must be greater than zero.");
54 }
55 if (radius <= 0_m) {
56 throw std::domain_error("radius must be greater than zero.");
57 }
58 if (gearing <= 0.0) {
59 throw std::domain_error("gearing must be greater than zero.");
60 }
61
63 {0.0, 1.0},
64 {0.0, (-gcem::pow(gearing, 2) * motor.Kt /
65 (motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
66 .value()}};
67 Matrixd<2, 1> B{{0.0},
68 {(gearing * motor.Kt / (motor.R * radius * mass)).value()}};
69 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
70 Matrixd<2, 1> D{{0.0}, {0.0}};
71
72 return LinearSystem<2, 1, 2>(A, B, C, D);
73 }
74
75 /**
76 * Create a state-space model of a single-jointed arm system.The states of the
77 * system are [angle, angular velocity], inputs are [voltage], and outputs are
78 * [angle].
79 *
80 * @param motor The motor (or gearbox) attached to the arm.
81 * @param J The moment of inertia J of the arm.
82 * @param gearing Gear ratio from motor to arm.
83 * @throws std::domain_error if J <= 0 or gearing <= 0.
84 */
86 DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
87 if (J <= 0_kg_sq_m) {
88 throw std::domain_error("J must be greater than zero.");
89 }
90 if (gearing <= 0.0) {
91 throw std::domain_error("gearing must be greater than zero.");
92 }
93
95 {0.0, 1.0},
96 {0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
97 .value()}};
98 Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}};
99 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
100 Matrixd<2, 1> D{{0.0}, {0.0}};
101
102 return LinearSystem<2, 1, 2>(A, B, C, D);
103 }
104
105 /**
106 * Create a state-space model for a 1 DOF velocity system from its kV
107 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
108 * found using SysId. The states of the system are [velocity], inputs are
109 * [voltage], and outputs are [velocity].
110 *
111 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
112 * argument. You may still use non-SI units (such as feet or inches) for the
113 * actual method arguments; they will automatically be converted to SI
114 * internally.
115 *
116 * The parameters provided by the user are from this feedforward model:
117 *
118 * u = K_v v + K_a a
119 *
120 * @param kV The velocity gain, in volts/(unit/sec).
121 * @param kA The acceleration gain, in volts/(unit/sec²).
122 * @throws std::domain_error if kV < 0 or kA <= 0.
123 * @see <a
124 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
125 */
126 template <typename Distance>
127 requires std::same_as<units::meter, Distance> ||
128 std::same_as<units::radian, Distance>
130 decltype(1_V / Velocity_t<Distance>(1)) kV,
131 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
132 if (kV < decltype(kV){0}) {
133 throw std::domain_error("Kv must be greater than or equal to zero.");
134 }
135 if (kA <= decltype(kA){0}) {
136 throw std::domain_error("Ka must be greater than zero.");
137 }
138
139 Matrixd<1, 1> A{{-kV.value() / kA.value()}};
140 Matrixd<1, 1> B{{1.0 / kA.value()}};
141 Matrixd<1, 1> C{{1.0}};
142 Matrixd<1, 1> D{{0.0}};
143
144 return LinearSystem<1, 1, 1>(A, B, C, D);
145 }
146
147 /**
148 * Create a state-space model for a 1 DOF position system from its kV
149 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
150 * found using SysId. the states of the system are [position, velocity],
151 * inputs are [voltage], and outputs are [position].
152 *
153 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
154 * argument. You may still use non-SI units (such as feet or inches) for the
155 * actual method arguments; they will automatically be converted to SI
156 * internally.
157 *
158 * The parameters provided by the user are from this feedforward model:
159 *
160 * u = K_v v + K_a a
161 *
162 * @param kV The velocity gain, in volts/(unit/sec).
163 * @param kA The acceleration gain, in volts/(unit/sec²).
164 *
165 * @throws std::domain_error if kV < 0 or kA <= 0.
166 * @see <a
167 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
168 */
169 template <typename Distance>
170 requires std::same_as<units::meter, Distance> ||
171 std::same_as<units::radian, Distance>
173 decltype(1_V / Velocity_t<Distance>(1)) kV,
174 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
175 if (kV < decltype(kV){0}) {
176 throw std::domain_error("Kv must be greater than or equal to zero.");
177 }
178 if (kA <= decltype(kA){0}) {
179 throw std::domain_error("Ka must be greater than zero.");
180 }
181
182 Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
183 Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
184 Matrixd<1, 2> C{1.0, 0.0};
185 Matrixd<1, 1> D{0.0};
186
187 return LinearSystem<2, 1, 1>(A, B, C, D);
188 }
189
190 /**
191 * Identify a differential drive drivetrain given the drivetrain's kV and kA
192 * in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
193 * {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be
194 * found using SysId.
195 *
196 * States: [[left velocity], [right velocity]]<br>
197 * Inputs: [[left voltage], [right voltage]]<br>
198 * Outputs: [[left velocity], [right velocity]]
199 *
200 * @param kVLinear The linear velocity gain in volts per (meters per second).
201 * @param kALinear The linear acceleration gain in volts per (meters per
202 * second squared).
203 * @param kVAngular The angular velocity gain in volts per (meters per
204 * second).
205 * @param kAAngular The angular acceleration gain in volts per (meters per
206 * second squared).
207 * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
208 * or kAAngular <= 0.
209 * @see <a
210 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
211 */
213 decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
214 decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
215 if (kVLinear <= decltype(kVLinear){0}) {
216 throw std::domain_error("Kv,linear must be greater than zero.");
217 }
218 if (kALinear <= decltype(kALinear){0}) {
219 throw std::domain_error("Ka,linear must be greater than zero.");
220 }
221 if (kVAngular <= decltype(kVAngular){0}) {
222 throw std::domain_error("Kv,angular must be greater than zero.");
223 }
224 if (kAAngular <= decltype(kAAngular){0}) {
225 throw std::domain_error("Ka,angular must be greater than zero.");
226 }
227
228 double A1 = -(kVLinear.value() / kALinear.value() +
229 kVAngular.value() / kAAngular.value());
230 double A2 = -(kVLinear.value() / kALinear.value() -
231 kVAngular.value() / kAAngular.value());
232 double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
233 double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
234
235 A1 /= 2.0;
236 A2 /= 2.0;
237 B1 /= 2.0;
238 B2 /= 2.0;
239
240 Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
241 Matrixd<2, 2> B{{B1, B2}, {B2, B1}};
242 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
243 Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
244
245 return LinearSystem<2, 2, 2>(A, B, C, D);
246 }
247
248 /**
249 * Identify a differential drive drivetrain given the drivetrain's kV and kA
250 * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
251 * {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found
252 * using SysId.
253 *
254 * States: [[left velocity], [right velocity]]<br>
255 * Inputs: [[left voltage], [right voltage]]<br>
256 * Outputs: [[left velocity], [right velocity]]
257 *
258 * @param kVLinear The linear velocity gain in volts per (meters per
259 * second).
260 * @param kALinear The linear acceleration gain in volts per (meters per
261 * second squared).
262 * @param kVAngular The angular velocity gain in volts per (radians per
263 * second).
264 * @param kAAngular The angular acceleration gain in volts per (radians per
265 * second squared).
266 * @param trackwidth The distance between the differential drive's left and
267 * right wheels, in meters.
268 * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
269 * kAAngular <= 0, or trackwidth <= 0.
270 * @see <a
271 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
272 */
274 decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
275 decltype(1_V / 1_rad_per_s) kVAngular,
276 decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth) {
277 if (kVLinear <= decltype(kVLinear){0}) {
278 throw std::domain_error("Kv,linear must be greater than zero.");
279 }
280 if (kALinear <= decltype(kALinear){0}) {
281 throw std::domain_error("Ka,linear must be greater than zero.");
282 }
283 if (kVAngular <= decltype(kVAngular){0}) {
284 throw std::domain_error("Kv,angular must be greater than zero.");
285 }
286 if (kAAngular <= decltype(kAAngular){0}) {
287 throw std::domain_error("Ka,angular must be greater than zero.");
288 }
289 if (trackwidth <= 0_m) {
290 throw std::domain_error("r must be greater than zero.");
291 }
292
293 // We want to find a factor to include in Kv,angular that will convert
294 // `u = Kv,angular omega` to `u = Kv,angular v`.
295 //
296 // v = omega r
297 // omega = v/r
298 // omega = 1/r v
299 // omega = 1/(trackwidth/2) v
300 // omega = 2/trackwidth v
301 //
302 // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
303 // to V/(m/s).
304 return IdentifyDrivetrainSystem(kVLinear, kALinear,
305 kVAngular * 2.0 / trackwidth * 1_rad,
306 kAAngular * 2.0 / trackwidth * 1_rad);
307 }
308
309 /**
310 * Create a state-space model of a flywheel system, the states of the system
311 * are [angular velocity], inputs are [voltage], and outputs are [angular
312 * velocity].
313 *
314 * @param motor The motor (or gearbox) attached to the flywheel.
315 * @param J The moment of inertia J of the flywheel.
316 * @param gearing Gear ratio from motor to flywheel.
317 * @throws std::domain_error if J <= 0 or gearing <= 0.
318 */
320 DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
321 if (J <= 0_kg_sq_m) {
322 throw std::domain_error("J must be greater than zero.");
323 }
324 if (gearing <= 0.0) {
325 throw std::domain_error("gearing must be greater than zero.");
326 }
327
329 {(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
330 .value()}};
331 Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}};
332 Matrixd<1, 1> C{{1.0}};
333 Matrixd<1, 1> D{{0.0}};
334
335 return LinearSystem<1, 1, 1>(A, B, C, D);
336 }
337
338 /**
339 * Create a state-space model of a DC motor system. The states of the system
340 * are [angular position, angular velocity], inputs are [voltage], and outputs
341 * are [angular position, angular velocity].
342 *
343 * @param motor The motor (or gearbox) attached to the system.
344 * @param J the moment of inertia J of the DC motor.
345 * @param gearing Gear ratio from motor to output.
346 * @throws std::domain_error if J <= 0 or gearing <= 0.
347 * @see <a
348 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
349 */
351 DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
352 if (J <= 0_kg_sq_m) {
353 throw std::domain_error("J must be greater than zero.");
354 }
355 if (gearing <= 0.0) {
356 throw std::domain_error("gearing must be greater than zero.");
357 }
358
360 {0.0, 1.0},
361 {0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
362 .value()}};
363 Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}};
364 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
365 Matrixd<2, 1> D{{0.0}, {0.0}};
366
367 return LinearSystem<2, 1, 2>(A, B, C, D);
368 }
369
370 /**
371 * Create a state-space model of a DC motor system from its kV
372 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
373 * found using SysId. the states of the system are [position, velocity],
374 * inputs are [voltage], and outputs are [position].
375 *
376 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
377 * argument. You may still use non-SI units (such as feet or inches) for the
378 * actual method arguments; they will automatically be converted to SI
379 * internally.
380 *
381 * The parameters provided by the user are from this feedforward model:
382 *
383 * u = K_v v + K_a a
384 *
385 * @param kV The velocity gain, in volts/(unit/sec).
386 * @param kA The acceleration gain, in volts/(unit/sec²).
387 *
388 * @throws std::domain_error if kV < 0 or kA <= 0.
389 */
390 template <typename Distance>
391 requires std::same_as<units::meter, Distance> ||
392 std::same_as<units::radian, Distance>
394 decltype(1_V / Velocity_t<Distance>(1)) kV,
395 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
396 if (kV < decltype(kV){0}) {
397 throw std::domain_error("Kv must be greater than or equal to zero.");
398 }
399 if (kA <= decltype(kA){0}) {
400 throw std::domain_error("Ka must be greater than zero.");
401 }
402
403 Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
404 Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
405 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
406 Matrixd<2, 1> D{{0.0}, {0.0}};
407
408 return LinearSystem<2, 1, 2>(A, B, C, D);
409 }
410
411 /**
412 * Create a state-space model of differential drive drivetrain. In this model,
413 * the states are [left velocity, right velocity], the inputs are [left
414 * voltage, right voltage], and the outputs are [left velocity, right
415 * velocity]
416 *
417 * @param motor The motor (or gearbox) driving the drivetrain.
418 * @param mass The mass of the robot in kilograms.
419 * @param r The radius of the wheels in meters.
420 * @param rb The radius of the base (half of the track width), in meters.
421 * @param J The moment of inertia of the robot.
422 * @param gearing Gear ratio from motor to wheel.
423 * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
424 * gearing <= 0.
425 */
427 const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
428 units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
429 if (mass <= 0_kg) {
430 throw std::domain_error("mass must be greater than zero.");
431 }
432 if (r <= 0_m) {
433 throw std::domain_error("r must be greater than zero.");
434 }
435 if (rb <= 0_m) {
436 throw std::domain_error("rb must be greater than zero.");
437 }
438 if (J <= 0_kg_sq_m) {
439 throw std::domain_error("J must be greater than zero.");
440 }
441 if (gearing <= 0.0) {
442 throw std::domain_error("gearing must be greater than zero.");
443 }
444
445 auto C1 = -gcem::pow(gearing, 2) * motor.Kt /
446 (motor.Kv * motor.R * units::math::pow<2>(r));
447 auto C2 = gearing * motor.Kt / (motor.R * r);
448
449 Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
450 ((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
451 {((1 / mass - units::math::pow<2>(rb) / J) * C1).value(),
452 ((1 / mass + units::math::pow<2>(rb) / J) * C1).value()}};
453 Matrixd<2, 2> B{{((1 / mass + units::math::pow<2>(rb) / J) * C2).value(),
454 ((1 / mass - units::math::pow<2>(rb) / J) * C2).value()},
455 {((1 / mass - units::math::pow<2>(rb) / J) * C2).value(),
456 ((1 / mass + units::math::pow<2>(rb) / J) * C2).value()}};
457 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
458 Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
459
460 return LinearSystem<2, 2, 2>(A, B, C, D);
461 }
462};
463
464} // namespace frc
#define WPILIB_DLLEXPORT
Definition SymbolExports.h:36
Holds the constants for a DC motor.
Definition DCMotor.h:20
radians_per_second_per_volt_t Kv
Motor velocity constant.
Definition DCMotor.h:48
units::ohm_t R
Motor internal resistance.
Definition DCMotor.h:45
newton_meters_per_ampere_t Kt
Motor torque constant.
Definition DCMotor.h:51
A plant defined using state-space notation.
Definition LinearSystem.h:35
Linear system ID utility functions.
Definition LinearSystemId.h:27
static constexpr LinearSystem< 2, 1, 2 > DCMotorSystem(decltype(1_V/Velocity_t< Distance >(1)) kV, decltype(1_V/Acceleration_t< Distance >(1)) kA)
Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA (volts/(unit/se...
Definition LinearSystemId.h:393
static constexpr LinearSystem< 2, 1, 2 > SingleJointedArmSystem(DCMotor motor, units::kilogram_square_meter_t J, double gearing)
Create a state-space model of a single-jointed arm system.The states of the system are [angle,...
Definition LinearSystemId.h:85
static constexpr LinearSystem< 2, 1, 2 > ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, double gearing)
Create a state-space model of the elevator system.
Definition LinearSystemId.h:48
static constexpr LinearSystem< 2, 1, 1 > IdentifyPositionSystem(decltype(1_V/Velocity_t< Distance >(1)) kV, decltype(1_V/Acceleration_t< Distance >(1)) kA)
Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA (volts/(...
Definition LinearSystemId.h:172
static constexpr LinearSystem< 1, 1, 1 > FlywheelSystem(DCMotor motor, units::kilogram_square_meter_t J, double gearing)
Create a state-space model of a flywheel system, the states of the system are [angular velocity],...
Definition LinearSystemId.h:319
static constexpr LinearSystem< 2, 2, 2 > DrivetrainVelocitySystem(const DCMotor &motor, units::kilogram_t mass, units::meter_t r, units::meter_t rb, units::kilogram_square_meter_t J, double gearing)
Create a state-space model of differential drive drivetrain.
Definition LinearSystemId.h:426
static constexpr LinearSystem< 2, 2, 2 > IdentifyDrivetrainSystem(decltype(1_V/1_mps) kVLinear, decltype(1_V/1_mps_sq) kALinear, decltype(1_V/1_mps) kVAngular, decltype(1_V/1_mps_sq) kAAngular)
Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear {(volts/(met...
Definition LinearSystemId.h:212
static constexpr LinearSystem< 1, 1, 1 > IdentifyVelocitySystem(decltype(1_V/Velocity_t< Distance >(1)) kV, decltype(1_V/Acceleration_t< Distance >(1)) kA)
Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA (volts/(...
Definition LinearSystemId.h:129
static constexpr LinearSystem< 2, 1, 2 > DCMotorSystem(DCMotor motor, units::kilogram_square_meter_t J, double gearing)
Create a state-space model of a DC motor system.
Definition LinearSystemId.h:350
static constexpr LinearSystem< 2, 2, 2 > IdentifyDrivetrainSystem(decltype(1_V/1_mps) kVLinear, decltype(1_V/1_mps_sq) kALinear, decltype(1_V/1_rad_per_s) kVAngular, decltype(1_V/1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear {(volts/(met...
Definition LinearSystemId.h:273
Container for values which represent quantities of a given unit.
Definition base.h:1930
typename units::detail::inverse_impl< U >::type inverse
represents the inverse unit type of class U.
Definition base.h:1138
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition base.h:1438
Definition CAN.h:11
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition EigenCore.h:21
constexpr common_t< T1, T2 > pow(const T1 base, const T2 exp_term) noexcept
Compile-time power function.
Definition pow.hpp:82
constexpr auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition base.h:2810