mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
Merge branch '2022'
This commit is contained in:
Peter Johnson
@@ -0,0 +1,170 @@
|
||||
// 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 edu.wpi.first.math.system;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.math.Discretization;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
|
||||
@SuppressWarnings("ClassTypeParameterName")
|
||||
public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
|
||||
/** Continuous system matrix. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, States> m_A;
|
||||
|
||||
/** Continuous input matrix. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<States, Inputs> m_B;
|
||||
|
||||
/** Output matrix. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<Outputs, States> m_C;
|
||||
|
||||
/** Feedthrough matrix. */
|
||||
@SuppressWarnings("MemberName")
|
||||
private final Matrix<Outputs, Inputs> m_D;
|
||||
|
||||
/**
|
||||
* Construct a new LinearSystem from the four system matrices.
|
||||
*
|
||||
* @param a The system matrix A.
|
||||
* @param b The input matrix B.
|
||||
* @param c The output matrix C.
|
||||
* @param d The feedthrough matrix D.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public LinearSystem(
|
||||
Matrix<States, States> a,
|
||||
Matrix<States, Inputs> b,
|
||||
Matrix<Outputs, States> c,
|
||||
Matrix<Outputs, Inputs> d) {
|
||||
|
||||
this.m_A = a;
|
||||
this.m_B = b;
|
||||
this.m_C = c;
|
||||
this.m_D = d;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the system matrix A.
|
||||
*
|
||||
* @return the system matrix A.
|
||||
*/
|
||||
public Matrix<States, States> getA() {
|
||||
return m_A;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the system matrix A.
|
||||
*
|
||||
* @param row Row of A.
|
||||
* @param col Column of A.
|
||||
* @return the system matrix A at (i, j).
|
||||
*/
|
||||
public double getA(int row, int col) {
|
||||
return m_A.get(row, col);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the input matrix B.
|
||||
*
|
||||
* @return the input matrix B.
|
||||
*/
|
||||
public Matrix<States, Inputs> getB() {
|
||||
return m_B;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the input matrix B.
|
||||
*
|
||||
* @param row Row of B.
|
||||
* @param col Column of B.
|
||||
* @return The value of the input matrix B at (i, j).
|
||||
*/
|
||||
public double getB(int row, int col) {
|
||||
return m_B.get(row, col);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the output matrix C.
|
||||
*
|
||||
* @return Output matrix C.
|
||||
*/
|
||||
public Matrix<Outputs, States> getC() {
|
||||
return m_C;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the output matrix C.
|
||||
*
|
||||
* @param row Row of C.
|
||||
* @param col Column of C.
|
||||
* @return the double value of C at the given position.
|
||||
*/
|
||||
public double getC(int row, int col) {
|
||||
return m_C.get(row, col);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the feedthrough matrix D.
|
||||
*
|
||||
* @return the feedthrough matrix D.
|
||||
*/
|
||||
public Matrix<Outputs, Inputs> getD() {
|
||||
return m_D;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the feedthrough matrix D.
|
||||
*
|
||||
* @param row Row of D.
|
||||
* @param col Column of D.
|
||||
* @return The feedthrough matrix D at (i, j).
|
||||
*/
|
||||
public double getD(int row, int col) {
|
||||
return m_D.get(row, col);
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the new x given the old x and the control input.
|
||||
*
|
||||
* <p>This is used by state observers directly to run updates based on state estimate.
|
||||
*
|
||||
* @param x The current state.
|
||||
* @param clampedU The control input.
|
||||
* @param dtSeconds Timestep for model update.
|
||||
* @return the updated x.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<States, N1> calculateX(
|
||||
Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
|
||||
var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
|
||||
|
||||
return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
|
||||
}
|
||||
|
||||
/**
|
||||
* Computes the new y given the control input.
|
||||
*
|
||||
* <p>This is used by state observers directly to run updates based on state estimate.
|
||||
*
|
||||
* @param x The current state.
|
||||
* @param clampedU The control input.
|
||||
* @return the updated output matrix Y.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
|
||||
return m_C.times(x).plus(m_D.times(clampedU));
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format(
|
||||
"Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
|
||||
m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,346 @@
|
||||
// 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 edu.wpi.first.math.system;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
|
||||
import edu.wpi.first.math.controller.LinearQuadraticRegulator;
|
||||
import edu.wpi.first.math.estimator.KalmanFilter;
|
||||
import edu.wpi.first.math.math.StateSpaceUtil;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import java.util.function.Function;
|
||||
import org.ejml.MatrixDimensionException;
|
||||
import org.ejml.simple.SimpleMatrix;
|
||||
|
||||
/**
|
||||
* Combines a controller, feedforward, and observer for controlling a mechanism with full state
|
||||
* feedback.
|
||||
*
|
||||
* <p>For everything in this file, "inputs" and "outputs" are defined from the perspective of the
|
||||
* plant. This means U is an input and Y is an output (because you give the plant U (powers) and it
|
||||
* gives you back a Y (sensor values). This is the opposite of what they mean from the perspective
|
||||
* of the controller (U is an output because that's what goes to the motors and Y is an input
|
||||
* because that's what comes back from the sensors).
|
||||
*
|
||||
* <p>For more on the underlying math, read
|
||||
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
|
||||
*/
|
||||
@SuppressWarnings("ClassTypeParameterName")
|
||||
public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
|
||||
|
||||
private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
|
||||
private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
|
||||
private final KalmanFilter<States, Inputs, Outputs> m_observer;
|
||||
private Matrix<States, N1> m_nextR;
|
||||
private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
|
||||
|
||||
/**
|
||||
* Constructs a state-space loop with the given plant, controller, and observer. By default, the
|
||||
* initial reference is all zeros. Users should call reset with the initial system state before
|
||||
* enabling the loop. This constructor assumes that the input(s) to this system are voltage.
|
||||
*
|
||||
* @param plant State-space plant.
|
||||
* @param controller State-space controller.
|
||||
* @param observer State-space observer.
|
||||
* @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
|
||||
* @param dtSeconds The nominal timestep.
|
||||
*/
|
||||
public LinearSystemLoop(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
|
||||
KalmanFilter<States, Inputs, Outputs> observer,
|
||||
double maxVoltageVolts,
|
||||
double dtSeconds) {
|
||||
this(
|
||||
controller,
|
||||
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
|
||||
observer,
|
||||
u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a state-space loop with the given plant, controller, and observer. By default, the
|
||||
* initial reference is all zeros. Users should call reset with the initial system state before
|
||||
* enabling the loop.
|
||||
*
|
||||
* @param plant State-space plant.
|
||||
* @param controller State-space controller.
|
||||
* @param observer State-space observer.
|
||||
* @param clampFunction The function used to clamp the input U.
|
||||
* @param dtSeconds The nominal timestep.
|
||||
*/
|
||||
public LinearSystemLoop(
|
||||
LinearSystem<States, Inputs, Outputs> plant,
|
||||
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
|
||||
KalmanFilter<States, Inputs, Outputs> observer,
|
||||
Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
|
||||
double dtSeconds) {
|
||||
this(
|
||||
controller,
|
||||
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
|
||||
observer,
|
||||
clampFunction);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a state-space loop with the given controller, feedforward and observer. By default,
|
||||
* the initial reference is all zeros. Users should call reset with the initial system state
|
||||
* before enabling the loop.
|
||||
*
|
||||
* @param controller State-space controller.
|
||||
* @param feedforward Plant inversion feedforward.
|
||||
* @param observer State-space observer.
|
||||
* @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the inputs are
|
||||
* voltages.
|
||||
*/
|
||||
public LinearSystemLoop(
|
||||
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
|
||||
LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
|
||||
KalmanFilter<States, Inputs, Outputs> observer,
|
||||
double maxVoltageVolts) {
|
||||
this(
|
||||
controller,
|
||||
feedforward,
|
||||
observer,
|
||||
u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a state-space loop with the given controller, feedforward, and observer. By default,
|
||||
* the initial reference is all zeros. Users should call reset with the initial system state
|
||||
* before enabling the loop.
|
||||
*
|
||||
* @param controller State-space controller.
|
||||
* @param feedforward Plant inversion feedforward.
|
||||
* @param observer State-space observer.
|
||||
* @param clampFunction The function used to clamp the input U.
|
||||
*/
|
||||
public LinearSystemLoop(
|
||||
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
|
||||
LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
|
||||
KalmanFilter<States, Inputs, Outputs> observer,
|
||||
Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
|
||||
this.m_controller = controller;
|
||||
this.m_feedforward = feedforward;
|
||||
this.m_observer = observer;
|
||||
this.m_clampFunction = clampFunction;
|
||||
|
||||
m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
|
||||
reset(m_nextR);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the observer's state estimate x-hat.
|
||||
*
|
||||
* @return the observer's state estimate x-hat.
|
||||
*/
|
||||
public Matrix<States, N1> getXHat() {
|
||||
return getObserver().getXhat();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the observer's state estimate x-hat.
|
||||
*
|
||||
* @param row Row of x-hat.
|
||||
* @return the i-th element of the observer's state estimate x-hat.
|
||||
*/
|
||||
public double getXHat(int row) {
|
||||
return getObserver().getXhat(row);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the initial state estimate x-hat.
|
||||
*
|
||||
* @param xhat The initial state estimate x-hat.
|
||||
*/
|
||||
public void setXHat(Matrix<States, N1> xhat) {
|
||||
getObserver().setXhat(xhat);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set an element of the initial state estimate x-hat.
|
||||
*
|
||||
* @param row Row of x-hat.
|
||||
* @param value Value for element of x-hat.
|
||||
*/
|
||||
public void setXHat(int row, double value) {
|
||||
getObserver().setXhat(row, value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the controller's next reference r.
|
||||
*
|
||||
* @param row Row of r.
|
||||
* @return the element i of the controller's next reference r.
|
||||
*/
|
||||
public double getNextR(int row) {
|
||||
return getNextR().get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the controller's next reference r.
|
||||
*
|
||||
* @return the controller's next reference r.
|
||||
*/
|
||||
public Matrix<States, N1> getNextR() {
|
||||
return m_nextR;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the next reference r.
|
||||
*
|
||||
* @param nextR Next reference.
|
||||
*/
|
||||
public void setNextR(Matrix<States, N1> nextR) {
|
||||
m_nextR = nextR;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the next reference r.
|
||||
*
|
||||
* @param nextR Next reference.
|
||||
*/
|
||||
public void setNextR(double... nextR) {
|
||||
if (nextR.length != m_nextR.getNumRows()) {
|
||||
throw new MatrixDimensionException(
|
||||
String.format(
|
||||
"The next reference does not have the "
|
||||
+ "correct number of entries! Expected %s, but got %s.",
|
||||
m_nextR.getNumRows(), nextR.length));
|
||||
}
|
||||
m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the controller's calculated control input u plus the calculated feedforward u_ff.
|
||||
*
|
||||
* @return the calculated control input u.
|
||||
*/
|
||||
public Matrix<Inputs, N1> getU() {
|
||||
return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns an element of the controller's calculated control input u.
|
||||
*
|
||||
* @param row Row of u.
|
||||
* @return the calculated control input u at the row i.
|
||||
*/
|
||||
public double getU(int row) {
|
||||
return getU().get(row, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the controller used internally.
|
||||
*
|
||||
* @return the controller used internally.
|
||||
*/
|
||||
public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
|
||||
return m_controller;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the feedforward used internally.
|
||||
*
|
||||
* @return the feedforward used internally.
|
||||
*/
|
||||
public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
|
||||
return m_feedforward;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the observer used internally.
|
||||
*
|
||||
* @return the observer used internally.
|
||||
*/
|
||||
public KalmanFilter<States, Inputs, Outputs> getObserver() {
|
||||
return m_observer;
|
||||
}
|
||||
|
||||
/**
|
||||
* Zeroes reference r and controller output u. The previous reference of the
|
||||
* PlantInversionFeedforward and the initial state estimate of the KalmanFilter are set to the
|
||||
* initial state provided.
|
||||
*
|
||||
* @param initialState The initial state.
|
||||
*/
|
||||
public void reset(Matrix<States, N1> initialState) {
|
||||
m_nextR.fill(0.0);
|
||||
m_controller.reset();
|
||||
m_feedforward.reset(initialState);
|
||||
m_observer.setXhat(initialState);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns difference between reference r and current state x-hat.
|
||||
*
|
||||
* @return The state error matrix.
|
||||
*/
|
||||
public Matrix<States, N1> getError() {
|
||||
return getController().getR().minus(m_observer.getXhat());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns difference between reference r and current state x-hat.
|
||||
*
|
||||
* @param index The index of the error matrix to return.
|
||||
* @return The error at that index.
|
||||
*/
|
||||
public double getError(int index) {
|
||||
return (getController().getR().minus(m_observer.getXhat())).get(index, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the function used to clamp the input u.
|
||||
*
|
||||
* @return The clamping function.
|
||||
*/
|
||||
public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
|
||||
return m_clampFunction;
|
||||
}
|
||||
|
||||
/** Set the clamping function used to clamp inputs. */
|
||||
public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
|
||||
this.m_clampFunction = clampFunction;
|
||||
}
|
||||
|
||||
/**
|
||||
* Correct the state estimate x-hat using the measurements in y.
|
||||
*
|
||||
* @param y Measurement vector.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void correct(Matrix<Outputs, N1> y) {
|
||||
getObserver().correct(getU(), y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets new controller output, projects model forward, and runs observer prediction.
|
||||
*
|
||||
* <p>After calling this, the user should send the elements of u to the actuators.
|
||||
*
|
||||
* @param dtSeconds Timestep for model update.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public void predict(double dtSeconds) {
|
||||
var u =
|
||||
clampInput(
|
||||
m_controller
|
||||
.calculate(getObserver().getXhat(), m_nextR)
|
||||
.plus(m_feedforward.calculate(m_nextR)));
|
||||
getObserver().predict(u, dtSeconds);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clamp the input u to the min and max.
|
||||
*
|
||||
* @param unclampedU The input to clamp.
|
||||
* @return The clamped input.
|
||||
*/
|
||||
public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
|
||||
return m_clampFunction.apply(unclampedU);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,290 @@
|
||||
// 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 edu.wpi.first.math.system;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.numbers.N6;
|
||||
import java.util.function.BiFunction;
|
||||
import java.util.function.DoubleFunction;
|
||||
import java.util.function.Function;
|
||||
|
||||
public final class NumericalIntegration {
|
||||
private NumericalIntegration() {
|
||||
// utility Class
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs Runge Kutta integration (4th order).
|
||||
*
|
||||
* @param f The function to integrate, which takes one argument x.
|
||||
* @param x The initial value of x.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return the integration of dx/dt = f(x) for dt.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
|
||||
final var halfDt = 0.5 * dtSeconds;
|
||||
final var k1 = f.apply(x);
|
||||
final var k2 = f.apply(x + k1 * halfDt);
|
||||
final var k3 = f.apply(x + k2 * halfDt);
|
||||
final var k4 = f.apply(x + k3 * dtSeconds);
|
||||
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs Runge Kutta integration (4th order).
|
||||
*
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return The result of Runge Kutta integration (4th order).
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static double rk4(
|
||||
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
|
||||
final var halfDt = 0.5 * dtSeconds;
|
||||
final var k1 = f.apply(x, u);
|
||||
final var k2 = f.apply(x + k1 * halfDt, u);
|
||||
final var k3 = f.apply(x + k2 * halfDt, u);
|
||||
final var k4 = f.apply(x + k3 * dtSeconds, u);
|
||||
return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
|
||||
*
|
||||
* @param <States> A Num representing the states of the system to integrate.
|
||||
* @param <Inputs> A Num representing the inputs of the system to integrate.
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return the integration of dx/dt = f(x, u) for dt.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double dtSeconds) {
|
||||
|
||||
final var halfDt = 0.5 * dtSeconds;
|
||||
Matrix<States, N1> k1 = f.apply(x, u);
|
||||
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
|
||||
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
|
||||
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
|
||||
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
|
||||
*
|
||||
* @param <States> A Num prepresenting the states of the system.
|
||||
* @param f The function to integrate. It must take one argument x.
|
||||
* @param x The initial value of x.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
|
||||
public static <States extends Num> Matrix<States, N1> rk4(
|
||||
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
|
||||
|
||||
final var halfDt = 0.5 * dtSeconds;
|
||||
Matrix<States, N1> k1 = f.apply(x);
|
||||
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
|
||||
Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
|
||||
Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
|
||||
return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
|
||||
* https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
|
||||
* error is 1e-6.
|
||||
*
|
||||
* @param <States> A Num representing the states of the system to integrate.
|
||||
* @param <Inputs> A Num representing the inputs of the system to integrate.
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return the integration of dx/dt = f(x, u) for dt.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double dtSeconds) {
|
||||
return rkf45(f, x, u, dtSeconds, 1e-6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
|
||||
* https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
|
||||
*
|
||||
* @param <States> A Num representing the states of the system to integrate.
|
||||
* @param <Inputs> A Num representing the inputs of the system to integrate.
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
|
||||
* @return the integration of dx/dt = f(x, u) for dt.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double dtSeconds,
|
||||
double maxError) {
|
||||
|
||||
double dtElapsed = 0;
|
||||
double previousH = dtSeconds;
|
||||
// Loop until we've gotten to our desired dt
|
||||
while (dtElapsed < dtSeconds) {
|
||||
// RKF45 will give us an updated x and a dt back.
|
||||
// We use the new dt (h) as the initial dt for our next loop
|
||||
var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
|
||||
dtElapsed += ret.getSecond();
|
||||
previousH = ret.getSecond();
|
||||
x = ret.getFirst();
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0};
|
||||
static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0};
|
||||
static final Matrix<N6, N5> Bs =
|
||||
Matrix.mat(Nat.N6(), Nat.N5())
|
||||
.fill(
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
2 / 9.0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1 / 12.0,
|
||||
1 / 4.0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
69 / 128.0,
|
||||
-243 / 128.0,
|
||||
135 / 64.0,
|
||||
0,
|
||||
0,
|
||||
-17 / 12.0,
|
||||
27 / 4.0,
|
||||
-27 / 5.0,
|
||||
16 / 15.0,
|
||||
0,
|
||||
65 / 432.0,
|
||||
-5 / 16.0,
|
||||
13 / 16.0,
|
||||
4 / 27.0,
|
||||
5 / 144.0);
|
||||
|
||||
/**
|
||||
* Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error,
|
||||
* and returns a new x and the dt over which that x was updated. This should be called until there
|
||||
* is no dt remaining.
|
||||
*
|
||||
* @param <States> Num representing the states of the system to integrate.
|
||||
* @param <Inputs> A Num representing the inputs of the system to integrate.
|
||||
* @param f The function to integrate. It must take two arguments x and u.
|
||||
* @param x The initial value of x.
|
||||
* @param u The value u held constant over the integration period.
|
||||
* @param initialH The initial dt guess. This is refined to clamp truncation error to the
|
||||
* specified max.
|
||||
* @param maxTruncationError The max truncation error acceptable. Usually a small number like
|
||||
* 1e-6.
|
||||
* @param dtRemaining How much time is left to integrate over. Used to clamp h.
|
||||
* @return the integration of dx/dt = f(x, u) for dt.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
private static <States extends Num, Inputs extends Num>
|
||||
Pair<Matrix<States, N1>, Double> rkf45Impl(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double initialH,
|
||||
double maxTruncationError,
|
||||
double dtRemaining) {
|
||||
|
||||
double truncationErr;
|
||||
double h = initialH;
|
||||
Matrix<States, N1> newX;
|
||||
|
||||
do {
|
||||
// only allow us to advance up to the dt remaining
|
||||
h = Math.min(h, dtRemaining);
|
||||
|
||||
// Notice how the derivative in the Wikipedia notation is dy/dx.
|
||||
// That means their y is our x and their x is our t
|
||||
Matrix<States, N1> k1 = f.apply(x, u).times(h);
|
||||
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h);
|
||||
Matrix<States, N1> k3 =
|
||||
f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h);
|
||||
Matrix<States, N1> k4 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(3, 0)))
|
||||
.plus(k2.times(Bs.get(3, 1)))
|
||||
.plus(k3.times(Bs.get(3, 2))),
|
||||
u)
|
||||
.times(h);
|
||||
Matrix<States, N1> k5 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(4, 0)))
|
||||
.plus(k2.times(Bs.get(4, 1)))
|
||||
.plus(k3.times(Bs.get(4, 2)))
|
||||
.plus(k4.times(Bs.get(4, 3))),
|
||||
u)
|
||||
.times(h);
|
||||
Matrix<States, N1> k6 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(5, 0)))
|
||||
.plus(k2.times(Bs.get(5, 1)))
|
||||
.plus(k3.times(Bs.get(5, 2)))
|
||||
.plus(k4.times(Bs.get(5, 3)))
|
||||
.plus(k5.times(Bs.get(5, 4))),
|
||||
u)
|
||||
.times(h);
|
||||
|
||||
newX =
|
||||
x.plus(k1.times(ch[0]))
|
||||
.plus(k2.times(ch[1]))
|
||||
.plus(k3.times(ch[2]))
|
||||
.plus(k4.times(ch[3]))
|
||||
.plus(k5.times(ch[4]))
|
||||
.plus(k6.times(ch[5]));
|
||||
|
||||
truncationErr =
|
||||
k1.times(ct[0])
|
||||
.plus(k2.times(ct[1]))
|
||||
.plus(k3.times(ct[2]))
|
||||
.plus(k4.times(ct[3]))
|
||||
.plus(k5.times(ct[4]))
|
||||
.plus(k6.times(ct[5]))
|
||||
.normF();
|
||||
|
||||
h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0);
|
||||
} while (truncationErr > maxTruncationError);
|
||||
|
||||
// Return the new x, and the timestep
|
||||
return Pair.of(newX, h);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
// 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 edu.wpi.first.math.system;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import java.util.function.BiFunction;
|
||||
import java.util.function.Function;
|
||||
|
||||
public final class NumericalJacobian {
|
||||
private NumericalJacobian() {
|
||||
// Utility Class.
|
||||
}
|
||||
|
||||
private static final double kEpsilon = 1e-5;
|
||||
|
||||
/**
|
||||
* Computes the numerical Jacobian with respect to x for f(x).
|
||||
*
|
||||
* @param <Rows> Number of rows in the result of f(x).
|
||||
* @param <States> Num representing the number of rows in the output of f.
|
||||
* @param <Cols> Number of columns in the result of f(x).
|
||||
* @param rows Number of rows in the result of f(x).
|
||||
* @param cols Number of columns in the result of f(x).
|
||||
* @param f Vector-valued function from which to compute the Jacobian.
|
||||
* @param x Vector argument.
|
||||
* @return The numerical Jacobian with respect to x for f(x, u, ...).
|
||||
*/
|
||||
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
|
||||
public static <Rows extends Num, Cols extends Num, States extends Num>
|
||||
Matrix<Rows, Cols> numericalJacobian(
|
||||
Nat<Rows> rows,
|
||||
Nat<Cols> cols,
|
||||
Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
|
||||
Matrix<Cols, N1> x) {
|
||||
var result = new Matrix<>(rows, cols);
|
||||
|
||||
for (int i = 0; i < cols.getNum(); i++) {
|
||||
var dxPlus = x.copy();
|
||||
var dxMinus = x.copy();
|
||||
dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
|
||||
dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
|
||||
|
||||
result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns numerical Jacobian with respect to x for f(x, u, ...).
|
||||
*
|
||||
* @param <Rows> Number of rows in the result of f(x, u).
|
||||
* @param <States> Number of rows in x.
|
||||
* @param <Inputs> Number of rows in the second input to f.
|
||||
* @param <Outputs> Num representing the rows in the output of f.
|
||||
* @param rows Number of rows in the result of f(x, u).
|
||||
* @param states Number of rows in x.
|
||||
* @param f Vector-valued function from which to compute Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
* @return The numerical Jacobian with respect to x for f(x, u, ...).
|
||||
*/
|
||||
@SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
|
||||
public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
|
||||
Matrix<Rows, States> numericalJacobianX(
|
||||
Nat<Rows> rows,
|
||||
Nat<States> states,
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u) {
|
||||
return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the numerical Jacobian with respect to u for f(x, u).
|
||||
*
|
||||
* @param <States> The states of the system.
|
||||
* @param <Inputs> The inputs to the system.
|
||||
* @param <Rows> Number of rows in the result of f(x, u).
|
||||
* @param rows Number of rows in the result of f(x, u).
|
||||
* @param inputs Number of rows in u.
|
||||
* @param f Vector-valued function from which to compute the Jacobian.
|
||||
* @param x State vector.
|
||||
* @param u Input vector.
|
||||
* @return the numerical Jacobian with respect to u for f(x, u).
|
||||
*/
|
||||
@SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
|
||||
public static <Rows extends Num, States extends Num, Inputs extends Num>
|
||||
Matrix<Rows, Inputs> numericalJacobianU(
|
||||
Nat<Rows> rows,
|
||||
Nat<Inputs> inputs,
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u) {
|
||||
return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,194 @@
|
||||
// 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 edu.wpi.first.math.system.plant;
|
||||
|
||||
import edu.wpi.first.math.util.Units;
|
||||
|
||||
/** Holds the constants for a DC motor. */
|
||||
public class DCMotor {
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double nominalVoltageVolts;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double stallTorqueNewtonMeters;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double stallCurrentAmps;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double freeCurrentAmps;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double freeSpeedRadPerSec;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double rOhms;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double KvRadPerSecPerVolt;
|
||||
|
||||
@SuppressWarnings("MemberName")
|
||||
public final double KtNMPerAmp;
|
||||
|
||||
/**
|
||||
* Constructs a DC motor.
|
||||
*
|
||||
* @param nominalVoltageVolts Voltage at which the motor constants were measured.
|
||||
* @param stallTorqueNewtonMeters Current draw when stalled.
|
||||
* @param stallCurrentAmps Current draw when stalled.
|
||||
* @param freeCurrentAmps Current draw under no load.
|
||||
* @param freeSpeedRadPerSec Angular velocity under no load.
|
||||
* @param numMotors Number of motors in a gearbox.
|
||||
*/
|
||||
public DCMotor(
|
||||
double nominalVoltageVolts,
|
||||
double stallTorqueNewtonMeters,
|
||||
double stallCurrentAmps,
|
||||
double freeCurrentAmps,
|
||||
double freeSpeedRadPerSec,
|
||||
int numMotors) {
|
||||
this.nominalVoltageVolts = nominalVoltageVolts;
|
||||
this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
|
||||
this.stallCurrentAmps = stallCurrentAmps * numMotors;
|
||||
this.freeCurrentAmps = freeCurrentAmps * numMotors;
|
||||
this.freeSpeedRadPerSec = freeSpeedRadPerSec;
|
||||
|
||||
this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
|
||||
this.KvRadPerSecPerVolt =
|
||||
freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
|
||||
this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
|
||||
}
|
||||
|
||||
/**
|
||||
* Estimate the current being drawn by this motor.
|
||||
*
|
||||
* @param speedRadiansPerSec The speed of the rotor.
|
||||
* @param voltageInputVolts The input voltage.
|
||||
*/
|
||||
public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
|
||||
return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of CIM motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getCIM(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of 775Pro motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getVex775Pro(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of NEO motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getNEO(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of MiniCIM motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getMiniCIM(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Bag motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getBag(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Andymark RS775-125 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getAndymarkRs775_125(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Banebots RS775 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getBanebotsRs775(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Andymark 9015 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getAndymark9015(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Banebots RS 550 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getBanebotsRs550(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Neo 550 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getNeo550(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Falcon 500 motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getFalcon500(int numMotors) {
|
||||
return new DCMotor(
|
||||
12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a gearbox of Romi/TI_RSLK MAX motors.
|
||||
*
|
||||
* @param numMotors Number of motors in the gearbox.
|
||||
*/
|
||||
public static DCMotor getRomiBuiltIn(int numMotors) {
|
||||
// From https://www.pololu.com/product/1520/specs
|
||||
return new DCMotor(
|
||||
4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,211 @@
|
||||
// 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 edu.wpi.first.math.system.plant;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N2;
|
||||
import edu.wpi.first.math.system.LinearSystem;
|
||||
|
||||
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]^T, inputs are [voltage], and outputs are [position].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param massKg The mass of the elevator carriage, in kilograms.
|
||||
* @param radiusMeters The radius of thd driving drum of the elevator, in meters.
|
||||
* @param G The reduction between motor and drum, as a ratio of output to input.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static LinearSystem<N2, N1, N1> createElevatorSystem(
|
||||
DCMotor motor, double massKg, double radiusMeters, double G) {
|
||||
return new LinearSystem<>(
|
||||
Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(
|
||||
0,
|
||||
1,
|
||||
0,
|
||||
-Math.pow(G, 2)
|
||||
* motor.KtNMPerAmp
|
||||
/ (motor.rOhms
|
||||
* radiusMeters
|
||||
* radiusMeters
|
||||
* massKg
|
||||
* motor.KvRadPerSecPerVolt)),
|
||||
VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
|
||||
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
|
||||
new Matrix<>(Nat.N1(), 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 arm.
|
||||
* @param jKgMetersSquared The moment of inertia J of the flywheel.
|
||||
* @param G The reduction between motor and drum, as a ratio of output to input.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
|
||||
DCMotor motor, double jKgMetersSquared, double G) {
|
||||
return new LinearSystem<>(
|
||||
VecBuilder.fill(
|
||||
-G
|
||||
* G
|
||||
* motor.KtNMPerAmp
|
||||
/ (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
|
||||
VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
|
||||
Matrix.eye(Nat.N1()),
|
||||
new Matrix<>(Nat.N1(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a differential drive drivetrain. In this model, the states are
|
||||
* [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are [v_left, v_right]^T.
|
||||
*
|
||||
* @param motor the gearbox representing the motors driving the drivetrain.
|
||||
* @param massKg the mass of the robot.
|
||||
* @param rMeters the radius of the wheels in meters.
|
||||
* @param rbMeters the radius of the base (half the track width) in meters.
|
||||
* @param JKgMetersSquared the moment of inertia of the robot.
|
||||
* @param G the gearing reduction as output over input.
|
||||
* @return A LinearSystem representing a differential drivetrain.
|
||||
*/
|
||||
@SuppressWarnings({"LocalVariableName", "ParameterName"})
|
||||
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
|
||||
DCMotor motor,
|
||||
double massKg,
|
||||
double rMeters,
|
||||
double rbMeters,
|
||||
double JKgMetersSquared,
|
||||
double G) {
|
||||
var C1 =
|
||||
-(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
|
||||
var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
|
||||
|
||||
final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
|
||||
final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
|
||||
var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1);
|
||||
var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2);
|
||||
var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
|
||||
var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(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].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param jKgSquaredMeters The moment of inertia J of the arm.
|
||||
* @param G 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.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
|
||||
DCMotor motor, double jKgSquaredMeters, double G) {
|
||||
return new LinearSystem<>(
|
||||
Matrix.mat(Nat.N2(), Nat.N2())
|
||||
.fill(
|
||||
0,
|
||||
1,
|
||||
0,
|
||||
-Math.pow(G, 2)
|
||||
* motor.KtNMPerAmp
|
||||
/ (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)),
|
||||
VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)),
|
||||
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
|
||||
new Matrix<>(Nat.N1(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
|
||||
* constants cam be found using frc-characterization. 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 edu.wpi.first.math.util.Units} class for converting between unit types.
|
||||
*
|
||||
* @param kV The velocity gain, in volts per (units per second)
|
||||
* @param kA The acceleration gain, in volts per (units 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<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
|
||||
return new LinearSystem<>(
|
||||
VecBuilder.fill(-kV / kA),
|
||||
VecBuilder.fill(1.0 / kA),
|
||||
VecBuilder.fill(1.0),
|
||||
VecBuilder.fill(0.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
|
||||
* constants cam be found using frc-characterization. The states of the system are [position,
|
||||
* velocity]^T, 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 edu.wpi.first.math.util.Units} class for converting between unit types.
|
||||
*
|
||||
* @param kV The velocity gain, in volts per (units per second)
|
||||
* @param kA The acceleration gain, in volts per (units 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, N1, N1> identifyPositionSystem(double kV, double kA) {
|
||||
return new LinearSystem<>(
|
||||
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
|
||||
VecBuilder.fill(0.0, 1.0 / kA),
|
||||
Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
|
||||
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/(radian/sec) and
|
||||
* volts/(radian/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 (radians per second).
|
||||
* @param kAAngular The angular acceleration gain, volts per (radians 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 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));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user