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