Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Reorganize LinearSystem factories (#8468)

Browse Source
This commit is contained in:
Tyler Veness
2026-01-12 19:09:35 -08:00
committed by GitHub
parent 89d0759ef2
commit 00fa8361dd
108 changed files with 1808 additions and 2138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java
View File

@@ -16,7 +16,7 @@ import org.wpilib.math.util.Nat;
* Filters the provided voltages to limit a differential drive's linear and angular acceleration.
*
* <p>The differential drive model can be created via the functions in {@link
* org.wpilib.math.system.plant.LinearSystemId}.
* org.wpilib.math.system.Models}.
*/
public class DifferentialDriveAccelerationLimiter {
private final LinearSystem<N2, N2, N2> m_system;

6
wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java
View File

@@ -9,7 +9,7 @@ import org.wpilib.math.controller.struct.DifferentialDriveFeedforwardStruct;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;
@@ -41,7 +41,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
*/
public DifferentialDriveFeedforward(
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
// See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double)
// See Models.differentialDriveFromSysId(double, double, double, double, double)
this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
}
@@ -55,7 +55,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
*/
public DifferentialDriveFeedforward(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
m_plant = Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular);
m_kVLinear = kVLinear;
m_kALinear = kALinear;
m_kVAngular = kVAngular;

6
wpimath/src/main/java/org/wpilib/math/system/plant/DCMotor.java → wpimath/src/main/java/org/wpilib/math/system/DCMotor.java
View File

@@ -2,10 +2,10 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant;
package org.wpilib.math.system;
import org.wpilib.math.system.plant.proto.DCMotorProto;
import org.wpilib.math.system.plant.struct.DCMotorStruct;
import org.wpilib.math.system.proto.DCMotorProto;
import org.wpilib.math.system.struct.DCMotorStruct;
import org.wpilib.math.util.Units;
import org.wpilib.util.protobuf.ProtobufSerializable;
import org.wpilib.util.struct.StructSerializable;

370
wpimath/src/main/java/org/wpilib/math/system/Models.java Normal file
View File

@@ -0,0 +1,370 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.util.Nat;
/** Linear system factories. */
public final class Models {
private Models() {
// Utility class
}
/**
* Creates a flywheel state-space model from physical constants.
*
* <p>The states are [angular velocity], the inputs are [voltage], and the outputs are [angular
* velocity].
*
* @param motor The motor (or gearbox) attached to the flywheel.
* @param J The moment of inertia J of the flywheel.
* @param gearing Gear ratio from motor to flywheel (greater than 1 is a reduction).
* @return Flywheel state-space model.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N1, N1, N1> flywheelFromPhysicalConstants(
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
var A =
MatBuilder.fill(
Nat.N1(), Nat.N1(), -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J));
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), gearing * motor.Kt / (motor.R * J));
var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0);
var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ (V/(rad/s²))
* from the feedforward model u = kᵥv + kₐa.
*
* <p>The states are [velocity], the inputs are [voltage], and the outputs are [velocity].
*
* @param kV The velocity gain, in V/(rad/s).
* @param kA The acceleration gain, in V/(rad/s²).
* @return Flywheel state-space model.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N1, N1, N1> flywheelFromSysId(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kV / kA);
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / kA);
var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0);
var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates an elevator state-space model from physical constants.
*
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
* [position, velocity].
*
* @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 gearing Gear ratio from motor to carriage (greater than 1 is a reduction).
* @return Elevator state-space model.
* @throws IllegalArgumentException if mass &lt;= 0, radius &lt;= 0, or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> elevatorFromPhysicalConstants(
DCMotor motor, double mass, double radius, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (radius <= 0.0) {
throw new IllegalArgumentException("radius must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
var A =
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0.0,
1.0,
0.0,
-Math.pow(gearing, 2) * motor.Kt / (motor.R * Math.pow(radius, 2) * mass * motor.Kv));
var B =
MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * radius * mass));
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and kₐ (V/(m/s²)) from
* the feedforward model u = kᵥv + kₐa.
*
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
* [position, velocity].
*
* @param kV The velocity gain, in V/(m/s).
* @param kA The acceleration gain, in V/(m/s²).
* @return Elevator state-space model.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N1, N2> elevatorFromSysId(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA);
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Create a single-jointed arm state-space model from physical constants.
*
* <p>The states are [angle, angular velocity], the inputs are [voltage], and the outputs are
* [angle, angular velocity].
*
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
* @param gearing Gear ratio from motor to arm (greater than 1 is a reduction).
* @return Single-jointed arm state-space model.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> singleJointedArmFromPhysicalConstants(
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
var A =
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0.0,
1.0,
0.0,
-Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J));
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * J));
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates a single-jointed arm state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ
* (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
*
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
* [position, velocity].
*
* @param kV The velocity gain, in volts/(unit/sec).
* @param kA The acceleration gain, in volts/(unit/sec²).
* @return Single-jointed arm state-space model.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N1, N2> singleJointedArmFromSysId(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA);
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates a differential drive state-space model from physical constants.
*
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
* voltage], and the outputs are [left velocity, right velocity].
*
* @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 trackwidth), in meters.
* @param J The moment of inertia of the robot.
* @param gearing Gear ratio from motor to wheel (greater than 1 is a reduction).
* @return Differential drive state-space model.
* @throws IllegalArgumentException if mass &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
* &lt;= 0.
*/
public static LinearSystem<N2, N2, N2> differentialDriveFromPhysicalConstants(
DCMotor motor, double mass, double r, double rb, double J, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (r <= 0.0) {
throw new IllegalArgumentException("r must be greater than zero.");
}
if (rb <= 0.0) {
throw new IllegalArgumentException("rb must be greater than zero.");
}
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
double C1 = -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * Math.pow(r, 2));
double C2 = gearing * motor.Kt / (motor.R * r);
var A =
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
(1 / mass + Math.pow(rb, 2) / J) * C1,
(1 / mass - Math.pow(rb, 2) / J) * C1,
(1 / mass - Math.pow(rb, 2) / J) * C1,
(1 / mass + Math.pow(rb, 2) / J) * C1);
var B =
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
(1 / mass + Math.pow(rb, 2) / J) * C2,
(1 / mass - Math.pow(rb, 2) / J) * C2,
(1 / mass - Math.pow(rb, 2) / J) * C2,
(1 / mass + Math.pow(rb, 2) / J) * C2);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear
* {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases.
*
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
* voltage], and the outputs are [left velocity, right velocity].
*
* @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 (meters per second).
* @param kAAngular The angular acceleration gain in volts per (meters per second squared).
* @return Differential drive state-space model.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
* kAAngular &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N2, N2> differentialDriveFromSysId(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
double A1 = -0.5 * (kVLinear / kALinear + kVAngular / kAAngular);
double A2 = -0.5 * (kVLinear / kALinear - kVAngular / kAAngular);
double B1 = 0.5 / kALinear + 0.5 / kAAngular;
double B2 = 0.5 / kALinear - 0.5 / kAAngular;
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1);
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear
* {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases.
*
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
* voltage], and the outputs are [left velocity, right velocity].
*
* @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 (radians per second).
* @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, in
* meters.
* @return Differential drive state-space model.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
* kAAngular &lt;= 0, or trackwidth &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N2, N2> differentialDriveFromSysId(
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0.0) {
throw new IllegalArgumentException("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 differentialDriveFromSysId(
kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
}
}

390
wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java
View File

@@ -1,390 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.util.Nat;
/** Linear system ID utility functions. */
public final class LinearSystemId {
private LinearSystemId() {
// Utility class
}
/**
* Create a state-space model of an elevator system. The states of the system are [position,
* velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* @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 gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if mass &lt;= 0, radius &lt;= 0, or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> createElevatorSystem(
DCMotor motor, double mass, double radius, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (radius <= 0.0) {
throw new IllegalArgumentException("radius must be greater than zero.");
}
if (gearing <= 0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
return new LinearSystem<>(
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* 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].
*
* @param motor The motor (or gearbox) attached to the flywheel.
* @param J The moment of inertia J of the flywheel in kg-m².
* @param gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
return new LinearSystem<>(
VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N1()),
new Matrix<>(Nat.N1(), Nat.N1()));
}
/**
* 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]ᵀ.
*
* @param motor The motor (or gearbox) attached to system.
* @param J The moment of inertia J of the DC motor in kg-m².
* @param gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> createDCMotorSystem(
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
return new LinearSystem<>(
MatBuilder.fill(
Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Create a state-space model of a DC motor 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].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link org.wpilib.math.util.Units} class for converting between unit types.
*
* <p>The parameters provided by the user are from this feedforward model:
*
* <p>u = K_v v + K_a a
*
* @param kV The velocity gain, in volts/(unit/sec)
* @param kA The acceleration gain, in volts/(unit/sec²)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
return new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
VecBuilder.fill(0, 1 / kA),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Create a state-space model of a differential drive drivetrain. In this model, the states are
* [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
* [left velocity, right velocity]ᵀ.
*
* @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 the trackwidth) in meters.
* @param J The moment of inertia of the robot in kg-m².
* @param gearing The gearing reduction as output over input.
* @return A LinearSystem representing a differential drivetrain.
* @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
* &lt;= 0.
*/
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
DCMotor motor, double mass, double r, double rb, double J, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (r <= 0.0) {
throw new IllegalArgumentException("r must be greater than zero.");
}
if (rb <= 0.0) {
throw new IllegalArgumentException("rb must be greater than zero.");
}
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r);
var C2 = gearing * motor.Kt / (motor.R * r);
final double C3 = 1 / mass + rb * rb / J;
final double C4 = 1 / mass - rb * rb / J;
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
/**
* 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, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm in kg-m².
* @param gearing The gearing between the motor and arm, in output over input. Most of the time
* this will be greater than 1.
* @return A LinearSystem representing the given characterized constants.
*/
public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
return new LinearSystem<>(
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
* [velocity], inputs are [voltage], and outputs are [velocity].
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link org.wpilib.math.util.Units} class for converting between unit types.
*
* <p>The parameters provided by the user are from this feedforward model:
*
* <p>u = K_v v + K_a a
*
* @param kV The velocity gain, in volts/(unit/sec)
* @param kA The acceleration gain, in volts/(unit/sec²)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
return new LinearSystem<>(
VecBuilder.fill(-kV / kA),
VecBuilder.fill(1.0 / kA),
VecBuilder.fill(1.0),
VecBuilder.fill(0.0));
}
/**
* Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link org.wpilib.math.util.Units} class for converting between unit types.
*
* <p>The parameters provided by the user are from this feedforward model:
*
* <p>u = K_v v + K_a a
*
* @param kV The velocity gain, in volts/(unit/sec)
* @param kA The acceleration gain, in volts/(unit/sec²)
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N1, N2> identifyPositionSystem(double kV, double kA) {
if (kV < 0.0) {
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
}
if (kA <= 0.0) {
throw new IllegalArgumentException("Ka must be greater than zero.");
}
return new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
VecBuilder.fill(0.0, 1.0 / kA),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA 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.
*
* <p>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 (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 (meters per second).
* @param kAAngular The angular acceleration gain in volts per (meters per second squared).
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
* kAAngular &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
return new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
}
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
* {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
* (volts/(radian/sec²))} cases. This can be found using SysId.
*
* <p>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 (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 (radians per second).
* @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, in
* meters.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
* kAAngular &lt;= 0, or trackwidth &lt;= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
if (kVLinear <= 0.0) {
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
}
if (kALinear <= 0.0) {
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
}
if (kVAngular <= 0.0) {
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
}
if (kAAngular <= 0.0) {
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
}
if (trackwidth <= 0.0) {
throw new IllegalArgumentException("trackwidth 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, kAAngular * 2.0 / trackwidth);
}
}

6
wpimath/src/main/java/org/wpilib/math/system/plant/proto/DCMotorProto.java → wpimath/src/main/java/org/wpilib/math/system/proto/DCMotorProto.java
View File

@@ -2,10 +2,10 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant.proto;
package org.wpilib.math.system.proto;
import org.wpilib.math.proto.Plant.ProtobufDCMotor;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.proto.System.ProtobufDCMotor;
import org.wpilib.math.system.DCMotor;
import org.wpilib.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;

4
wpimath/src/main/java/org/wpilib/math/system/plant/struct/DCMotorStruct.java → wpimath/src/main/java/org/wpilib/math/system/struct/DCMotorStruct.java
View File

@@ -2,10 +2,10 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant.struct;
package org.wpilib.math.system.struct;
import java.nio.ByteBuffer;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.DCMotor;
import org.wpilib.util.struct.Struct;
public class DCMotorStruct implements Struct<DCMotor> {

4
wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp → wpimath/src/main/native/cpp/system/proto/DCMotorProto.cpp
View File

@@ -2,11 +2,11 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/system/plant/proto/DCMotorProto.hpp"
#include "wpi/math/system/proto/DCMotorProto.hpp"
#include <optional>
#include "wpimath/protobuf/plant.npb.h"
#include "wpimath/protobuf/system.npb.h"
std::optional<wpi::math::DCMotor>
wpi::util::Protobuf<wpi::math::DCMotor>::Unpack(InputStream& stream) {

2
wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp → wpimath/src/main/native/cpp/system/struct/DCMotorStruct.cpp
View File

@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/system/plant/struct/DCMotorStruct.hpp"
#include "wpi/math/system/struct/DCMotorStruct.hpp"
namespace {
constexpr size_t kNominalVoltageOff = 0;

3
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp
View File

@@ -23,8 +23,7 @@ namespace wpi::math {
* Filters the provided voltages to limit a differential drive's linear and
* angular acceleration.
*
* The differential drive model can be created via the functions in
* LinearSystemId.
* The differential drive model can be created via the functions in Models.
*/
class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
public:

6
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp
View File

@@ -6,7 +6,7 @@
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/acceleration.hpp"
#include "wpi/units/angular_acceleration.hpp"
#include "wpi/units/angular_velocity.hpp"
@@ -42,7 +42,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
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, wpi::units::meter_t trackwidth)
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
// See Models::DifferentialDriveFromSysId(decltype(1_V / 1_mps),
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
// 1_rad_per_s_sq))
: DifferentialDriveFeedforward{kVLinear, kALinear,
@@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular,
decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
: m_plant{wpi::math::Models::DifferentialDriveFromSysId(
kVLinear, kALinear, kVAngular, kAAngular)},
m_kVLinear{kVLinear},
m_kALinear{kALinear},

42
wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp
View File

@@ -6,7 +6,6 @@
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"
@@ -70,47 +69,6 @@ class ElevatorFeedforward {
}
}
/**
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> velocity,
wpi::units::unit_t<Acceleration> acceleration) const {
return kS * wpi::util::sgn(velocity) + kG + kV * velocity +
kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Vectord<1> r{{currentVelocity.value()}};
Vectord<1> nextR{{nextVelocity.value()}};
return kG + kS * wpi::util::sgn(currentVelocity.value()) +
wpi::units::volt_t{feedforward.Calculate(r, nextR)(0)};
}
/**
* Calculates the feedforward from the gains and setpoint assuming discrete
* control. Use this method when the setpoint does not change.

4
wpimath/src/main/native/include/wpi/math/system/plant/DCMotor.hpp → wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp
View File

@@ -289,5 +289,5 @@ class WPILIB_DLLEXPORT DCMotor {
} // namespace wpi::math
#include "wpi/math/system/plant/proto/DCMotorProto.hpp"
#include "wpi/math/system/plant/struct/DCMotorStruct.hpp"
#include "wpi/math/system/proto/DCMotorProto.hpp"
#include "wpi/math/system/struct/DCMotorStruct.hpp"

499
wpimath/src/main/native/include/wpi/math/system/plant/LinearSystemId.hpp → wpimath/src/main/native/include/wpi/math/system/Models.hpp
View File

@@ -4,13 +4,12 @@
#pragma once
#include <concepts>
#include <stdexcept>
#include <gcem.hpp>
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/units/acceleration.hpp"
#include "wpi/units/angular_acceleration.hpp"
#include "wpi/units/angular_velocity.hpp"
@@ -22,9 +21,9 @@
namespace wpi::math {
/**
* Linear system ID utility functions.
* Linear system factories.
*/
class WPILIB_DLLEXPORT LinearSystemId {
class WPILIB_DLLEXPORT Models {
public:
template <typename Distance>
using Velocity_t = wpi::units::unit_t<wpi::units::compound_unit<
@@ -37,17 +36,80 @@ class WPILIB_DLLEXPORT LinearSystemId {
wpi::units::inverse<wpi::units::seconds>>>;
/**
* Create a state-space model of the elevator system. The states of the system
* are [position, velocity]ᵀ, inputs are [voltage], and outputs are [position,
* velocity]ᵀ.
* Creates a flywheel state-space model from physical constants.
*
* The states are [angular velocity], the inputs are [voltage], and the
* outputs are [angular velocity].
*
* @param motor The motor (or gearbox) attached to the flywheel.
* @param J The moment of inertia J of the flywheel.
* @param gearing Gear ratio from motor to flywheel (greater than 1 is a
* reduction).
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static constexpr LinearSystem<1, 1, 1> FlywheelFromPhysicalConstants(
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
{(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<1, 1> B{{(gearing * 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);
}
/**
* Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s))
* and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
*
* The states are [velocity], the inputs are [voltage], and the outputs are
* [velocity].
*
* @param kV The velocity gain, in V/(rad/s).
* @param kA The acceleration gain, in V/(rad/s²).
* @throws std::domain_error if kV < 0 or kA <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
static constexpr LinearSystem<1, 1, 1> FlywheelFromSysId(
decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) {
if (kV < decltype(kV){0}) {
throw std::domain_error("Kv must be greater than or equal to zero.");
}
if (kA <= decltype(kA){0}) {
throw std::domain_error("Ka must be greater than zero.");
}
Matrixd<1, 1> A{{-kV.value() / kA.value()}};
Matrixd<1, 1> B{{1.0 / kA.value()}};
Matrixd<1, 1> C{{1.0}};
Matrixd<1, 1> D{{0.0}};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Creates an elevator state-space model from physical constants.
*
* The states are [position, velocity], the inputs are [voltage], and the
* outputs are [position, velocity].
*
* @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 gearing Gear ratio from motor to carriage.
* @param gearing Gear ratio from motor to carriage (greater than 1 is a
* reduction).
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
*/
static constexpr LinearSystem<2, 1, 2> ElevatorSystem(
static constexpr LinearSystem<2, 1, 2> ElevatorFromPhysicalConstants(
DCMotor motor, wpi::units::kilogram_t mass, wpi::units::meter_t radius,
double gearing) {
if (mass <= 0_kg) {
@@ -74,105 +136,21 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
/**
* 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, angular velocity]ᵀ.
* Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and
* kₐ (V/(m/s²)) from the feedforward model u = kᵥv + kₐa.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
* @param gearing Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static constexpr LinearSystem<2, 1, 2> SingleJointedArmSystem(
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
{0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<2, 1> B{{0.0}, {(gearing * 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);
}
/**
* 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].
* The states are [position, velocity], the inputs are [voltage], and the
* outputs are [position, 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.
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @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.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
template <typename Distance>
requires std::same_as<wpi::units::meter, Distance> ||
std::same_as<wpi::units::radian, Distance>
static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
if (kV < decltype(kV){0}) {
throw std::domain_error("Kv must be greater than or equal to zero.");
}
if (kA <= decltype(kA){0}) {
throw std::domain_error("Ka must be greater than zero.");
}
Matrixd<1, 1> A{{-kV.value() / kA.value()}};
Matrixd<1, 1> B{{1.0 / kA.value()}};
Matrixd<1, 1> C{{1.0}};
Matrixd<1, 1> D{{0.0}};
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* 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, 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.
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volts/(unit/sec).
* @param kA The acceleration gain, in volts/(unit/sec²).
* @param kV The velocity gain, in V/(m/s).
* @param kA The acceleration gain, in V/(m/s²).
*
* @throws std::domain_error if kV < 0 or kA <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
template <typename Distance>
requires std::same_as<wpi::units::meter, Distance> ||
std::same_as<wpi::units::radian, Distance>
static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
static constexpr LinearSystem<2, 1, 2> ElevatorFromSysId(
decltype(1_V / 1_mps) kV, decltype(1_V / 1_mps_sq) kA) {
if (kV < decltype(kV){0}) {
throw std::domain_error("Kv must be greater than or equal to zero.");
}
@@ -189,167 +167,18 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA
* 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.
* Create a single-jointed arm state-space model from physical constants.
*
* States: [[left velocity], [right velocity]]<br>
* Inputs: [[left voltage], [right voltage]]<br>
* Outputs: [[left velocity], [right velocity]]
* The states are [angle, angular velocity], the inputs are [voltage], and the
* outputs are [angle, angular velocity].
*
* @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 (meters per
* second).
* @param kAAngular The angular acceleration gain in volts per (meters per
* second squared).
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* or kAAngular <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
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) {
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();
A1 /= 2.0;
A2 /= 2.0;
B1 /= 2.0;
B2 /= 2.0;
Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B{{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);
}
/**
* Identify a differential drive drivetrain given the drivetrain's kV and kA
* 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 (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 (radians per
* second).
* @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, in meters.
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* kAAngular <= 0, or trackwidth <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
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,
wpi::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);
}
/**
* 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].
*
* @param motor The motor (or gearbox) attached to the flywheel.
* @param J The moment of inertia J of the flywheel.
* @param gearing Gear ratio from motor to flywheel.
* @param motor The motor (or gearbox) attached to the arm.
* @param J The moment of inertia J of the arm.
* @param gearing Gear ratio from motor to arm (greater than 1 is a
* reduction).
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static constexpr LinearSystem<1, 1, 1> FlywheelSystem(
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
{(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
.value()}};
Matrixd<1, 1> B{{(gearing * 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);
}
/**
* 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]ᵀ.
*
* @param motor The motor (or gearbox) attached to the system.
* @param J the moment of inertia J of the DC motor.
* @param gearing Gear ratio from motor to output.
* @throws std::domain_error if J <= 0 or gearing <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromPhysicalConstants(
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
@@ -370,32 +199,21 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
/**
* Create a state-space model of a DC motor 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 [angular position, angular
* velocity]ᵀ, inputs are [voltage], and outputs are [angular position,
* angular velocity]ᵀ.
* Creates a single-jointed arm state-space model from SysId constants kᵥ
* (V/(rad/s)) and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
*
* 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.
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
* The states are [position, velocity], the inputs are [voltage], and the
* outputs are [position, velocity].
*
* @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.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
template <typename Distance>
requires std::same_as<wpi::units::meter, Distance> ||
std::same_as<wpi::units::radian, Distance>
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromSysId(
decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) {
if (kV < decltype(kV){0}) {
throw std::domain_error("Kv must be greater than or equal to zero.");
}
@@ -404,7 +222,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
@@ -412,21 +230,23 @@ class WPILIB_DLLEXPORT LinearSystemId {
}
/**
* Create a state-space model of differential drive drivetrain. In this model,
* the states are [left velocity, right velocity]ᵀ, the inputs are [left
* Creates a differential drive state-space model from physical constants.
*
* The states are [left velocity, right velocity], the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity]ᵀ.
* velocity].
*
* @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 trackwidth), in meters.
* @param J The moment of inertia of the robot.
* @param gearing Gear ratio from motor to wheel.
* @param gearing Gear ratio from motor to wheel (greater than 1 is a
* reduction).
* @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
* gearing <= 0.
*/
static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromPhysicalConstants(
const DCMotor& motor, wpi::units::kilogram_t mass, wpi::units::meter_t r,
wpi::units::meter_t rb, wpi::units::kilogram_square_meter_t J,
double gearing) {
@@ -465,6 +285,119 @@ class WPILIB_DLLEXPORT LinearSystemId {
return LinearSystem<2, 2, 2>(A, B, C, D);
}
/**
* Creates a differential drive state-space model from SysId constants kᵥ and
* kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s),
* (V/(rad/s²))} cases.
*
* The states are [left velocity, right velocity], the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity].
*
* @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 (meters per
* second).
* @param kAAngular The angular acceleration gain in volts per (meters per
* second squared).
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* or kAAngular <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId(
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 = -0.5 * (kVLinear.value() / kALinear.value() +
kVAngular.value() / kAAngular.value());
double A2 = -0.5 * (kVLinear.value() / kALinear.value() -
kVAngular.value() / kAAngular.value());
double B1 = 0.5 / kALinear.value() + 0.5 / kAAngular.value();
double B2 = 0.5 / kALinear.value() - 0.5 / kAAngular.value();
Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
Matrixd<2, 2> B{{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);
}
/**
* Creates a differential drive state-space model from SysId constants kᵥ and
* kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s),
* (V/(rad/s²))} cases.
*
* The states are [left velocity, right velocity], the inputs are [left
* voltage, right voltage], and the outputs are [left velocity, right
* velocity].
*
* @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 (radians per
* second).
* @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, in meters.
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
* kAAngular <= 0, or trackwidth <= 0.
* @see <a
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
*/
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId(
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,
wpi::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 DifferentialDriveFromSysId(kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad);
}
};
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/system/plant/proto/DCMotorProto.hpp → wpimath/src/main/native/include/wpi/math/system/proto/DCMotorProto.hpp
View File

@@ -4,10 +4,10 @@
#pragma once
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/plant.npb.h"
#include "wpimath/protobuf/system.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DCMotor> {

2
wpimath/src/main/native/include/wpi/math/system/plant/struct/DCMotorStruct.hpp → wpimath/src/main/native/include/wpi/math/system/struct/DCMotorStruct.hpp
View File

@@ -4,7 +4,7 @@
#pragma once
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"

13
wpimath/src/main/proto/plant.proto
View File

@@ -1,13 +0,0 @@
syntax = "proto3";
package wpi.proto;
option java_package = "org.wpilib.math.proto";
message ProtobufDCMotor {
double nominal_voltage = 1;
double stall_torque = 2;
double stall_current = 3;
double free_current = 4;
double free_speed = 5;
}

8
wpimath/src/main/proto/system.proto
View File

@@ -6,6 +6,14 @@ import "wpimath.proto";
option java_package = "org.wpilib.math.proto";
message ProtobufDCMotor {
double nominal_voltage = 1;
double stall_torque = 2;
double stall_current = 3;
double free_current = 4;
double free_speed = 5;
}
message ProtobufLinearSystem {
uint32 num_states = 1;
uint32 num_inputs = 2;

6
wpimath/src/main/python/pyproject.toml
View File

@@ -1560,17 +1560,15 @@ SplineHelper = "wpi/math/spline/SplineHelper.hpp"
SplineParameterizer = "wpi/math/spline/SplineParameterizer.hpp"
# wpi/math/system
DCMotor = "wpi/math/system/DCMotor.hpp"
# Discretization = "wpi/math/system/Discretization.hpp"
LinearSystem = "wpi/math/system/LinearSystem.hpp"
LinearSystemLoop = "wpi/math/system/LinearSystemLoop.hpp"
# LinearSystemUtil = "wpi/math/system/LinearSystemUtil.hpp"
Models = "wpi/math/system/Models.hpp"
# NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp"
# NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp"
# wpi/math/system/plant
DCMotor = "wpi/math/system/plant/DCMotor.hpp"
LinearSystemId = "wpi/math/system/plant/LinearSystemId.hpp"
# wpi/math/trajectory
ExponentialProfile = "wpi/math/trajectory/ExponentialProfile.hpp"
Trajectory = "wpi/math/trajectory/Trajectory.hpp"

4
wpimath/src/main/python/semiwrap/ElevatorFeedforward.yml
View File

@@ -10,10 +10,6 @@ classes:
wpi::units::volt_t, wpi::units::volt_t, wpi::units::unit_t<kv_unit>, wpi::units::unit_t<ka_unit>:
Calculate:
overloads:
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Acceleration> [const]:
ignore: true
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Velocity>, wpi::units::second_t [const]:
ignore: true
wpi::units::unit_t<Velocity> [const]:
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Velocity> [const]:
MaxAchievableVelocity:

59
wpimath/src/main/python/semiwrap/LinearSystemId.yml
View File

@@ -1,59 +0,0 @@
classes:
wpi::math::LinearSystemId:
typealias:
- kv_meters = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::meters_per_second>>>
- kv_radians = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second>>>
- ka_meters = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::meters_per_second_squared>>>
- ka_radians = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second_squared>>>
methods:
ElevatorSystem:
SingleJointedArmSystem:
IdentifyVelocitySystem:
rename: identifyVelocitySystemMeters
cpp_code: |
[](kv_meters kV, ka_meters kA) {
return wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(kV, kA);
}
IdentifyPositionSystem:
rename: identifyPositionSystemMeters
cpp_code: |
[](kv_meters kV, ka_meters kA) {
return wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(kV, kA);
}
IdentifyDrivetrainSystem:
overloads:
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq):
cpp_code: |
[](kv_meters kVlinear, ka_meters kAlinear, kv_meters kVangular, ka_meters kAangular) {
return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular);
}
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t:
cpp_code: |
[](kv_meters kVlinear, ka_meters kAlinear, kv_radians kVangular, ka_radians kAangular, wpi::units::meter_t trackWidth) {
return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular, trackWidth);
}
FlywheelSystem:
DCMotorSystem:
overloads:
DCMotor, wpi::units::kilogram_square_meter_t, double:
decltype(1_V/Velocity_t<Distance> (1)), decltype(1_V/Acceleration_t<Distance> (1)):
cpp_code: |
[](kv_meters kv, ka_meters ka) {
return wpi::math::LinearSystemId::DCMotorSystem<wpi::units::meter>(kv, ka);
}
DrivetrainVelocitySystem:
inline_code: |
.def_static("DCMotorSystemRadians", [](kv_radians kV, ka_radians kA) {
return wpi::math::LinearSystemId::DCMotorSystem<wpi::units::radian>(kV, kA);
}, py::arg("kV"), py::arg("kA"), release_gil()
)
.def_static("identifyVelocitySystemRadians", [](kv_radians kV, ka_radians kA) {
return wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(kV, kA);
}, py::arg("kV"), py::arg("kA"), release_gil()
)
.def_static("identifyPositionSystemRadians", [](kv_radians kV, ka_radians kA) {
return wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::radian>(kV, kA);
}, py::arg("kV"), py::arg("kA"), release_gil()
)

14
wpimath/src/main/python/semiwrap/Models.yml Normal file
View File

@@ -0,0 +1,14 @@
classes:
wpi::math::Models:
methods:
FlywheelFromPhysicalConstants:
FlywheelFromSysId:
ElevatorFromPhysicalConstants:
ElevatorFromSysId:
SingleJointedArmFromPhysicalConstants:
SingleJointedArmFromSysId:
DifferentialDriveFromPhysicalConstants:
DifferentialDriveFromSysId:
overloads:
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq):
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t:

4
wpimath/src/main/python/wpimath/__init__.py
View File

@@ -60,7 +60,6 @@ from ._wpimath import (
LinearQuadraticRegulator_2_1,
LinearQuadraticRegulator_2_2,
LinearQuadraticRegulator_3_2,
LinearSystemId,
LinearSystemLoop_1_1_1,
LinearSystemLoop_2_1_1,
LinearSystemLoop_2_1_2,
@@ -94,6 +93,7 @@ from ._wpimath import (
MecanumDriveWheelPositions,
MecanumDriveWheelSpeeds,
MedianFilter,
Models,
PIDController,
Pose2d,
Pose3d,
@@ -247,7 +247,6 @@ __all__ = [
"LinearQuadraticRegulator_2_1",
"LinearQuadraticRegulator_2_2",
"LinearQuadraticRegulator_3_2",
"LinearSystemId",
"LinearSystemLoop_1_1_1",
"LinearSystemLoop_2_1_1",
"LinearSystemLoop_2_1_2",
@@ -281,6 +280,7 @@ __all__ = [
"MecanumDriveWheelPositions",
"MecanumDriveWheelSpeeds",
"MedianFilter",
"Models",
"PIDController",
"Pose2d",
"Pose3d",