Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] LinearSystemId: Fix docs, move C++ impls out of header (#4388)

Browse Source 
- Fix inaccuracies and inconsistencies in Java & C++ LinearSystemId documentation.
- Move LinearSystemId function definitions to LinearSystemId.cpp
This commit is contained in:
CarloWoolsey
2022-08-31 09:12:48 -07:00
committed by GitHub
parent d22ff8a158
commit 97c15af238
3 changed files with 333 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

330
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -31,92 +31,49 @@ class WPILIB_DLLEXPORT LinearSystemId {
units::inverse<units::seconds>>>;
/**
* Constructs the state-space model for an elevator.
* Create a state-space model of the elevator system. The states of the system
* are [position, velocity], inputs are [voltage], and outputs are [position].
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* @param motor Instance of DCMotor.
* @param m Carriage mass.
* @param r Pulley radius.
* @param motor The motor (or gearbox) attached to the carriage.
* @param mass The mass of the elevator carriage, in kilograms.
* @param radius The radius of the elevator's driving drum, in meters.
* @param G Gear ratio from motor to carriage.
* @throws std::domain_error if m <= 0, r <= 0, or G <= 0.
* @throws std::domain_error if mass <= 0, radius <= 0, or G <= 0.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
units::kilogram_t m,
units::meter_t r, double G) {
if (m <= 0_kg) {
throw std::domain_error("m must be greater than zero.");
}
if (r <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
}
Matrixd<2, 2> A{{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * r * m)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
units::kilogram_t mass,
units::meter_t radius, double G);
/**
* Constructs the state-space model for a single-jointed arm.
* Create a state-space model of a single-jointed arm system.The states of the
* system are [angle, angular velocity], inputs are [voltage], and outputs are
* [angle].
*
* States: [[angle], [angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angle]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
* @param G Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
}
DCMotor motor, units::kilogram_square_meter_t J, double G);
/**
* Constructs the state-space model for a 1 DOF velocity-only system from
* system identification data.
* Create a state-space model for a 1 DOF velocity system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. The states of the system are [velocity], inputs are
* [voltage], and outputs are [velocity].
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
* actual method arguments; they will automatically be converted to SI
* internally.
*
* States: [[velocity]]
* Inputs: [[voltage]]
* Outputs: [[velocity]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param kV The velocity gain, in volts/(unit/sec).
* @param kA The acceleration gain, in volts/(unit/sec²).
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<
@@ -141,24 +98,23 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
/**
* Constructs the state-space model for a 1 DOF position system from system
* identification data.
* Create a state-space model for a 1 DOF position system from its kV
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
* found using SysId. the states of the system are [position, velocity],
* inputs are [voltage], and outputs are [position].
*
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
* argument. You may still use non-SI units (such as feet or inches) for the
* actual method arguments; they will automatically be converted to SI
* internally.
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param kV The velocity gain, in volts/(unit/sec).
* @param kA The acceleration gain, in volts/(unit/sec²).
*
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<
@@ -184,227 +140,101 @@ class WPILIB_DLLEXPORT LinearSystemId {
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA
* in both linear (volts/(meter/sec) and volts/(meter/sec²)) and angular
* (volts/(radian/sec) and volts/(radian/sec²)) cases. This can be found using
* SysId.
* in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
* {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be
* found using SysId.
*
* States: [[left velocity], [right velocity]]<br>
* Inputs: [[left voltage], [right voltage]]<br>
* Outputs: [[left velocity], [right velocity]]
*
* @param kVlinear The linear velocity gain in volts per (meter per second).
* @param kAlinear The linear acceleration gain in volts per (meter per
* @param kVLinear The linear velocity gain in volts per (meters per second).
* @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
* @param kVangular The angular velocity gain in volts per (meter per second).
* @param kAangular The angular acceleration gain in volts per (meter per
* @param kVAngular The angular velocity gain in volts per (radians per
* second).
* @param kAAngular The angular acceleration gain in volts per (radians per
* second squared).
* @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
* or kAangular <= 0.
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* or kAAngular <= 0.
*/
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) {
if (kVlinear <= decltype(kVlinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kAlinear <= decltype(kAlinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVangular <= decltype(kVangular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAangular <= decltype(kAangular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
double A1 = -(kVlinear.value() / kAlinear.value() +
kVangular.value() / kAangular.value());
double A2 = -(kVlinear.value() / kAlinear.value() -
kVangular.value() / kAangular.value());
double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
Matrixd<2, 2> A = 0.5 * Matrixd<2, 2>{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B = 0.5 * Matrixd<2, 2>{{B1, B2}, {B2, B1}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
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/(meter/sec) and volts/(meter/sec²)) and angular
* (volts/(radian/sec) and volts/(radian/sec²)) cases. This can be found using
* SysId.
* in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
* {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found
* using SysId.
*
* States: [[left velocity], [right velocity]]<br>
* Inputs: [[left voltage], [right voltage]]<br>
* Outputs: [[left velocity], [right velocity]]
*
* @param kVlinear The linear velocity gain in volts per (meter per second).
* @param kAlinear The linear acceleration gain in volts per (meter per
* @param kVLinear The linear velocity gain in volts per (meters per
* second).
* @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
* @param kVangular The angular velocity gain in volts per (radian per
* @param kVAngular The angular velocity gain in volts per (radians per
* second).
* @param kAangular The angular acceleration gain in volts per (radian per
* @param kAAngular The angular acceleration gain in volts per (radians per
* second squared).
* @param trackwidth The distance between the differential drive's left and
* right wheels.
* @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
* kAangular <= 0, or trackwidth <= 0.
* right wheels, in meters.
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* kAAngular <= 0, or trackwidth <= 0.
*/
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) {
if (kVlinear <= decltype(kVlinear){0}) {
throw std::domain_error("Kv,linear must be greater than zero.");
}
if (kAlinear <= decltype(kAlinear){0}) {
throw std::domain_error("Ka,linear must be greater than zero.");
}
if (kVangular <= decltype(kVangular){0}) {
throw std::domain_error("Kv,angular must be greater than zero.");
}
if (kAangular <= decltype(kAangular){0}) {
throw std::domain_error("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
// We want to find a factor to include in Kv,angular that will convert
// `u = Kv,angular omega` to `u = Kv,angular v`.
//
// v = omega r
// omega = v/r
// omega = 1/r v
// omega = 1/(trackwidth/2) v
// omega = 2/trackwidth v
//
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
// to V/(m/s).
return IdentifyDrivetrainSystem(kVlinear, kAlinear,
kVangular * 2.0 / trackwidth * 1_rad,
kAangular * 2.0 / trackwidth * 1_rad);
}
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);
/**
* Constructs the state-space model for a flywheel.
* Create a state-space model of a flywheel system, the states of the system
* are [angular velocity], inputs are [voltage], and outputs are [angular
* velocity].
*
* States: [[angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angular velocity]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
* @param motor The motor (or gearbox) attached to the flywheel.
* @param J The moment of inertia J of the flywheel.
* @param G Gear ratio from motor to flywheel.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double G) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
}
Matrixd<1, 1> A{
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
double G);
/**
* Constructs the state-space model for a DC motor motor.
* Create a state-space model of a DC motor system. The states of the system
* are [angular position, angular velocity], inputs are [voltage], and outputs
* are [angular position, angular velocity].
*
* States: [[angular position, angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angular position, angular velocity]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
* @param motor The motor (or gearbox) attached to the system.
* @param J the moment of inertia J of the DC motor.
* @param G Gear ratio from motor to output.
* @throws std::domain_error if J <= 0 or G <= 0.
*/
static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double G) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 2>(A, B, C, D);
}
double G);
/**
* Constructs the state-space model for a drivetrain.
* Create a state-space model of differential drive drivetrain. In this model,
* the states are [left velocity, right velocity], the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity]
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param motor Instance of DCMotor.
* @param m Drivetrain mass.
* @param r Wheel radius.
* @param rb Robot radius.
* @param J Moment of inertia.
* @param motor The motor (or gearbox) driving the drivetrain.
* @param mass The mass of the robot in kilograms.
* @param r The radius of the wheels in meters.
* @param rb The radius of the base (half of the track width), in meters.
* @param J The moment of inertia of the robot.
* @param G Gear ratio from motor to wheel.
* @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or
* @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
* G <= 0.
*/
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
const DCMotor& motor, units::kilogram_t m, units::meter_t r,
units::meter_t rb, units::kilogram_square_meter_t J, double G) {
if (m <= 0_kg) {
throw std::domain_error("m must be greater than zero.");
}
if (r <= 0_m) {
throw std::domain_error("r must be greater than zero.");
}
if (rb <= 0_m) {
throw std::domain_error("rb must be greater than zero.");
}
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (G <= 0.0) {
throw std::domain_error("G must be greater than zero.");
}
auto C1 = -std::pow(G, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
Matrixd<2, 2> B{{((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
{((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
return LinearSystem<2, 2, 2>(A, B, C, D);
}
const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
units::meter_t rb, units::kilogram_square_meter_t J, double G);
};
} // namespace frc