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

[wpimath] Fix drivetrain system identification (#3406)

Browse Source 
The units for angular Kv and Ka were inconsistent with the derivation. A
second factory function overload was added for angular units that uses a
trackwidth to convert to the other form.

Notice how section 15.2 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf
defines the angular feedforward as u = Kv,angular v instead of u = Kv,angular + omega.
The units cancel for elements of A but not B, so just the B matrix was incorrect in our code.

This breaks existing C++ code since the units are part of the function
signature.
This commit is contained in:
Tyler Veness
2021-06-05 11:22:05 -07:00
committed by GitHub
parent 989de4a1bf
commit 15c521a7fe
4 changed files with 104 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
View File

@@ -98,8 +98,7 @@ class Drivetrain {
frc::Field2d m_fieldSim;
frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
frc::LinearSystemId::IdentifyDrivetrainSystem(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_rad_per_s,
0.3_V / 1_rad_per_s_sq);
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
};

8
wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
View File

@@ -52,11 +52,11 @@ constexpr double kEncoderDistancePerPulse =
// Toolsuite provides a convenient tool for obtaining these values for your
// robot.
constexpr auto ks = 0.22_V;
constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
constexpr auto kv = 1.98 * 1_V / 1_mps;
constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad;
constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad;
constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;

59
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -177,6 +177,37 @@ public final class LinearSystemId {
VecBuilder.fill(0.0));
}
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
* volts/(meter/sec^2)) cases. This can be found using frc-characterization. The states of the
* system are [left velocity, right velocity]^T, inputs are [left voltage, right voltage]^T, and
* outputs are [left velocity, right velocity]^T.
*
* @param kVLinear The linear velocity gain, volts per (meter per second).
* @param kALinear The linear acceleration gain, volts per (meter per second squared).
* @param kVAngular The angular velocity gain, volts per (meter per second).
* @param kAAngular The angular acceleration gain, volts per (meter per second squared).
* @return A LinearSystem representing the given characterized constants.
* @see <a href="https://github.com/wpilibsuite/frc-characterization">
* https://github.com/wpilibsuite/frc-characterization</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
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<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
}
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
@@ -188,24 +219,26 @@ public final class LinearSystemId {
* @param kALinear The linear acceleration gain, volts per (meter per second squared).
* @param kVAngular The angular velocity gain, volts per (radians per second).
* @param kAAngular The angular acceleration gain, volts per (radians per second squared).
* @param trackwidth The width of the drivetrain in meters.
* @return A LinearSystem representing the given characterized constants.
* @see <a href="https://github.com/wpilibsuite/frc-characterization">
* https://github.com/wpilibsuite/frc-characterization</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
final double c = 0.5 / (kALinear * kAAngular);
final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular);
final double B1 = c * (kALinear + kAAngular);
final double B2 = c * (kAAngular - kALinear);
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
// 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);
}
}

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

@@ -10,6 +10,7 @@
#include "units/acceleration.h"
#include "units/angular_acceleration.h"
#include "units/angular_velocity.h"
#include "units/length.h"
#include "units/moment_of_inertia.h"
#include "units/velocity.h"
#include "units/voltage.h"
@@ -156,33 +157,68 @@ class LinearSystemId {
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param kVlinear The linear velocity gain, in volt seconds per distance.
* @param kAlinear The linear acceleration gain, in volt seconds^2 per
* distance.
* @param kVangular The angular velocity gain, in volt seconds per angle.
* @param kAangular The angular acceleration gain, in volt seconds^2 per
* angle.
* @param kVlinear The linear velocity gain in volts per (meter per second).
* @param kAlinear The linear acceleration gain in volts per (meter 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
* second squared).
*/
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) {
double c = 0.5 / (kAlinear.to<double>() * kAangular.to<double>());
double A1 = c * (-kAlinear.to<double>() * kVangular.to<double>() -
kVlinear.to<double>() * kAangular.to<double>());
double A2 = c * (kAlinear.to<double>() * kVangular.to<double>() -
kVlinear.to<double>() * kAangular.to<double>());
double B1 = c * (kAlinear.to<double>() + kAangular.to<double>());
double B2 = c * (kAangular.to<double>() - kAlinear.to<double>());
decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
double A1 = -(kVlinear.to<double>() / kAlinear.to<double>() +
kVangular.to<double>() / kAangular.to<double>());
double A2 = -(kVlinear.to<double>() / kAlinear.to<double>() -
kVangular.to<double>() / kAangular.to<double>());
double B1 = 1.0 / kAlinear.to<double>() + 1.0 / kAangular.to<double>();
double B2 = 1.0 / kAlinear.to<double>() - 1.0 / kAangular.to<double>();
auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
auto A = 0.5 * frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
auto B = 0.5 * frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
return LinearSystem<2, 2, 2>(A, B, C, D);
}
/**
* Constructs the state-space model for a 2 DOF drivetrain velocity system
* from system identification data.
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* 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
* second squared).
* @param kVangular The angular velocity gain in volts per (radian per
* second).
* @param kAangular The angular acceleration gain in volts per (radian per
* second squared).
* @param trackwidth The width of the drivetrain.
*/
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) {
// 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);
}
/**
* Constructs the state-space model for a flywheel.
*