WPILibC++ 2024.3.2
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 <wpi/SymbolExports.h>
11
14#include "units/acceleration.h"
17#include "units/length.h"
19#include "units/velocity.h"
20#include "units/voltage.h"
21
22namespace frc {
23/**
24 * Linear system ID utility functions.
25 */
27 public:
28 template <typename Distance>
31
32 template <typename Distance>
36
37 /**
38 * Create a state-space model of the elevator system. The states of the system
39 * are [position, velocity], inputs are [voltage], and outputs are [position].
40 *
41 * @param motor The motor (or gearbox) attached to the carriage.
42 * @param mass The mass of the elevator carriage, in kilograms.
43 * @param radius The radius of the elevator's driving drum, in meters.
44 * @param gearing Gear ratio from motor to carriage.
45 * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
46 */
48 units::kilogram_t mass,
50 double gearing);
51
52 /**
53 * Create a state-space model of a single-jointed arm system.The states of the
54 * system are [angle, angular velocity], inputs are [voltage], and outputs are
55 * [angle].
56 *
57 * @param motor The motor (or gearbox) attached to the arm.
58 * @param J The moment of inertia J of the arm.
59 * @param gearing Gear ratio from motor to arm.
60 * @throws std::domain_error if J <= 0 or gearing <= 0.
61 */
63 DCMotor motor, units::kilogram_square_meter_t J, double gearing);
64
65 /**
66 * Create a state-space model for a 1 DOF velocity system from its kV
67 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
68 * found using SysId. The states of the system are [velocity], inputs are
69 * [voltage], and outputs are [velocity].
70 *
71 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
72 * argument. You may still use non-SI units (such as feet or inches) for the
73 * actual method arguments; they will automatically be converted to SI
74 * internally.
75 *
76 * The parameters provided by the user are from this feedforward model:
77 *
78 * u = K_v v + K_a a
79 *
80 * @param kV The velocity gain, in volts/(unit/sec).
81 * @param kA The acceleration gain, in volts/(unit/sec²).
82 * @throws std::domain_error if kV < 0 or kA <= 0.
83 * @see <a
84 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
85 */
86 template <typename Distance>
87 requires std::same_as<units::meter, Distance> ||
90 decltype(1_V / Velocity_t<Distance>(1)) kV,
91 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
92 if (kV < decltype(kV){0}) {
93 throw std::domain_error("Kv must be greater than or equal to zero.");
94 }
95 if (kA <= decltype(kA){0}) {
96 throw std::domain_error("Ka must be greater than zero.");
97 }
98
99 Matrixd<1, 1> A{-kV.value() / kA.value()};
100 Matrixd<1, 1> B{1.0 / kA.value()};
101 Matrixd<1, 1> C{1.0};
102 Matrixd<1, 1> D{0.0};
103
104 return LinearSystem<1, 1, 1>(A, B, C, D);
105 }
106
107 /**
108 * Create a state-space model for a 1 DOF position system from its kV
109 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
110 * found using SysId. the states of the system are [position, velocity],
111 * inputs are [voltage], and outputs are [position].
112 *
113 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
114 * argument. You may still use non-SI units (such as feet or inches) for the
115 * actual method arguments; they will automatically be converted to SI
116 * internally.
117 *
118 * The parameters provided by the user are from this feedforward model:
119 *
120 * u = K_v v + K_a a
121 *
122 * @param kV The velocity gain, in volts/(unit/sec).
123 * @param kA The acceleration gain, in volts/(unit/sec²).
124 *
125 * @throws std::domain_error if kV < 0 or kA <= 0.
126 * @see <a
127 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
128 */
129 template <typename Distance>
130 requires std::same_as<units::meter, Distance> ||
133 decltype(1_V / Velocity_t<Distance>(1)) kV,
134 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
135 if (kV < decltype(kV){0}) {
136 throw std::domain_error("Kv must be greater than or equal to zero.");
137 }
138 if (kA <= decltype(kA){0}) {
139 throw std::domain_error("Ka must be greater than zero.");
140 }
141
142 Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
143 Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
144 Matrixd<1, 2> C{1.0, 0.0};
145 Matrixd<1, 1> D{0.0};
146
147 return LinearSystem<2, 1, 1>(A, B, C, D);
148 }
149
150 /**
151 * Identify a differential drive drivetrain given the drivetrain's kV and kA
152 * in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
154 * found using SysId.
155 *
156 * States: [[left velocity], [right velocity]]<br>
157 * Inputs: [[left voltage], [right voltage]]<br>
158 * Outputs: [[left velocity], [right velocity]]
159 *
160 * @param kVLinear The linear velocity gain in volts per (meters per second).
161 * @param kALinear The linear acceleration gain in volts per (meters per
162 * second squared).
163 * @param kVAngular The angular velocity gain in volts per (meters per
164 * second).
165 * @param kAAngular The angular acceleration gain in volts per (meters per
166 * second squared).
167 * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
168 * or kAAngular <= 0.
169 * @see <a
170 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
171 */
173 decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
174 decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular);
175
176 /**
177 * Identify a differential drive drivetrain given the drivetrain's kV and kA
178 * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
180 * using SysId.
181 *
182 * States: [[left velocity], [right velocity]]<br>
183 * Inputs: [[left voltage], [right voltage]]<br>
184 * Outputs: [[left velocity], [right velocity]]
185 *
186 * @param kVLinear The linear velocity gain in volts per (meters per
187 * second).
188 * @param kALinear The linear acceleration gain in volts per (meters per
189 * second squared).
190 * @param kVAngular The angular velocity gain in volts per (radians per
191 * second).
192 * @param kAAngular The angular acceleration gain in volts per (radians per
193 * second squared).
194 * @param trackwidth The distance between the differential drive's left and
195 * right wheels, in meters.
196 * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
197 * kAAngular <= 0, or trackwidth <= 0.
198 * @see <a
199 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
200 */
202 decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
204 decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth);
205
206 /**
207 * Create a state-space model of a flywheel system, the states of the system
208 * are [angular velocity], inputs are [voltage], and outputs are [angular
209 * velocity].
210 *
211 * @param motor The motor (or gearbox) attached to the flywheel.
212 * @param J The moment of inertia J of the flywheel.
213 * @param gearing Gear ratio from motor to flywheel.
214 * @throws std::domain_error if J <= 0 or gearing <= 0.
215 */
217 units::kilogram_square_meter_t J,
218 double gearing);
219
220 /**
221 * Create a state-space model of a DC motor system. The states of the system
222 * are [angular position, angular velocity], inputs are [voltage], and outputs
223 * are [angular position, angular velocity].
224 *
225 * @param motor The motor (or gearbox) attached to the system.
226 * @param J the moment of inertia J of the DC motor.
227 * @param gearing Gear ratio from motor to output.
228 * @throws std::domain_error if J <= 0 or gearing <= 0.
229 * @see <a
230 * href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
231 */
233 units::kilogram_square_meter_t J,
234 double gearing);
235
236 /**
237 * Create a state-space model of a DC motor system from its kV
238 * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
239 * found using SysId. the states of the system are [position, velocity],
240 * inputs are [voltage], and outputs are [position].
241 *
242 * You MUST use an SI unit (i.e. meters or radians) for the Distance template
243 * argument. You may still use non-SI units (such as feet or inches) for the
244 * actual method arguments; they will automatically be converted to SI
245 * internally.
246 *
247 * The parameters provided by the user are from this feedforward model:
248 *
249 * u = K_v v + K_a a
250 *
251 * @param kV The velocity gain, in volts/(unit/sec).
252 * @param kA The acceleration gain, in volts/(unit/sec²).
253 *
254 * @throws std::domain_error if kV < 0 or kA <= 0.
255 */
256 template <typename Distance>
257 requires std::same_as<units::meter, Distance> ||
260 decltype(1_V / Velocity_t<Distance>(1)) kV,
261 decltype(1_V / Acceleration_t<Distance>(1)) kA) {
262 if (kV < decltype(kV){0}) {
263 throw std::domain_error("Kv must be greater than or equal to zero.");
264 }
265 if (kA <= decltype(kA){0}) {
266 throw std::domain_error("Ka must be greater than zero.");
267 }
268
269 Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
270 Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
271 Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
272 Matrixd<2, 1> D{{0.0}, {0.0}};
273
274 return LinearSystem<2, 1, 2>(A, B, C, D);
275 }
276
277 /**
278 * Create a state-space model of differential drive drivetrain. In this model,
279 * the states are [left velocity, right velocity], the inputs are [left
280 * voltage, right voltage], and the outputs are [left velocity, right
281 * velocity]
282 *
283 * @param motor The motor (or gearbox) driving the drivetrain.
284 * @param mass The mass of the robot in kilograms.
285 * @param r The radius of the wheels in meters.
286 * @param rb The radius of the base (half of the track width), in meters.
287 * @param J The moment of inertia of the robot.
288 * @param gearing Gear ratio from motor to wheel.
289 * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
290 * gearing <= 0.
291 */
293 const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
294 units::meter_t rb, units::kilogram_square_meter_t J, double gearing);
295};
296
297} // namespace frc
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Holds the constants for a DC motor.
Definition: DCMotor.h:20
A plant defined using state-space notation.
Definition: LinearSystem.h:31
Linear system ID utility functions.
Definition: LinearSystemId.h:26
static 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.
static 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.
static 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],...
static 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:132
static 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:259
static 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:89
static LinearSystem< 2, 1, 1 > ElevatorSystem(DCMotor motor, units::kilogram_t mass, units::meter_t radius, double gearing)
Create a state-space model of the elevator system.
static 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...
static LinearSystem< 2, 1, 1 > 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,...
static 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...
Container for values which represent quantities of a given unit.
Definition: base.h:1926
typename units::detail::inverse_impl< U >::type inverse
represents the inverse unit type of class U.
Definition: base.h:1134
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition: base.h:1434
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21