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] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -7,6 +7,30 @@ package edu.wpi.first.math.system;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N10;
import edu.wpi.first.math.numbers.N11;
import edu.wpi.first.math.numbers.N12;
import edu.wpi.first.math.numbers.N13;
import edu.wpi.first.math.numbers.N14;
import edu.wpi.first.math.numbers.N15;
import edu.wpi.first.math.numbers.N16;
import edu.wpi.first.math.numbers.N17;
import edu.wpi.first.math.numbers.N18;
import edu.wpi.first.math.numbers.N19;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N20;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.math.numbers.N9;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
import org.ejml.simple.SimpleMatrix;
/**
* A plant defined using state-space notation.
@@ -196,6 +220,141 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
return m_C.times(x).plus(m_D.times(clampedU));
}
/**
* Returns the LinearSystem with the outputs listed in outputIndices.
*
* <p>This is used by state observers such as the Kalman Filter.
*
* @param outputIndices the list of output indices to include in the sliced system.
* @return the sliced LinearSystem with outputs set to row vectors of LinearSystem.
* @throws IllegalArgumentException if any outputIndices are outside the range of system outputs.
* @throws IllegalArgumentException if number of outputIndices exceeds the number of system
* outputs.
* @throws IllegalArgumentException if duplication exists in outputIndices.
*/
public LinearSystem<States, Inputs, ? extends Num> slice(int... outputIndices) {
for (int index : outputIndices) {
if (index < 0 || index >= m_C.getNumRows()) {
throw new IllegalArgumentException(
"Output indices out of range. This is usually due to model implementation errors.");
}
}
if (outputIndices.length >= m_C.getNumRows()) {
throw new IllegalArgumentException(
"More outputs requested than available. This is usually due to model implementation "
+ "errors.");
}
List<Integer> outputIndicesList =
Arrays.stream(outputIndices).distinct().boxed().collect(Collectors.toList());
Collections.sort(outputIndicesList);
if (outputIndices.length != outputIndicesList.size()) {
throw new IllegalArgumentException(
"Duplicate indices exist. This is usually due to model implementation " + "errors.");
}
SimpleMatrix new_C_Storage = new SimpleMatrix(outputIndices.length, m_C.getNumCols());
int row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_C.extractRowVector(index).getData();
new_C_Storage.setRow(row, 0, current_row_data);
row++;
}
SimpleMatrix new_D_Storage = new SimpleMatrix(outputIndices.length, m_D.getNumCols());
row = 0;
for (var index : outputIndicesList) {
var current_row_data = m_D.extractRowVector(index).getData();
new_D_Storage.setRow(row, 0, current_row_data);
row++;
}
switch (outputIndices.length) {
case 20:
Matrix<N20, States> new_C20 = new Matrix<>(new_C_Storage);
Matrix<N20, Inputs> new_D20 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C20, new_D20);
case 19:
Matrix<N19, States> new_C19 = new Matrix<>(new_C_Storage);
Matrix<N19, Inputs> new_D19 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C19, new_D19);
case 18:
Matrix<N18, States> new_C18 = new Matrix<>(new_C_Storage);
Matrix<N18, Inputs> new_D18 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C18, new_D18);
case 17:
Matrix<N17, States> new_C17 = new Matrix<>(new_C_Storage);
Matrix<N17, Inputs> new_D17 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C17, new_D17);
case 16:
Matrix<N16, States> new_C16 = new Matrix<>(new_C_Storage);
Matrix<N16, Inputs> new_D16 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C16, new_D16);
case 15:
Matrix<N15, States> new_C15 = new Matrix<>(new_C_Storage);
Matrix<N15, Inputs> new_D15 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C15, new_D15);
case 14:
Matrix<N14, States> new_C14 = new Matrix<>(new_C_Storage);
Matrix<N14, Inputs> new_D14 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C14, new_D14);
case 13:
Matrix<N13, States> new_C13 = new Matrix<>(new_C_Storage);
Matrix<N13, Inputs> new_D13 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C13, new_D13);
case 12:
Matrix<N12, States> new_C12 = new Matrix<>(new_C_Storage);
Matrix<N12, Inputs> new_D12 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C12, new_D12);
case 11:
Matrix<N11, States> new_C11 = new Matrix<>(new_C_Storage);
Matrix<N11, Inputs> new_D11 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C11, new_D11);
case 10:
Matrix<N10, States> new_C10 = new Matrix<>(new_C_Storage);
Matrix<N10, Inputs> new_D10 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C10, new_D10);
case 9:
Matrix<N9, States> new_C9 = new Matrix<>(new_C_Storage);
Matrix<N9, Inputs> new_D9 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C9, new_D9);
case 8:
Matrix<N8, States> new_C8 = new Matrix<>(new_C_Storage);
Matrix<N8, Inputs> new_D8 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C8, new_D8);
case 7:
Matrix<N7, States> new_C7 = new Matrix<>(new_C_Storage);
Matrix<N7, Inputs> new_D7 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C7, new_D7);
case 6:
Matrix<N6, States> new_C6 = new Matrix<>(new_C_Storage);
Matrix<N6, Inputs> new_D6 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C6, new_D6);
case 5:
Matrix<N5, States> new_C5 = new Matrix<>(new_C_Storage);
Matrix<N5, Inputs> new_D5 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C5, new_D5);
case 4:
Matrix<N4, States> new_C4 = new Matrix<>(new_C_Storage);
Matrix<N4, Inputs> new_D4 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C4, new_D4);
case 3:
Matrix<N3, States> new_C3 = new Matrix<>(new_C_Storage);
Matrix<N3, Inputs> new_D3 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C3, new_D3);
case 2:
Matrix<N2, States> new_C2 = new Matrix<>(new_C_Storage);
Matrix<N2, Inputs> new_D2 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C2, new_D2);
default:
Matrix<N1, States> new_C1 = new Matrix<>(new_C_Storage);
Matrix<N1, Inputs> new_D1 = new Matrix<>(new_D_Storage);
return new LinearSystem<>(m_A, m_B, new_C1, new_D1);
}
}
@Override
public String toString() {
return String.format(

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

@@ -29,7 +29,7 @@ public final class LinearSystemId {
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N1> createElevatorSystem(
public static LinearSystem<N2, N1, N2> createElevatorSystem(
DCMotor motor, double massKg, double radiusMeters, double gearing) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
@@ -52,8 +52,8 @@ public final class LinearSystemId {
* motor.KtNMPerAmp
/ (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
@@ -219,7 +219,7 @@ public final class LinearSystemId {
* this will be greater than 1.
* @return A LinearSystem representing the given characterized constants.
*/
public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
DCMotor motor, double JKgSquaredMeters, double gearing) {
if (JKgSquaredMeters <= 0.0) {
throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
@@ -239,8 +239,8 @@ public final class LinearSystemId {
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**
@@ -294,7 +294,7 @@ public final class LinearSystemId {
* @throws IllegalArgumentException if kV &lt; 0 or kA &lt;= 0.
* @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
*/
public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
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.");
}
@@ -305,8 +305,8 @@ public final class LinearSystemId {
return new LinearSystem<>(
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
VecBuilder.fill(0.0, 1.0 / kA),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0),
VecBuilder.fill(0.0));
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
/**

16
wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
View File

@@ -6,7 +6,7 @@
using namespace frc;
LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
LinearSystem<2, 1, 2> LinearSystemId::ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing) {
@@ -27,13 +27,13 @@ LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
.value()}};
Matrixd<2, 1> B{0.0,
(gearing * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
LinearSystem<2, 1, 2> LinearSystemId::SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
@@ -47,10 +47,10 @@ LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
{0.0,
(-std::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<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{{0.0}, {0.0}};
return LinearSystem<2, 1, 1>(A, B, C, D);
return LinearSystem<2, 1, 2>(A, B, C, D);
}
LinearSystem<2, 2, 2> LinearSystemId::IdentifyDrivetrainSystem(

54
wpimath/src/main/native/include/frc/system/LinearSystem.h
View File

@@ -5,9 +5,13 @@
#pragma once
#include <algorithm>
#include <concepts>
#include <functional>
#include <stdexcept>
#include <wpi/Algorithm.h>
#include <wpi/SmallVector.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
@@ -164,6 +168,56 @@ class LinearSystem {
return m_C * x + m_D * clampedU;
}
/**
* Returns the LinearSystem with the outputs listed in outputIndices.
*
* <p>This is used by state observers such as the Kalman Filter.
*
* @param outputIndices the list of output indices to include in the sliced
* system.
* @return the sliced LinearSystem with outputs set to row vectors of
* LinearSystem.
* @throws std::domain_error if any outputIndices are outside the range of
* system outputs.
* @throws std::domain_error if number of outputIndices exceeds the system
* outputs.
* @throws std::domain_error if duplication exists in outputIndices.
*/
template <std::same_as<int>... OutputIndices>
LinearSystem<States, Inputs, sizeof...(OutputIndices)> Slice(
OutputIndices... outputIndices) {
static_assert(sizeof...(OutputIndices) <= Outputs,
"More outputs requested than available. This is usually due "
"to model implementation errors.");
wpi::for_each(
[](size_t i, const auto& elem) {
if (elem < 0 || elem >= Outputs) {
throw std::domain_error(
"Slice indices out of range. This is usually due to model "
"implementation errors.");
}
},
outputIndices...);
// Sort and deduplicate output indices
wpi::SmallVector<int> outputIndicesArray{outputIndices...};
std::sort(outputIndicesArray.begin(), outputIndicesArray.end());
auto last =
std::unique(outputIndicesArray.begin(), outputIndicesArray.end());
outputIndicesArray.erase(last, outputIndicesArray.end());
if (outputIndicesArray.size() != sizeof...(outputIndices)) {
throw std::domain_error(
"Duplicate indices exist. This is usually due to model "
"implementation errors.");
}
return LinearSystem<States, Inputs, sizeof...(OutputIndices)>{
m_A, m_B, m_C(outputIndicesArray, Eigen::placeholders::all),
m_D(outputIndicesArray, Eigen::placeholders::all)};
}
private:
/**
* Continuous system matrix.

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

@@ -44,7 +44,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to carriage.
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
static LinearSystem<2, 1, 2> ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
double gearing);
@@ -59,7 +59,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* @param gearing Gear ratio from motor to arm.
* @throws std::domain_error if J <= 0 or gearing <= 0.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
static LinearSystem<2, 1, 2> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double gearing);
/**