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

[build] Apply spotless for java formatting (#1768)

Browse Source 
Update checkstyle config to be compatible with spotless.

Co-authored-by: Austin Shalit <austinshalit@gmail.com>
This commit is contained in:
Peter Johnson
2020-12-29 22:45:16 -08:00
committed by GitHub
parent e563a0b7db
commit a751fa22d2
883 changed files with 16526 additions and 17751 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
wpimath/src/main/java/edu/wpi/first/math/Drake.java
View File

@@ -4,14 +4,12 @@
package edu.wpi.first.math;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import org.ejml.simple.SimpleMatrix;
public final class Drake {
private Drake() {
}
private Drake() {}
/**
* Solves the discrete alegebraic Riccati equation.
@@ -24,14 +22,16 @@ public final class Drake {
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A,
SimpleMatrix B,
SimpleMatrix Q,
SimpleMatrix R) {
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
var S = new SimpleMatrix(A.numRows(), A.numCols());
WPIMathJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
S.getDDRM().getData());
WPIMathJNI.discreteAlgebraicRiccatiEquation(
A.getDDRM().getData(),
B.getDDRM().getData(),
Q.getDDRM().getData(),
R.getDDRM().getData(),
A.numCols(),
B.numCols(),
S.getDDRM().getData());
return S;
}
@@ -45,12 +45,14 @@ public final class Drake {
* @return Solution of DARE.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num> Matrix<States, States>
discreteAlgebraicRiccatiEquation(Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
return new Matrix<>(discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(),
Q.getStorage(), R.getStorage()));
public static <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
return new Matrix<>(
discreteAlgebraicRiccatiEquation(
A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
}
}

26
wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
View File

@@ -7,30 +7,24 @@ package edu.wpi.first.math;
public final class MathSharedStore {
private static MathShared mathShared;
private MathSharedStore() {
}
private MathSharedStore() {}
/**
* get the MathShared object.
*/
/** get the MathShared object. */
public static synchronized MathShared getMathShared() {
if (mathShared == null) {
mathShared = new MathShared() {
@Override
public void reportError(String error, StackTraceElement[] stackTrace) {
}
mathShared =
new MathShared() {
@Override
public void reportError(String error, StackTraceElement[] stackTrace) {}
@Override
public void reportUsage(MathUsageId id, int count) {
}
};
@Override
public void reportUsage(MathUsageId id, int count) {}
};
}
return mathShared;
}
/**
* set the MathShared object.
*/
/** set the MathShared object. */
public static synchronized void setMathShared(MathShared shared) {
mathShared = shared;
}

51
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -4,11 +4,10 @@
package edu.wpi.first.math;
import edu.wpi.first.wpiutil.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
import edu.wpi.first.wpiutil.RuntimeLoader;
public final class WPIMathJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<WPIMathJNI> loader = null;
@@ -16,8 +15,9 @@ public final class WPIMathJNI {
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
WPIMathJNI.class);
loader =
new RuntimeLoader<>(
"wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
loader.loadLibrary();
} catch (IOException ex) {
ex.printStackTrace();
@@ -36,8 +36,9 @@ public final class WPIMathJNI {
if (libraryLoaded) {
return;
}
loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
WPIMathJNI.class);
loader =
new RuntimeLoader<>(
"wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
loader.loadLibrary();
libraryLoaded = true;
}
@@ -45,53 +46,47 @@ public final class WPIMathJNI {
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
* @param Q Array containing elements of Q in row-major order.
* @param R Array containing elements of R in row-major order.
* @param A Array containing elements of A in row-major order.
* @param B Array containing elements of B in row-major order.
* @param Q Array containing elements of Q in row-major order.
* @param R Array containing elements of R in row-major order.
* @param states Number of states in A matrix.
* @param inputs Number of inputs in B matrix.
* @param S Array storage for DARE solution.
* @param S Array storage for DARE solution.
*/
public static native void discreteAlgebraicRiccatiEquation(
double[] A,
double[] B,
double[] Q,
double[] R,
int states,
int inputs,
double[] S);
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Computes the matrix exp.
*
* @param src Array of elements of the matrix to be exponentiated.
* @param src Array of elements of the matrix to be exponentiated.
* @param rows How many rows there are.
* @param dst Array where the result will be stored.
* @param dst Array where the result will be stored.
*/
public static native void exp(double[] src, int rows, double[] dst);
/**
* Computes the matrix pow.
*
* @param src Array of elements of the matrix to be raised to a power.
* @param rows How many rows there are.
* @param src Array of elements of the matrix to be raised to a power.
* @param rows How many rows there are.
* @param exponent The exponent.
* @param dst Array where the result will be stored.
* @param dst Array where the result will be stored.
*/
public static native void pow(double[] src, int rows, double exponent, double[] dst);
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank(lambda * I - A, B) &lt; n where n is number of states.
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
* &lt; n where n is number of states.
*
* @param states the number of states of the system.
* @param inputs the number of inputs to the system.
* @param A System matrix.
* @param B Input matrix.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);

22
wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
View File

@@ -4,11 +4,10 @@
package edu.wpi.first.wpilibj;
import java.util.Arrays;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.wpiutil.CircularBuffer;
import java.util.Arrays;
/**
* This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
@@ -25,8 +24,8 @@ import edu.wpi.first.wpiutil.CircularBuffer;
* <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
* undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
* noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
* impact of these high frequency components. Likewise, a "high pass" filter gets rid of
* slow-moving signal components, letting you detect large changes more easily.
* impact of these high frequency components. Likewise, a "high pass" filter gets rid of slow-moving
* signal components, letting you detect large changes more easily.
*
* <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
* the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
@@ -78,10 +77,9 @@ public class LinearFilter {
* <p>This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
* @param period The period in seconds between samples taken by the user.
* @param period The period in seconds between samples taken by the user.
*/
public static LinearFilter singlePoleIIR(double timeConstant,
double period) {
public static LinearFilter singlePoleIIR(double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {1.0 - gain};
double[] fbGains = {-gain};
@@ -96,10 +94,9 @@ public class LinearFilter {
* <p>This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
* @param period The period in seconds between samples taken by the user.
* @param period The period in seconds between samples taken by the user.
*/
public static LinearFilter highPass(double timeConstant,
double period) {
public static LinearFilter highPass(double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {gain, -gain};
double[] fbGains = {-gain};
@@ -131,9 +128,7 @@ public class LinearFilter {
return new LinearFilter(ffGains, fbGains);
}
/**
* Reset the filter state.
*/
/** Reset the filter state. */
public void reset() {
m_inputs.clear();
m_outputs.clear();
@@ -143,7 +138,6 @@ public class LinearFilter {
* Calculates the next value of the filter.
*
* @param input Current input value.
*
* @return The filtered value at this step
*/
public double calculate(double input) {

13
wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
View File

@@ -4,16 +4,15 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpiutil.CircularBuffer;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import edu.wpi.first.wpiutil.CircularBuffer;
/**
* A class that implements a moving-window median filter. Useful for reducing measurement noise,
* especially with processes that generate occasional, extreme outliers (such as values from
* vision processing, LIDAR, or ultrasonic sensors).
* A class that implements a moving-window median filter. Useful for reducing measurement noise,
* especially with processes that generate occasional, extreme outliers (such as values from vision
* processing, LIDAR, or ultrasonic sensors).
*/
public class MedianFilter {
private final CircularBuffer m_valueBuffer;
@@ -73,9 +72,7 @@ public class MedianFilter {
}
}
/**
* Resets the filter, clearing the window of all elements.
*/
/** Resets the filter, clearing the window of all elements. */
public void reset() {
m_orderedValues.clear();
m_valueBuffer.clear();

75
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
View File

@@ -5,8 +5,8 @@
package edu.wpi.first.wpilibj.controller;
/**
* A helper class that computes feedforward outputs for a simple arm (modeled as a motor
* acting against the force of gravity on a beam suspended at an angle).
* A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
* against the force of gravity on a beam suspended at an angle).
*/
@SuppressWarnings("MemberName")
public class ArmFeedforward {
@@ -16,13 +16,13 @@ public class ArmFeedforward {
public final double ka;
/**
* Creates a new ArmFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
* Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
* units of the computed feedforward.
*
* @param ks The static gain.
* @param ks The static gain.
* @param kcos The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
*/
public ArmFeedforward(double ks, double kcos, double kv, double ka) {
this.ks = ks;
@@ -32,12 +32,12 @@ public class ArmFeedforward {
}
/**
* Creates a new ArmFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
* Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
* Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param ks The static gain.
* @param kcos The gravity gain.
* @param kv The velocity gain.
* @param kv The velocity gain.
*/
public ArmFeedforward(double ks, double kcos, double kv) {
this(ks, kcos, kv, 0);
@@ -46,24 +46,25 @@ public class ArmFeedforward {
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param positionRadians The position (angle) setpoint.
* @param velocityRadPerSec The velocity setpoint.
* @param positionRadians The position (angle) setpoint.
* @param velocityRadPerSec The velocity setpoint.
* @param accelRadPerSecSquared The acceleration setpoint.
* @return The computed feedforward.
*/
public double calculate(double positionRadians, double velocityRadPerSec,
double accelRadPerSecSquared) {
return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
public double calculate(
double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
return ks * Math.signum(velocityRadPerSec)
+ kcos * Math.cos(positionRadians)
+ kv * velocityRadPerSec
+ ka * accelRadPerSecSquared;
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param positionRadians The position (angle) setpoint.
* @param velocity The velocity setpoint.
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double positionRadians, double velocity) {
@@ -74,11 +75,10 @@ public class ArmFeedforward {
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an
* acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
* profile are simultaneously achievable - enter the acceleration constraint, and this will give
* you a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm.
@@ -91,11 +91,10 @@ public class ArmFeedforward {
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an
* acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
* profile are simultaneously achievable - enter the acceleration constraint, and this will give
* you a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm.
@@ -108,11 +107,10 @@ public class ArmFeedforward {
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and
* a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
* profile are simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm.
@@ -124,11 +122,10 @@ public class ArmFeedforward {
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and
* a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
* profile are simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm.

107
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java
View File

@@ -4,45 +4,38 @@
package edu.wpi.first.wpilibj.controller;
import java.util.function.BiFunction;
import java.util.function.Function;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.Function;
/**
* Constructs a control-affine plant inversion model-based feedforward from
* given model dynamics.
* Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
*
* <p>If given the vector valued function as f(x, u) where x is the state
* vector and u is the input vector, the B matrix(continuous input matrix)
* is calculated through a {@link edu.wpi.first.wpilibj.system.NumericalJacobian}.
* In this case f has to be control-affine (of the form f(x) + Bu).
* <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
* vector, the B matrix(continuous input matrix) is calculated through a {@link
* edu.wpi.first.wpilibj.system.NumericalJacobian}. In this case f has to be control-affine (of the
* form f(x) + Bu).
*
* <p>The feedforward is calculated as
* <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
* <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
* <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
*
* <p>This feedforward does not account for a dynamic B matrix, B is either
* determined or supplied when the feedforward is created and remains constant.
* <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
* when the feedforward is created and remains constant.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
/**
* The current reference state.
*/
/** The current reference state. */
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/**
* The computed feedforward.
*/
/** The computed feedforward. */
private Matrix<Inputs, N1> m_uff;
@SuppressWarnings("MemberName")
@@ -52,34 +45,30 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
private final double m_dt;
/**
* The model dynamics.
*/
/** The model dynamics. */
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
/**
* Constructs a feedforward with given model dynamics as a function
* of state and input.
* Constructs a feedforward with given model dynamics as a function of state and input.
*
* @param states A {@link Nat} representing the number of states.
* @param inputs A {@link Nat} representing the number of inputs.
* @param f A vector-valued function of x, the state, and
* u, the input, that returns the derivative of
* the state vector. HAS to be control-affine
* (of the form f(x) + Bu).
* @param states A {@link Nat} representing the number of states.
* @param inputs A {@link Nat} representing the number of inputs.
* @param f A vector-valued function of x, the state, and u, the input, that returns the
* derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu).
* @param dtSeconds The timestep between calls of calculate().
*/
public ControlAffinePlantInversionFeedforward(
Nat<States> states,
Nat<Inputs> inputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds) {
Nat<States> states,
Nat<Inputs> inputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds) {
this.m_dt = dtSeconds;
this.m_f = f;
this.m_inputs = inputs;
this.m_B = NumericalJacobian.numericalJacobianU(states, inputs,
m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
this.m_B =
NumericalJacobian.numericalJacobianU(
states, inputs, m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
m_r = new Matrix<>(states, Nat.N1());
m_uff = new Matrix<>(inputs, Nat.N1());
@@ -88,22 +77,22 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
}
/**
* Constructs a feedforward with given model dynamics as a function of state,
* and the plant's B(continuous input matrix) matrix.
* Constructs a feedforward with given model dynamics as a function of state, and the plant's
* B(continuous input matrix) matrix.
*
* @param states A {@link Nat} representing the number of states.
* @param inputs A {@link Nat} representing the number of inputs.
* @param f A vector-valued function of x, the state,
* that returns the derivative of the state vector.
* @param B Continuous input matrix of the plant being controlled.
* @param states A {@link Nat} representing the number of states.
* @param inputs A {@link Nat} representing the number of inputs.
* @param f A vector-valued function of x, the state, that returns the derivative of the state
* vector.
* @param B Continuous input matrix of the plant being controlled.
* @param dtSeconds The timestep between calls of calculate().
*/
public ControlAffinePlantInversionFeedforward(
Nat<States> states,
Nat<Inputs> inputs,
Function<Matrix<States, N1>, Matrix<States, N1>> f,
Matrix<States, Inputs> B,
double dtSeconds) {
Nat<States> states,
Nat<Inputs> inputs,
Function<Matrix<States, N1>, Matrix<States, N1>> f,
Matrix<States, Inputs> B,
double dtSeconds) {
this.m_dt = dtSeconds;
this.m_inputs = inputs;
@@ -116,7 +105,6 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
reset(m_r);
}
/**
* Returns the previously calculated feedforward as an input vector.
*
@@ -130,7 +118,6 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
* Returns an element of the previously calculated feedforward.
*
* @param row Row of uff.
*
* @return The row of the calculated feedforward.
*/
public double getUff(int row) {
@@ -150,7 +137,6 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
* Returns an element of the current reference vector r.
*
* @param row Row of r.
*
* @return The row of the current reference vector.
*/
public double getR(int row) {
@@ -167,25 +153,21 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
m_uff.fill(0.0);
}
/**
* Resets the feedforward with a zero initial state vector.
*/
/** Resets the feedforward with a zero initial state vector. */
public void reset() {
m_r.fill(0.0);
m_uff.fill(0.0);
}
/**
* Calculate the feedforward with only the desired
* future reference. This uses the internally stored "current"
* reference.
* Calculate the feedforward with only the desired future reference. This uses the internally
* stored "current" reference.
*
* <p>If this method is used the initial state of the system is the one
* set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
* If the initial state is not set it defaults to a zero vector.
* <p>If this method is used the initial state of the system is the one set using {@link
* LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
* a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
@@ -195,9 +177,8 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
/**
* Calculate the feedforward with current and future reference vectors.
*
* @param r The reference state of the current timestep (k).
* @param r The reference state of the current timestep (k).
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})

54
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
View File

@@ -5,8 +5,8 @@
package edu.wpi.first.wpilibj.controller;
/**
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
* acting against the force of gravity).
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
* against the force of gravity).
*/
@SuppressWarnings("MemberName")
public class ElevatorFeedforward {
@@ -16,8 +16,8 @@ public class ElevatorFeedforward {
public final double ka;
/**
* Creates a new ElevatorFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
* Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
* dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kg The gravity gain.
@@ -32,8 +32,8 @@ public class ElevatorFeedforward {
}
/**
* Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
* Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
* zero. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kg The gravity gain.
@@ -46,7 +46,7 @@ public class ElevatorFeedforward {
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint.
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
*/
@@ -55,8 +55,8 @@ public class ElevatorFeedforward {
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
@@ -69,11 +69,10 @@ public class ElevatorFeedforward {
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the acceleration constraint, and this will give you a
* simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param acceleration The acceleration of the elevator.
@@ -85,11 +84,10 @@ public class ElevatorFeedforward {
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the acceleration constraint, and this will give you a
* simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param acceleration The acceleration of the elevator.
@@ -101,11 +99,10 @@ public class ElevatorFeedforward {
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param velocity The velocity of the elevator.
@@ -116,11 +113,10 @@ public class ElevatorFeedforward {
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the elevator.
* @param velocity The velocity of the elevator.

50
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java
View File

@@ -4,13 +4,12 @@
package edu.wpi.first.wpilibj.controller;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
@@ -22,17 +21,13 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
public class LinearPlantInversionFeedforward<States extends Num, Inputs extends Num,
Outputs extends Num> {
/**
* The current reference state.
*/
public class LinearPlantInversionFeedforward<
States extends Num, Inputs extends Num, Outputs extends Num> {
/** The current reference state. */
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/**
* The computed feedforward.
*/
/** The computed feedforward. */
private Matrix<Inputs, N1> m_uff;
@SuppressWarnings("MemberName")
@@ -44,26 +39,24 @@ public class LinearPlantInversionFeedforward<States extends Num, Inputs extends
/**
* Constructs a feedforward with the given plant.
*
* @param plant The plant being controlled.
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep.
*/
public LinearPlantInversionFeedforward(
LinearSystem<States, Inputs, Outputs> plant,
double dtSeconds
) {
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
this(plant.getA(), plant.getB(), dtSeconds);
}
/**
* Constructs a feedforward with the given coefficients.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearPlantInversionFeedforward(Matrix<States, States> A, Matrix<States, Inputs> B,
double dtSeconds) {
public LinearPlantInversionFeedforward(
Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
this.m_A = discABPair.getFirst();
this.m_B = discABPair.getSecond();
@@ -87,7 +80,6 @@ public class LinearPlantInversionFeedforward<States extends Num, Inputs extends
* Returns an element of the previously calculated feedforward.
*
* @param row Row of uff.
*
* @return The row of the calculated feedforward.
*/
public double getUff(int row) {
@@ -107,7 +99,6 @@ public class LinearPlantInversionFeedforward<States extends Num, Inputs extends
* Returns an element of the current reference vector r.
*
* @param row Row of r.
*
* @return The row of the current reference vector.
*/
public double getR(int row) {
@@ -124,25 +115,21 @@ public class LinearPlantInversionFeedforward<States extends Num, Inputs extends
m_uff.fill(0.0);
}
/**
* Resets the feedforward with a zero initial state vector.
*/
/** Resets the feedforward with a zero initial state vector. */
public void reset() {
m_r.fill(0.0);
m_uff.fill(0.0);
}
/**
* Calculate the feedforward with only the desired
* future reference. This uses the internally stored "current"
* reference.
* Calculate the feedforward with only the desired future reference. This uses the internally
* stored "current" reference.
*
* <p>If this method is used the initial state of the system is the one
* set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
* If the initial state is not set it defaults to a zero vector.
* <p>If this method is used the initial state of the system is the one set using {@link
* LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
* a zero vector.
*
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
@@ -152,9 +139,8 @@ public class LinearPlantInversionFeedforward<States extends Num, Inputs extends
/**
* Calculate the feedforward with current and future reference vectors.
*
* @param r The reference state of the current timestep (k).
* @param r The reference state of the current timestep (k).
* @param nextR The reference state of the future timestep (k + dt).
*
* @return The calculated feedforward.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})

135
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.controller;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.Drake;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
@@ -15,28 +13,22 @@ import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.Vector;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* Contains the controller coefficients and logic for a linear-quadratic
* regulator (LQR).
* LQRs use the control law u = K(r - x).
* Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
* the control law u = K(r - x).
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*/
@SuppressWarnings("ClassTypeParameterName")
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
Outputs extends Num> {
/**
* The current reference state.
*/
public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
/** The current reference state. */
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_r;
/**
* The computed and capped controller output.
*/
/** The computed and capped controller output. */
@SuppressWarnings("MemberName")
private Matrix<Inputs, N1> m_u;
@@ -47,52 +39,64 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
/**
* Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
*
* @param plant The plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param plant The plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
*/
public LinearQuadraticRegulator(
LinearSystem<States, Inputs, Outputs> plant,
Vector<States> qelms,
Vector<Inputs> relms,
double dtSeconds
) {
this(plant.getA(), plant.getB(), StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms), dtSeconds);
}
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
Vector<States> qelms, Vector<Inputs> relms,
double dtSeconds
) {
this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms),
LinearSystem<States, Inputs, Outputs> plant,
Vector<States> qelms,
Vector<Inputs> relms,
double dtSeconds) {
this(
plant.getA(),
plant.getB(),
StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms),
dtSeconds);
}
/**
* Constructs a controller with the given coefficients and plant.
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
Matrix<States, States> Q, Matrix<Inputs, Inputs> R,
double dtSeconds
) {
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Vector<States> qelms,
Vector<Inputs> relms,
double dtSeconds) {
this(
A,
B,
StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms),
dtSeconds);
}
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
@@ -118,9 +122,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
*/
@SuppressWarnings("ParameterName")
public LinearQuadraticRegulator(
Nat<States> states, Nat<Inputs> inputs,
Matrix<Inputs, States> k
) {
Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs, States> k) {
m_K = k;
m_r = new Matrix<>(states, Nat.N1());
@@ -142,7 +144,6 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
* Returns an element of the control input vector u.
*
* @param row Row of u.
*
* @return The row of the control input vector.
*/
public double getU(int row) {
@@ -162,7 +163,6 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
* Returns an element of the reference vector r.
*
* @param row Row of r.
*
* @return The row of the reference vector.
*/
public double getR(int row) {
@@ -178,9 +178,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
return m_K;
}
/**
* Resets the controller.
*/
/** Resets the controller. */
public void reset() {
m_r.fill(0.0);
m_u.fill(0.0);
@@ -200,7 +198,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
/**
* Returns the next output of the controller.
*
* @param x The current state x.
* @param x The current state x.
* @param nextR the next reference vector r.
*/
@SuppressWarnings("ParameterName")
@@ -210,24 +208,21 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
}
/**
* Adjusts LQR controller gain to compensate for a pure time delay in the
* input.
* Adjusts LQR controller gain to compensate for a pure time delay in the input.
*
* <p>Linear-Quadratic regulator controller gains tend to be aggressive. If
* sensor measurements are time-delayed too long, the LQR may be unstable.
* However, if we know the amount of delay, we can compute the control based
* on where the system will be after the time delay.
* <p>Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements
* are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
* can compute the control based on where the system will be after the time delay.
*
* <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
* appendix C.4 for a derivation.
* <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
* derivation.
*
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep in seconds.
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep in seconds.
* @param inputDelaySeconds Input time delay in seconds.
*/
public void latencyCompensate(
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds,
double inputDelaySeconds) {
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds, double inputDelaySeconds) {
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();

54
wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
View File

@@ -4,9 +4,7 @@
package edu.wpi.first.wpilibj.controller;
/**
* A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
*/
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
@SuppressWarnings("MemberName")
public class SimpleMotorFeedforward {
public final double ks;
@@ -14,8 +12,8 @@ public class SimpleMotorFeedforward {
public final double ka;
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
* Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
* dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
@@ -28,8 +26,8 @@ public class SimpleMotorFeedforward {
}
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
* to zero. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
@@ -41,7 +39,7 @@ public class SimpleMotorFeedforward {
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint.
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
*/
@@ -53,8 +51,8 @@ public class SimpleMotorFeedforward {
// formulas for the methods below:
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
@@ -64,11 +62,10 @@ public class SimpleMotorFeedforward {
}
/**
* Calculates the maximum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the acceleration constraint, and this will give you a
* simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param acceleration The acceleration of the motor.
@@ -80,11 +77,10 @@ public class SimpleMotorFeedforward {
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply
* and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
* Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the acceleration constraint, and this will give you a
* simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param acceleration The acceleration of the motor.
@@ -96,11 +92,10 @@ public class SimpleMotorFeedforward {
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param velocity The velocity of the motor.
@@ -111,11 +106,10 @@ public class SimpleMotorFeedforward {
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
* Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
* simultaneously achievable - enter the velocity constraint, and this will give you a
* simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor.
* @param velocity The velocity of the motor.

31
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java
View File

@@ -4,14 +4,11 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiFunction;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import org.ejml.simple.SimpleMatrix;
public final class AngleStatistics {
private AngleStatistics() {
@@ -26,8 +23,8 @@ public final class AngleStatistics {
* @param b A vector to subtract with.
* @param angleStateIdx The row containing angles to be normalized.
*/
public static <S extends Num> Matrix<S, N1> angleResidual(Matrix<S, N1> a, Matrix<S, N1> b,
int angleStateIdx) {
public static <S extends Num> Matrix<S, N1> angleResidual(
Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
Matrix<S, N1> ret = a.minus(b);
ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
@@ -40,8 +37,8 @@ public final class AngleStatistics {
*
* @param angleStateIdx The row containing angles to be normalized.
*/
public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>>
angleResidual(int angleStateIdx) {
public static <S extends Num>
BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleResidual(int angleStateIdx) {
return (a, b) -> angleResidual(a, b, angleStateIdx);
}
@@ -52,8 +49,8 @@ public final class AngleStatistics {
* @param b A vector to add with.
* @param angleStateIdx The row containing angles to be normalized.
*/
public static <S extends Num> Matrix<S, N1> angleAdd(Matrix<S, N1> a, Matrix<S, N1> b,
int angleStateIdx) {
public static <S extends Num> Matrix<S, N1> angleAdd(
Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
Matrix<S, N1> ret = a.plus(b);
ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
@@ -66,8 +63,8 @@ public final class AngleStatistics {
*
* @param angleStateIdx The row containing angles to be normalized.
*/
public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>>
angleAdd(int angleStateIdx) {
public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleAdd(
int angleStateIdx) {
return (a, b) -> angleAdd(a, b, angleStateIdx);
}
@@ -90,8 +87,8 @@ public final class AngleStatistics {
* @param angleStateIdx The row containing the angles.
*/
@SuppressWarnings("checkstyle:ParameterName")
public static <S extends Num> Matrix<S, N1> angleMean(Matrix<S, ?> sigmas, Matrix<?, N1> Wm,
int angleStateIdx) {
public static <S extends Num> Matrix<S, N1> angleMean(
Matrix<S, ?> sigmas, Matrix<?, N1> Wm, int angleStateIdx) {
double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData();
Matrix<N1, ?> sinAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
Matrix<N1, ?> cosAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
@@ -116,8 +113,8 @@ public final class AngleStatistics {
* @param angleStateIdx The row containing the angles.
*/
@SuppressWarnings("LambdaParameterName")
public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>>
angleMean(int angleStateIdx) {
public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>> angleMean(
int angleStateIdx) {
return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx);
}
}

356
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiConsumer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
@@ -19,40 +17,36 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N3;
import edu.wpi.first.wpiutil.math.numbers.N5;
import java.util.function.BiConsumer;
/**
* This class wraps an
* {@link edu.wpi.first.wpilibj.estimator.UnscentedKalmanFilter Unscented Kalman Filter}
* to fuse latency-compensated vision
* measurements with differential drive encoder measurements. It will correct
* for noisy vision measurements and encoder drift. It is intended to be an easy
* drop-in for
* {@link edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry}; in fact,
* if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
* and only call {@link DifferentialDrivePoseEstimator#update} then this will
* behave exactly the same as DifferentialDriveOdometry.
* This class wraps an {@link edu.wpi.first.wpilibj.estimator.UnscentedKalmanFilter Unscented Kalman
* Filter} to fuse latency-compensated vision measurements with differential drive encoder
* measurements. It will correct for noisy vision measurements and encoder drift. It is intended to
* be an easy drop-in for {@link edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry}; in
* fact, if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call
* {@link DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
* DifferentialDriveOdometry.
*
* <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot
* loop (if your robot loops are faster than the default then you should change
* the {@link DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d,
* Matrix, Matrix, Matrix, double) nominal delta time}.)
* {@link DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as
* infrequently as you want; if you never call it then this class will behave
* <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
* loops are faster than the default then you should change the {@link
* DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
* Matrix, double) nominal delta time}.) {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
* can be called as infrequently as you want; if you never call it then this class will behave
* exactly like regular encoder odometry.
*
* <p>Our state-space system is:
*
* <p><strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong>
* in the field coordinate system (dist_* are wheel distances.)
* <p><strong> x = [[x, y, theta, dist_l, dist_r]]^T </strong> in the field coordinate system
* (dist_* are wheel distances.)
*
* <p><strong> u = [[vx, vy, omega]]^T </strong> (robot-relative velocities)
* -- NB: using velocities make things considerably easier, because it means that
* teams don't have to worry about getting an accurate model.
* Basically, we suspect that it's easier for teams to get good encoder data than it is for
* them to perform system identification well enough to get a good model.
* <p><strong> u = [[vx, vy, omega]]^T </strong> (robot-relative velocities) -- NB: using velocities
* make things considerably easier, because it means that teams don't have to worry about getting an
* accurate model. Basically, we suspect that it's easier for teams to get good encoder data than it
* is for them to perform system identification well enough to get a good model.
*
* <p><strong>y = [[x, y, theta]]^T </strong> from vision,
* or <strong>y = [[dist_l, dist_r, theta]] </strong> from encoders and gyro.
* <p><strong>y = [[x, y, theta]]^T </strong> from vision, or <strong>y = [[dist_l, dist_r, theta]]
* </strong> from encoders and gyro.
*/
public class DifferentialDrivePoseEstimator {
final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
@@ -70,84 +64,90 @@ public class DifferentialDrivePoseEstimator {
/**
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta, dist_l, dist_r]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [dist_l, dist_r, theta]^T, with units in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]^T,
* with units in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [dist_l, dist_r, theta]^T, with units in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs, Matrix<N3, N1> visionMeasurementStdDevs
) {
this(gyroAngle, initialPoseMeters,
stateStdDevs, localMeasurementStdDevs, visionMeasurementStdDevs, 0.02);
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
stateStdDevs,
localMeasurementStdDevs,
visionMeasurementStdDevs,
0.02);
}
/**
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta, dist_l, dist_r]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [dist_l, dist_r, theta]^T, with units in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]^T,
* with units in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [dist_l, dist_r, theta]^T, with units in meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs, Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds
) {
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds) {
m_nominalDt = nominalDtSeconds;
m_observer = new UnscentedKalmanFilter<>(
Nat.N5(), Nat.N3(),
this::f,
(x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
stateStdDevs, localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2),
m_nominalDt
);
m_observer =
new UnscentedKalmanFilter<>(
Nat.N5(),
Nat.N3(),
this::f,
(x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2),
m_nominalDt);
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2)
);
m_visionCorrect =
(u, y) ->
m_observer.correct(
Nat.N3(),
u,
y,
(x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
@@ -155,14 +155,13 @@ public class DifferentialDrivePoseEstimator {
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust
* in vision measurements after the autonomous period, or to change trust as distance to a
* vision target increases.
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -173,16 +172,36 @@ public class DifferentialDrivePoseEstimator {
private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
// Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
var theta = x.get(2, 0);
var toFieldRotation = new MatBuilder<>(Nat.N5(), Nat.N5()).fill(
Math.cos(theta), -Math.sin(theta), 0, 0, 0,
Math.sin(theta), Math.cos(theta), 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1
);
return toFieldRotation.times(VecBuilder.fill(
u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)
));
var toFieldRotation =
new MatBuilder<>(Nat.N5(), Nat.N5())
.fill(
Math.cos(theta),
-Math.sin(theta),
0,
0,
0,
Math.sin(theta),
Math.cos(theta),
0,
0,
0,
0,
0,
1,
0,
0,
0,
0,
0,
1,
0,
0,
0,
0,
0,
1);
return toFieldRotation.times(
VecBuilder.fill(u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)));
}
/**
@@ -190,11 +209,11 @@ public class DifferentialDrivePoseEstimator {
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
@@ -209,86 +228,79 @@ public class DifferentialDrivePoseEstimator {
*/
public Pose2d getEstimatedPosition() {
return new Pose2d(
m_observer.getXhat(0),
m_observer.getXhat(1),
new Rotation2d(m_observer.getXhat(2))
);
m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are
* calling {@link DifferentialDrivePoseEstimator#update} every loop.
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision
* camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if
* you don't use your own time source by calling
* {@link DifferentialDrivePoseEstimator#updateWithTime} then you
* must use a timestamp with an epoch since FPGA startup
* (i.e. the epoch of this timestamp is the same epoch as
* Timer.getFPGATimestamp.) This means that you should
* use Timer.getFPGATimestamp as your time source in
* this case.
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
* since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
* Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
* source in this case.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
m_latencyCompensator.applyPastGlobalMeasurement(
Nat.N3(),
m_observer, m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds
);
Nat.N3(),
m_observer,
m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* Note that this should be called every loop.
* Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
* should be called every loop.
*
* @param gyroAngle The current gyro angle.
* @param gyroAngle The current gyro angle.
* @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters
* since the last time you called
* {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceRightMeters The total distance travelled by the right wheel in meters
* since the last time you called
* {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
* last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceRightMeters The total distance travelled by the right wheel in meters since the
* last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters, double distanceRightMeters
) {
Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters,
double distanceRightMeters) {
return updateWithTime(
WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelVelocitiesMetersPerSecond,
distanceLeftMeters, distanceRightMeters
);
WPIUtilJNI.now() * 1.0e-6,
gyroAngle,
wheelVelocitiesMetersPerSecond,
distanceLeftMeters,
distanceRightMeters);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* Note that this should be called every loop.
* Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
* should be called every loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters
* since the last time you called
* {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceRightMeters The total distance travelled by the right wheel in meters
* since the last time you called
* {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
* last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
* @param distanceRightMeters The total distance travelled by the right wheel in meters since the
* last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
* @return The estimated pose of the robot in meters.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters, double distanceRightMeters
) {
double currentTimeSeconds,
Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
double distanceLeftMeters,
double distanceRightMeters) {
double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
m_prevTimeSeconds = currentTimeSeconds;
@@ -296,10 +308,11 @@ public class DifferentialDrivePoseEstimator {
// Diff drive forward kinematics:
// v_c = (v_l + v_r) / 2
var wheelVels = wheelVelocitiesMetersPerSecond;
var u = VecBuilder.fill(
(wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2, 0,
angle.minus(m_previousAngle).getRadians() / dt
);
var u =
VecBuilder.fill(
(wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2,
0,
angle.minus(m_previousAngle).getRadians() / dt);
m_previousAngle = angle;
var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
@@ -312,11 +325,10 @@ public class DifferentialDrivePoseEstimator {
private static Matrix<N5, N1> fillStateVector(Pose2d pose, double leftDist, double rightDist) {
return VecBuilder.fill(
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getRadians(),
leftDist,
rightDist
);
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getRadians(),
leftDist,
rightDist);
}
}

100
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiFunction;
import edu.wpi.first.math.Drake;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
@@ -15,6 +13,7 @@ import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
/**
* Kalman filters combine predictions from a model and measurements to give an estimate of the true
@@ -27,48 +26,50 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
*/
@SuppressWarnings("ClassTypeParameterName")
public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
implements KalmanTypeFilter<States, Inputs, Outputs> {
private final Nat<States> m_states;
private final Nat<Outputs> m_outputs;
@SuppressWarnings("MemberName")
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
@SuppressWarnings("MemberName")
private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
private final Matrix<States, States> m_contQ;
private final Matrix<States, States> m_initP;
private final Matrix<Outputs, Outputs> m_contR;
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_xHat;
@SuppressWarnings("MemberName")
private Matrix<States, States> m_P;
private double m_dtSeconds;
/**
* Constructs an extended Kalman filter.
*
* @param states a Nat representing the number of states.
* @param inputs a Nat representing the number of inputs.
* @param outputs a Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
* the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param states a Nat representing the number of states.
* @param inputs a Nat representing the number of inputs.
* @param outputs a Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @param dtSeconds Nominal discretization timestep.
*/
@SuppressWarnings("ParameterName")
public ExtendedKalmanFilter(
Nat<States> states,
Nat<Inputs> inputs,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds
) {
Nat<States> states,
Nat<Inputs> inputs,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds) {
m_states = states;
m_outputs = outputs;
@@ -81,10 +82,12 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
reset();
final var contA = NumericalJacobian
.numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var C = NumericalJacobian
.numericalJacobianX(outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var contA =
NumericalJacobian.numericalJacobianX(
states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var C =
NumericalJacobian.numericalJacobianX(
outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
final var discA = discPair.getFirst();
@@ -95,8 +98,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
// IsStabilizable(A^T, C^T) will tell us if the system is observable.
boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
if (isObservable && outputs.getNum() <= states.getNum()) {
m_initP = Drake.discreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR) ;
m_initP =
Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = new Matrix<>(states, states);
}
@@ -168,11 +171,10 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
m_xHat = xHat;
}
/**
* Set an element of the initial state estimate x-hat.
*
* @param row Row of x-hat.
* @param row Row of x-hat.
* @param value Value for element of x-hat.
*/
@Override
@@ -189,7 +191,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
/**
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
@SuppressWarnings("ParameterName")
@@ -201,16 +203,15 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
/**
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param f The function used to linearlize the model.
* @param u New control input from controller.
* @param f The function used to linearlize the model.
* @param dtSeconds Timestep for prediction.
*/
@SuppressWarnings("ParameterName")
public void predict(
Matrix<Inputs, N1> u, BiFunction<Matrix<States, N1>,
Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds
) {
Matrix<Inputs, N1> u,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds) {
// Find continuous A
final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
@@ -239,25 +240,24 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
/**
* Correct the state estimate x-hat using the measurements in y.
*
* <p>This is useful for when the measurements available during a timestep's
* Correct() call vary. The h(x, u) passed to the constructor is used if one is
* not provided (the two-argument version of this function).
* <p>This is useful for when the measurements available during a timestep's Correct() call vary.
* The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
* of this function).
*
* @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 rows Number of rows in the result of f(x, u).
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement
* vector.
* @param R Discrete measurement noise covariance matrix.
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Discrete measurement noise covariance matrix.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public <Rows extends Num> void correct(
Nat<Rows> rows, Matrix<Inputs, N1> u,
Matrix<Rows, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
Matrix<Rows, Rows> R
) {
Nat<Rows> rows,
Matrix<Inputs, N1> u,
Matrix<Rows, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
Matrix<Rows, Rows> R) {
final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
final var discR = Discretization.discretizeR(R, m_dtSeconds);

52
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java
View File

@@ -21,51 +21,46 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
*
* <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
* more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
* of squares error in the state estimate. This K gain is used to correct the state estimate by
* some amount of the difference between the actual measurements and the measurements predicted by
* the model.
* of squares error in the state estimate. This K gain is used to correct the state estimate by some
* amount of the difference between the actual measurements and the measurements predicted by the
* model.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
* theory".
*/
@SuppressWarnings("ClassTypeParameterName")
public class KalmanFilter<States extends Num, Inputs extends Num,
Outputs extends Num> {
public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
private final Nat<States> m_states;
private final LinearSystem<States, Inputs, Outputs> m_plant;
/**
* The steady-state Kalman gain matrix.
*/
/** The steady-state Kalman gain matrix. */
@SuppressWarnings("MemberName")
private final Matrix<States, Outputs> m_K;
/**
* The state estimate.
*/
/** The state estimate. */
@SuppressWarnings("MemberName")
private Matrix<States, N1> m_xHat;
/**
* Constructs a state-space observer with the given plant.
*
* @param states A Nat representing the states of the system.
* @param outputs A Nat representing the outputs of the system.
* @param plant The plant used for the prediction step.
* @param stateStdDevs Standard deviations of model states.
* @param states A Nat representing the states of the system.
* @param outputs A Nat representing the outputs of the system.
* @param plant The plant used for the prediction step.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @param dtSeconds Nominal discretization timestep.
*/
@SuppressWarnings("LocalVariableName")
public KalmanFilter(
Nat<States> states, Nat<Outputs> outputs,
Nat<States> states,
Nat<Outputs> outputs,
LinearSystem<States, Inputs, Outputs> plant,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds
) {
double dtSeconds) {
this.m_states = states;
this.m_plant = plant;
@@ -84,14 +79,16 @@ public class KalmanFilter<States extends Num, Inputs extends Num,
// isStabilizable(A^T, C^T) will tell us if the system is observable.
var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
if (!isObservable) {
MathSharedStore.reportError("The system passed to the Kalman filter is not observable!",
MathSharedStore.reportError(
"The system passed to the Kalman filter is not observable!",
Thread.currentThread().getStackTrace());
throw new IllegalArgumentException(
"The system passed to the Kalman filter is not observable!");
"The system passed to the Kalman filter is not observable!");
}
var P = new Matrix<>(Drake.discreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR));
var P =
new Matrix<>(
Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
var S = C.times(P).times(C.transpose()).plus(discR);
@@ -107,8 +104,9 @@ public class KalmanFilter<States extends Num, Inputs extends Num,
//
// K^T = S^T.solve(CP^T)
// K = (S^T.solve(CP^T))^T
m_K = new Matrix<>(S.transpose().getStorage()
.solve((C.times(P.transpose())).getStorage()).transpose());
m_K =
new Matrix<>(
S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
reset();
}
@@ -149,7 +147,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num,
/**
* Set an element of the initial state estimate x-hat.
*
* @param row Row of x-hat.
* @param row Row of x-hat.
* @param value Value for element of x-hat.
*/
public void setXhat(int row, double value) {
@@ -178,7 +176,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num,
/**
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
@SuppressWarnings("ParameterName")

70
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilterLatencyCompensator.java
View File

@@ -4,15 +4,14 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.function.BiConsumer;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.function.BiConsumer;
public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O extends Num> {
private static final int kMaxPastObserverStates = 300;
@@ -26,19 +25,19 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
/**
* Add past observer states to the observer snapshots list.
*
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
* @param timestampSeconds The timesnap of the state.
*/
@SuppressWarnings("ParameterName")
public void addObserverState(
KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY,
double timestampSeconds
) {
m_pastObserverSnapshots.add(Map.entry(
timestampSeconds, new ObserverSnapshot(observer, u, localY)
));
KalmanTypeFilter<S, I, O> observer,
Matrix<I, N1> u,
Matrix<O, N1> localY,
double timestampSeconds) {
m_pastObserverSnapshots.add(
Map.entry(timestampSeconds, new ObserverSnapshot(observer, u, localY)));
if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
m_pastObserverSnapshots.remove(0);
@@ -48,23 +47,22 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
/**
* Add past global measurements (such as from vision)to the estimator.
*
* @param <R> The rows in the global measurement vector.
* @param rows The rows in the global measurement vector.
* @param observer The observer to apply the past global measurement.
* @param nominalDtSeconds The nominal timestep.
* @param globalMeasurement The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the observer.
* @param <R> The rows in the global measurement vector.
* @param rows The rows in the global measurement vector.
* @param observer The observer to apply the past global measurement.
* @param nominalDtSeconds The nominal timestep.
* @param globalMeasurement The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the observer.
* @param globalMeasurementTimestampSeconds The timestamp of the measurement.
*/
@SuppressWarnings({"ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
public <R extends Num> void applyPastGlobalMeasurement(
Nat<R> rows,
KalmanTypeFilter<S, I, O> observer,
double nominalDtSeconds,
Matrix<R, N1> globalMeasurement,
BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
double globalMeasurementTimestampSeconds
) {
Nat<R> rows,
KalmanTypeFilter<S, I, O> observer,
double nominalDtSeconds,
Matrix<R, N1> globalMeasurement,
BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
double globalMeasurementTimestampSeconds) {
if (m_pastObserverSnapshots.isEmpty()) {
// State map was empty, which means that we got a past measurement right at startup. The only
// thing we can really do is ignore the measurement.
@@ -96,12 +94,13 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
double timestamp = globalMeasurementTimestampSeconds;
int indexOfClosestEntry =
Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
<= Math.abs(timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey())
<= Math.abs(
timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey())
? index - 1
: index;
double lastTimestamp =
m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
// We will now go back in time to the state of the system at the time when
// the measurement was captured. We will reset the observer to that state,
@@ -129,14 +128,14 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
}
lastTimestamp = key;
m_pastObserverSnapshots.set(i, Map.entry(key,
new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements)));
m_pastObserverSnapshots.set(
i,
Map.entry(
key, new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements)));
}
}
/**
* This class contains all the information about our observer at a given time.
*/
/** This class contains all the information about our observer at a given time. */
@SuppressWarnings("MemberName")
public class ObserverSnapshot {
public final Matrix<S, N1> xHat;
@@ -146,8 +145,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
@SuppressWarnings("ParameterName")
private ObserverSnapshot(
KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY
) {
KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY) {
this.xHat = observer.getXhat();
this.errorCovariances = observer.getP();

254
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiConsumer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -19,22 +17,21 @@ import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N3;
import java.util.function.BiConsumer;
/**
* This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
* latency-compensated vision measurements with mecanum drive encoder velocity measurements.
* It will correct for noisy measurements and encoder drift. It is intended to be an easy
* but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry}.
* latency-compensated vision measurements with mecanum drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
* drop-in for {@link edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry}.
*
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If
* your loops are faster or slower than the default of 0.02s, then you should change
* the nominal delta time using the secondary constructor:
* {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d, Pose2d,
* MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 0.02s, then you should change the nominal delta time using
* the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
* Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as
* infrequently as you want; if you never call it, then this class will behave mostly like regular
* encoder odometry.
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
*
* <p>Our state-space system is:
*
@@ -42,8 +39,8 @@ import edu.wpi.first.wpiutil.math.numbers.N3;
*
* <p><strong> u = [[vx, vy, theta]]^T </strong> in the field-coordinate system.
*
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision,
* or <strong> y = [[theta]]^T </strong> from the gyro.
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
* [[theta]]^T </strong> from the gyro.
*/
public class MecanumDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
@@ -62,87 +59,96 @@ public class MecanumDrivePoseEstimator {
/**
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs
) {
this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs,
visionMeasurementStdDevs, 0.02);
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
visionMeasurementStdDevs,
0.02);
}
/**
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs, double nominalDtSeconds
) {
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds) {
m_nominalDt = nominalDtSeconds;
m_observer = new UnscentedKalmanFilter<>(
Nat.N3(), Nat.N1(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
m_nominalDt
);
m_observer =
new UnscentedKalmanFilter<>(
Nat.N3(),
Nat.N1(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
m_nominalDt);
m_kinematics = kinematics;
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u1) -> x,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2)
);
m_visionCorrect =
(u, y) ->
m_observer.correct(
Nat.N3(),
u,
y,
(x, u1) -> x,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
@@ -150,14 +156,13 @@ public class MecanumDrivePoseEstimator {
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust
* in vision measurements after the autonomous period, or to change trust as distance to a
* vision target increases.
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -169,11 +174,11 @@ public class MecanumDrivePoseEstimator {
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset in the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
@@ -188,46 +193,39 @@ public class MecanumDrivePoseEstimator {
*/
public Pose2d getEstimatedPosition() {
return new Pose2d(
m_observer.getXhat(0),
m_observer.getXhat(1),
new Rotation2d(m_observer.getXhat(2))
);
m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are
* calling {@link MecanumDrivePoseEstimator#update} every loop.
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision
* camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if
* you don't use your own time source by calling
* {@link MecanumDrivePoseEstimator#updateWithTime} then you
* must use a timestamp with an epoch since FPGA startup
* (i.e. the epoch of this timestamp is the same epoch as
* Timer.getFPGATimestamp.) This means that you should
* use Timer.getFPGATimestamp as your time source
* or sync the epochs.
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
* then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
* timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
* Timer.getFPGATimestamp as your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
m_latencyCompensator.applyPastGlobalMeasurement(
Nat.N3(),
m_observer, m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds
);
Nat.N3(),
m_observer,
m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* This should be called every loop, and the correct loop period must be passed
* into the constructor of this class.
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this
* class.
*
* @param gyroAngle The current gyro angle.
* @param gyroAngle The current gyro angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @return The estimated pose of the robot in meters.
*/
@@ -236,18 +234,18 @@ public class MecanumDrivePoseEstimator {
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* This should be called every loop, and the correct loop period must be passed
* into the constructor of this class.
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this
* class.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @param gyroAngle The current gyroscope angle.
* @param wheelSpeeds The current speeds of the mecanum drive wheels.
* @return The estimated pose of the robot in meters.
*/
@SuppressWarnings("LocalVariableName")
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
m_prevTimeSeconds = currentTimeSeconds;
@@ -256,14 +254,10 @@ public class MecanumDrivePoseEstimator {
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
var fieldRelativeVelocities =
new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
.rotateBy(angle);
new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
.rotateBy(angle);
var u = VecBuilder.fill(
fieldRelativeVelocities.getX(),
fieldRelativeVelocities.getY(),
omega
);
var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
m_previousAngle = angle;
var localY = VecBuilder.fill(angle.getRadians());

46
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
View File

@@ -4,26 +4,23 @@
package edu.wpi.first.wpilibj.estimator;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* Generates sigma points and weights according to Van der Merwe's 2004
* dissertation[1] for the UnscentedKalmanFilter class.
* Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the
* UnscentedKalmanFilter class.
*
* <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the
* version seen in most publications. Unless you know better, this should be
* your default choice.
* <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the version seen in
* most publications. Unless you know better, this should be your default choice.
*
* <p>States is the dimensionality of the state. 2*States+1 weights will be
* generated.
* <p>States is the dimensionality of the state. 2*States+1 weights will be generated.
*
* <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
* Inference in Dynamic State-Space Models" (Doctoral dissertation)
* <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic
* State-Space Models" (Doctoral dissertation)
*/
public class MerweScaledSigmaPoints<S extends Num> {
@@ -37,11 +34,11 @@ public class MerweScaledSigmaPoints<S extends Num> {
* Constructs a generator for Van der Merwe scaled sigma points.
*
* @param states an instance of Num that represents the number of states.
* @param alpha Determines the spread of the sigma points around the mean.
* Usually a small positive value (1e-3).
* @param beta Incorporates prior knowledge of the distribution of the mean.
* For Gaussian distributions, beta = 2 is optimal.
* @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
* @param alpha Determines the spread of the sigma points around the mean. Usually a small
* positive value (1e-3).
* @param beta Incorporates prior knowledge of the distribution of the mean. For Gaussian
* distributions, beta = 2 is optimal.
* @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
*/
public MerweScaledSigmaPoints(Nat<S> states, double alpha, double beta, int kappa) {
this.m_states = states;
@@ -71,27 +68,24 @@ public class MerweScaledSigmaPoints<S extends Num> {
}
/**
* Computes the sigma points for an unscented Kalman filter given the mean
* (x) and covariance(P) of the filter.
* Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
* of the filter.
*
* @param x An array of the means.
* @param P Covariance of the filter.
* @return Two dimensional array of sigma points. Each column contains all of
* the sigmas for one dimension in the problem space. Ordered by
* Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
* @return Two dimensional array of sigma points. Each column contains all of the sigmas for one
* dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public Matrix<S, ?> sigmaPoints(
Matrix<S, N1> x,
Matrix<S, S> P) {
public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
var intermediate = P.times(lambda + m_states.getNum());
var U = intermediate.lltDecompose(true); // Lower triangular
// 2 * states + 1 by states
Matrix<S, ?> sigmas = new Matrix<>(
new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
Matrix<S, ?> sigmas =
new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
sigmas.setColumn(0, x);
for (int k = 0; k < m_states.getNum(); k++) {
var xPlusU = x.plus(U.extractColumnVector(k));

261
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiConsumer;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -19,22 +17,21 @@ import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N3;
import java.util.function.BiConsumer;
/**
* This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
* latency-compensated vision measurements with swerve drive encoder velocity measurements.
* It will correct for noisy measurements and encoder drift. It is intended to be an easy
* but more accurate drop-in for {@link edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry}.
* latency-compensated vision measurements with swerve drive encoder velocity measurements. It will
* correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
* drop-in for {@link edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry}.
*
* <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If
* your loops are faster or slower than the default of 0.02s, then you should change
* the nominal delta time using the secondary constructor:
* {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d, Pose2d,
* SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
* <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 0.02s, then you should change the nominal delta time using
* the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d,
* Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as
* infrequently as you want; if you never call it, then this class will behave mostly like regular
* encoder odometry.
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
*
* <p>Our state-space system is:
*
@@ -42,8 +39,8 @@ import edu.wpi.first.wpiutil.math.numbers.N3;
*
* <p><strong> u = [[vx, vy, omega]]^T </strong> in the field-coordinate system.
*
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision,
* or <strong> y = [[theta]]^T </strong> from the gyro.
* <p><strong> y = [[x, y, theta]]^T </strong> in field coords from vision, or <strong> y =
* [[theta]]^T </strong> from the gyro.
*/
public class SwerveDrivePoseEstimator {
private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
@@ -62,87 +59,96 @@ public class SwerveDrivePoseEstimator {
/**
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public SwerveDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs
) {
this(gyroAngle, initialPoseMeters, kinematics, stateStdDevs, localMeasurementStdDevs,
visionMeasurementStdDevs, 0.02);
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
visionMeasurementStdDevs,
0.02);
}
/**
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
* Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
* is in the form [theta], with units in radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
* @param nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
public SwerveDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs, Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs, double nominalDtSeconds
) {
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<N3, N1> stateStdDevs,
Matrix<N1, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs,
double nominalDtSeconds) {
m_nominalDt = nominalDtSeconds;
m_observer = new UnscentedKalmanFilter<>(
Nat.N3(), Nat.N1(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
m_nominalDt
);
m_observer =
new UnscentedKalmanFilter<>(
Nat.N3(),
Nat.N1(),
(x, u) -> u,
(x, u) -> x.extractRowVector(2),
stateStdDevs,
localMeasurementStdDevs,
AngleStatistics.angleMean(2),
AngleStatistics.angleMean(0),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
m_nominalDt);
m_kinematics = kinematics;
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u1) -> x,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2)
);
m_visionCorrect =
(u, y) ->
m_observer.correct(
Nat.N3(),
u,
y,
(x, u1) -> x,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
@@ -150,14 +156,13 @@ public class SwerveDrivePoseEstimator {
}
/**
* Sets the pose estimator's trust of global measurements. This might be used to change trust
* in vision measurements after the autonomous period, or to change trust as distance to a
* vision target increases.
* Sets the pose estimator's trust of global measurements. This might be used to change trust in
* vision measurements after the autonomous period, or to change trust as distance to a vision
* target increases.
*
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase
* these numbers to trust global measurements from vision less.
* This matrix is in the form [x, y, theta]^T, with units in
* meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
* numbers to trust global measurements from vision less. This matrix is in the form [x, y,
* theta]^T, with units in meters and radians.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
@@ -169,11 +174,11 @@ public class SwerveDrivePoseEstimator {
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset in the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_previousAngle = poseMeters.getRotation();
@@ -188,71 +193,59 @@ public class SwerveDrivePoseEstimator {
*/
public Pose2d getEstimatedPosition() {
return new Pose2d(
m_observer.getXhat(0),
m_observer.getXhat(1),
new Rotation2d(m_observer.getXhat(2))
);
m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the
* odometry pose estimate while still accounting for measurement noise.
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are
* calling {@link SwerveDrivePoseEstimator#update} every loop.
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision
* camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if
* you don't use your own time source by calling
* {@link SwerveDrivePoseEstimator#updateWithTime} then you
* must use a timestamp with an epoch since FPGA startup
* (i.e. the epoch of this timestamp is the same epoch as
* Timer.getFPGATimestamp.) This means that you should
* use Timer.getFPGATimestamp as your time source or
* sync the epochs.
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
* then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
* timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
* Timer.getFPGATimestamp as your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
m_latencyCompensator.applyPastGlobalMeasurement(
Nat.N3(),
m_observer, m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds
);
Nat.N3(),
m_observer,
m_nominalDt,
StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
m_visionCorrect,
timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* This should be called every loop, and the correct loop period must be passed
* into the constructor of this class.
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this
* class.
*
* @param gyroAngle The current gyro angle.
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
Rotation2d gyroAngle,
SwerveModuleState... moduleStates
) {
public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information.
* This should be called every loop, and the correct loop period must be passed
* into the constructor of this class.
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this
* class.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param moduleStates The current velocities and rotations of the swerve modules.
* @param gyroAngle The current gyroscope angle.
* @param moduleStates The current velocities and rotations of the swerve modules.
* @return The estimated pose of the robot in meters.
*/
@SuppressWarnings("LocalVariableName")
public Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle, SwerveModuleState... moduleStates
) {
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
m_prevTimeSeconds = currentTimeSeconds;
@@ -260,15 +253,11 @@ public class SwerveDrivePoseEstimator {
var omega = angle.minus(m_previousAngle).getRadians() / dt;
var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
var fieldRelativeVelocities = new Translation2d(
chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond
).rotateBy(angle);
var fieldRelativeVelocities =
new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
.rotateBy(angle);
var u = VecBuilder.fill(
fieldRelativeVelocities.getX(),
fieldRelativeVelocities.getY(),
omega
);
var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
m_previousAngle = angle;
var localY = VecBuilder.fill(angle.getRadians());

245
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
View File

@@ -4,10 +4,6 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.function.BiFunction;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.math.Discretization;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.NumericalJacobian;
@@ -17,6 +13,8 @@ import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.Pair;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import org.ejml.simple.SimpleMatrix;
/**
* A Kalman filter combines predictions from a model and measurements to give an estimate of the
@@ -29,8 +27,8 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
* the UKF works with nonlinear systems.
*/
@SuppressWarnings({"MemberName", "ClassTypeParameterName", "PMD.TooManyFields"})
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
Outputs extends Num> implements KalmanTypeFilter<States, Inputs, Outputs> {
public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
implements KalmanTypeFilter<States, Inputs, Outputs> {
private final Nat<States> m_states;
private final Nat<Outputs> m_outputs;
@@ -56,36 +54,36 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
/**
* Constructs an Unscented Kalman Filter.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
* the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDtSeconds Nominal discretization timestep.
* @param nominalDtSeconds Nominal discretization timestep.
*/
@SuppressWarnings("LambdaParameterName")
public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDtSeconds) {
public UnscentedKalmanFilter(
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDtSeconds) {
this(
states, outputs,
f, h,
stateStdDevs, measurementStdDevs,
states,
outputs,
f,
h,
stateStdDevs,
measurementStdDevs,
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
Matrix::minus,
Matrix::minus,
Matrix::plus,
nominalDtSeconds
);
nominalDtSeconds);
}
/**
@@ -93,39 +91,37 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
* custom functions for arithmetic can be useful if you have angles in the state or measurements,
* because they allow you to correctly account for the modular nature of angle arithmetic.
*
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns
* the derivative of the state vector.
* @param h A vector-valued function of x and u that returns
* the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param states A Nat representing the number of states.
* @param outputs A Nat representing the number of outputs.
* @param f A vector-valued function of x and u that returns the derivative of the state vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors
* using a given set of weights.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement
* vectors using a given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors
* (i.e. it subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param nominalDtSeconds Nominal discretization timestep.
* @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors using a
* given set of weights.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
* a given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param nominalDtSeconds Nominal discretization timestep.
*/
@SuppressWarnings({"ParameterName", "PMD.ExcessiveParameterList"})
public UnscentedKalmanFilter(
Nat<States> states, Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double nominalDtSeconds
) {
Nat<States> states,
Nat<Outputs> outputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double nominalDtSeconds) {
this.m_states = states;
this.m_outputs = outputs;
@@ -149,25 +145,30 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
}
@SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"})
static <S extends Num, C extends Num>
Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
Nat<S> s, Nat<C> dim, Matrix<C, ?> sigmas, Matrix<?, N1> Wm, Matrix<?, N1> Wc,
BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc
) {
static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
Nat<S> s,
Nat<C> dim,
Matrix<C, ?> sigmas,
Matrix<?, N1> Wm,
Matrix<?, N1> Wc,
BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc) {
if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got "
+ sigmas.getNumRows() + " by " + sigmas.getNumCols());
throw new IllegalArgumentException(
"Sigmas must be covDim by 2 * states + 1! Got "
+ sigmas.getNumRows()
+ " by "
+ sigmas.getNumCols());
}
if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1 ) {
throw new IllegalArgumentException("Wm must be 2 * states + 1 by 1! Got "
+ Wm.getNumRows() + " by " + Wm.getNumCols());
if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1) {
throw new IllegalArgumentException(
"Wm must be 2 * states + 1 by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
}
if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
throw new IllegalArgumentException("Wc must be 2 * states + 1 by 1! Got "
+ Wc.getNumRows() + " by " + Wc.getNumCols());
throw new IllegalArgumentException(
"Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
}
// New mean is usually just the sum of the sigmas * weight:
@@ -181,8 +182,9 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
// y[:, i] = sigmas[:, i] - x
y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
}
Matrix<C, C> P = y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
.times(Matrix.changeBoundsUnchecked(y.transpose()));
Matrix<C, C> P =
y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
.times(Matrix.changeBoundsUnchecked(y.transpose()));
return new Pair<>(x, P);
}
@@ -240,7 +242,6 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
return m_xHat.get(row, 0);
}
/**
* Set initial state estimate x-hat.
*
@@ -255,7 +256,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
/**
* Set an element of the initial state estimate x-hat.
*
* @param row Row of x-hat.
* @param row Row of x-hat.
* @param value Value for element of x-hat.
*/
@Override
@@ -263,9 +264,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
m_xHat.set(row, 0, value);
}
/**
* Resets the observer.
*/
/** Resets the observer. */
@Override
public void reset() {
m_xHat = new Matrix<>(m_states, Nat.N1());
@@ -276,7 +275,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
/**
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
@@ -285,8 +284,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
// Discretize Q before projecting mean and covariance forward
Matrix<States, States> contA =
NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
var discQ =
Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
@@ -296,8 +294,15 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds));
}
var ret = unscentedTransform(m_states, m_states,
m_sigmasF, m_pts.getWm(), m_pts.getWc(), m_meanFuncX, m_residualFuncX);
var ret =
unscentedTransform(
m_states,
m_states,
m_sigmasF,
m_pts.getWm(),
m_pts.getWc(),
m_meanFuncX,
m_residualFuncX);
m_xHat = ret.getFirst();
m_P = ret.getSecond().plus(discQ);
@@ -313,16 +318,16 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
@SuppressWarnings("ParameterName")
@Override
public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
correct(m_outputs, u, y, m_h, m_contR,
m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
correct(
m_outputs, u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
}
/**
* Correct the state estimate x-hat using the measurements in y.
*
* <p>This is useful for when the measurements available during a timestep's
* Correct() call vary. The h(x, u) passed to the constructor is used if one
* is not provided (the two-argument version of this function).
* <p>This is useful for when the measurements available during a timestep's Correct() call vary.
* The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
* of this function).
*
* @param u Same control input used in the predict step.
* @param y Measurement vector.
@@ -331,10 +336,11 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
*/
@SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
public <R extends Num> void correct(
Nat<R> rows, Matrix<Inputs, N1> u,
Matrix<R, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
Matrix<R, R> R) {
Nat<R> rows,
Matrix<Inputs, N1> u,
Matrix<R, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
Matrix<R, R> R) {
BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY =
(sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm));
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX =
@@ -347,50 +353,47 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
/**
* Correct the state estimate x-hat using the measurements in y.
*
* <p>This is useful for when the measurements available during a timestep's
* Correct() call vary. The h(x, u) passed to the constructor is used if one
* is not provided (the two-argument version of this function).
* <p>This is useful for when the measurements available during a timestep's Correct() call vary.
* The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
* of this function).
*
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the
* measurement vector.
* @param R Measurement noise covariance matrix.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors
* using a given set of weights.
* @param u Same control input used in the predict step.
* @param y Measurement vector.
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param R Measurement noise covariance matrix.
* @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
* a given set of weights.
* @param residualFuncX A function that computes the residual of two state vectors (i.e. it
* subtracts them.)
* subtracts them.)
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public <R extends Num> void correct(
Nat<R> rows, Matrix<Inputs, N1> u,
Matrix<R, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
Matrix<R, R> R,
BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY,
BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
Nat<R> rows,
Matrix<Inputs, N1> u,
Matrix<R, N1> y,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
Matrix<R, R> R,
BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY,
BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var discR = Discretization.discretizeR(R, m_dtSeconds);
// Transform sigma points into measurement space
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(
rows.getNum(), 2 * m_states.getNum() + 1));
Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
for (int i = 0; i < m_pts.getNumSigmas(); i++) {
Matrix<R, N1> hRet = h.apply(
sigmas.extractColumnVector(i),
u
);
Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
sigmasH.setColumn(i, hRet);
}
// Mean and covariance of prediction passed through unscented transform
var transRet = unscentedTransform(m_states, rows,
sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
var transRet =
unscentedTransform(
m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
var yHat = transRet.getFirst();
var Py = transRet.getSecond().plus(discR);
@@ -409,9 +412,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
// P_y^T K^T = P_{xy}^T
// K^T = P_y^T.solve(P_{xy}^T)
// K = (P_y^T.solve(P_{xy}^T)^T
Matrix<States, R> K = new Matrix<>(
Py.transpose().solve(Pxy.transpose()).transpose()
);
Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
// xHat + K * (y - yHat)
m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));

94
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -4,16 +4,13 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Objects;
/**
* Represents a 2d pose containing translational and rotational elements.
*/
/** Represents a 2d pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d {
@@ -21,8 +18,8 @@ public class Pose2d {
private final Rotation2d m_rotation;
/**
* Constructs a pose at the origin facing toward the positive X axis.
* (Translation2d{0, 0} and Rotation{0})
* Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
* Rotation{0})
*/
public Pose2d() {
m_translation = new Translation2d();
@@ -33,21 +30,22 @@ public class Pose2d {
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
* @param rotation The rotational component of the pose.
*/
@JsonCreator
public Pose2d(@JsonProperty(required = true, value = "translation") Translation2d translation,
@JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
public Pose2d(
@JsonProperty(required = true, value = "translation") Translation2d translation,
@JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Convenience constructors that takes in x and y values directly instead of
* having to construct a Translation2d.
* Convenience constructors that takes in x and y values directly instead of having to construct a
* Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose2d(double x, double y, Rotation2d rotation) {
@@ -56,13 +54,10 @@ public class Pose2d {
}
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
* Transforms the pose by the given transformation and returns the new transformed pose.
*
* <p>The matrix multiplication is as follows
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [0, 0, 1][transform.t]
* <p>The matrix multiplication is as follows [x_new] [cos, -sin, 0][transform.x] [y_new] += [sin,
* cos, 0][transform.y] [t_new] [0, 0, 1][transform.t]
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
@@ -121,26 +116,26 @@ public class Pose2d {
}
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
* Transforms the pose by the given transformation and returns the new pose. See + operator for
* the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d transformBy(Transform2d other) {
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
return new Pose2d(
m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
* <p>This function can often be used for trajectory tracking or pose stabilization algorithms to
* get the error between the reference and the current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
* @param other The pose that is the origin of the new coordinate frame that the current pose will
* be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose2d relativeTo(Pose2d other) {
@@ -151,22 +146,21 @@ public class Pose2d {
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
* Controls Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for
* a derivation.
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
* derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
* the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist2d{0.01, 0.0,
* toRadians(0.5)}
* @return The new pose of the robot.
*/
public Pose2d exp(Twist2d twist) {
@@ -186,15 +180,17 @@ public class Pose2d {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
var transform =
new Transform2d(
new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
return this.plus(transform);
}
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
* Returns a Twist2d that maps this pose to the end pose. If c is the output of a.Log(b), then
* a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
@@ -213,9 +209,11 @@ public class Pose2d {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart = transform.getTranslation().rotateBy(
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
Translation2d translationPart =
transform
.getTranslation()
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}

44
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -4,17 +4,13 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Objects;
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
*/
/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation2d {
@@ -22,9 +18,7 @@ public class Rotation2d {
private final double m_cos;
private final double m_sin;
/**
* Constructs a Rotation2d with a default angle of 0 degrees.
*/
/** Constructs a Rotation2d with a default angle of 0 degrees. */
public Rotation2d() {
m_value = 0.0;
m_cos = 1.0;
@@ -32,8 +26,7 @@ public class Rotation2d {
}
/**
* Constructs a Rotation2d with the given radian value.
* The x and y don't have to be normalized.
* Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
*
* @param value The value of the angle in radians.
*/
@@ -45,8 +38,7 @@ public class Rotation2d {
}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
* components.
* Constructs a Rotation2d with the given x and y (cosine and sine) components.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
@@ -74,11 +66,9 @@ public class Rotation2d {
}
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
* Adds two rotations together, with the result being bounded between -pi and pi.
*
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
* Rotation2d{-pi/2}
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/2}
*
* @param other The rotation to add.
* @return The sum of the two rotations.
@@ -88,11 +78,9 @@ public class Rotation2d {
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
* Subtracts the new rotation from the current rotation and returns the new rotation.
*
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
* Rotation2d{-pi/2}
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = Rotation2d{-pi/2}
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
@@ -102,8 +90,8 @@ public class Rotation2d {
}
/**
* Takes the inverse of the current rotation. This is simply the negative of
* the current angular value.
* Takes the inverse of the current rotation. This is simply the negative of the current angular
* value.
*
* @return The inverse of the current rotation.
*/
@@ -124,19 +112,15 @@ public class Rotation2d {
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* <p>The matrix multiplication is as follows:
* [cos_new] [other.cos, -other.sin][cos]
* [sin_new] = [other.sin, other.cos][sin]
* value_new = atan2(cos_new, sin_new)
* <p>The matrix multiplication is as follows: [cos_new] [other.cos, -other.sin][cos] [sin_new] =
* [other.sin, other.cos][sin] value_new = atan2(cos_new, sin_new)
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation2d.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
m_cos * other.m_cos - m_sin * other.m_sin,
m_cos * other.m_sin + m_sin * other.m_cos
);
m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
}
/**

19
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
View File

@@ -6,9 +6,7 @@ package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a transformation for a Pose2d.
*/
/** Represents a transformation for a Pose2d. */
public class Transform2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
@@ -17,13 +15,15 @@ public class Transform2d {
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform2d(Pose2d initial, Pose2d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = last.getTranslation().minus(initial.getTranslation())
m_translation =
last.getTranslation()
.minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
@@ -33,16 +33,14 @@ public class Transform2d {
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
@@ -103,7 +101,8 @@ public class Transform2d {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return new Transform2d(getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
return new Transform2d(
getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
getRotation().unaryMinus());
}

65
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
View File

@@ -4,20 +4,18 @@
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Objects;
/**
* Represents a translation in 2d space.
* This object can be used to represent a point or a vector.
* Represents a translation in 2d space. This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes.
* When the robot is placed on the origin, facing toward the X direction,
* moving forward increases the X, whereas moving to the left increases the Y.
* <p>This assumes that you are using conventional mathematical axes. When the robot is placed on
* the origin, facing toward the X direction, moving forward increases the X, whereas moving to the
* left increases the Y.
*/
@SuppressWarnings({"ParameterName", "MemberName"})
@JsonIgnoreProperties(ignoreUnknown = true)
@@ -26,33 +24,31 @@ public class Translation2d {
private final double m_x;
private final double m_y;
/**
* Constructs a Translation2d with X and Y components equal to zero.
*/
/** Constructs a Translation2d with X and Y components equal to zero. */
public Translation2d() {
this(0.0, 0.0);
}
/**
* Constructs a Translation2d with the X and Y components equal to the
* provided values.
* Constructs a Translation2d with the X and Y components equal to the provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
@JsonCreator
public Translation2d(@JsonProperty(required = true, value = "x") double x,
@JsonProperty(required = true, value = "y") double y) {
public Translation2d(
@JsonProperty(required = true, value = "x") double x,
@JsonProperty(required = true, value = "y") double y) {
m_x = x;
m_y = y;
}
/**
* Constructs a Translation2d with the provided distance and angle. This is
* essentially converting from polar coordinates to Cartesian coordinates.
* Constructs a Translation2d with the provided distance and angle. This is essentially converting
* from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
* @param angle The angle between the x-axis and the translation vector.
*/
public Translation2d(double distance, Rotation2d angle) {
m_x = distance * angle.getCos();
@@ -62,8 +58,8 @@ public class Translation2d {
/**
* Calculates the distance between two translations in 2d space.
*
* <p>This function uses the pythagorean theorem to calculate the distance.
* distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
* <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
* x1)^2 + (y2 - y1)^2)
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
@@ -104,30 +100,24 @@ public class Translation2d {
/**
* Applies a rotation to the translation in 2d space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
* <p>This multiplies the translation vector by a counterclockwise rotation matrix of the given
* angle. [x_new] [other.cos, -other.sin][x] [y_new] = [other.sin, other.cos][y]
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
* Translation2d of {0, 2}.
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a Translation2d of
* {0, 2}.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation2d rotateBy(Rotation2d other) {
return new Translation2d(
m_x * other.getCos() - m_y * other.getSin(),
m_x * other.getSin() + m_y * other.getCos()
);
m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to
* vector addition.
* Adds two translations in 2d space and returns the sum. This is similar to vector addition.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
* Translation2d{3.0, 8.0}
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} = Translation2d{3.0, 8.0}
*
* @param other The translation to add.
* @return The sum of the translations.
@@ -137,11 +127,9 @@ public class Translation2d {
}
/**
* Subtracts the other translation from the other translation and returns the
* difference.
* Subtracts the other translation from the other translation and returns the difference.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
* Translation2d{4.0, 2.0}
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} = Translation2d{4.0, 2.0}
*
* @param other The translation to subtract.
* @return The difference between the two translations.
@@ -151,9 +139,8 @@ public class Translation2d {
}
/**
* Returns the inverse of the current translation. This is equivalent to
* rotating by 180 degrees, flipping the point over both axes, or simply
* negating both components of the translation.
* Returns the inverse of the current translation. This is equivalent to rotating by 180 degrees,
* flipping the point over both axes, or simply negating both components of the translation.
*
* @return The inverse of the current translation.
*/

21
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
View File

@@ -7,34 +7,27 @@ package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
* A change in distance along arc since the last pose update. We can use ideas from differential
* calculus to create new Pose2ds from a Twist2d and vise versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist2d {
/**
* Linear "dx" component.
*/
/** Linear "dx" component. */
public double dx;
/**
* Linear "dy" component.
*/
/** Linear "dy" component. */
public double dy;
/**
* Angular "dtheta" component (radians).
*/
/** Angular "dtheta" component (radians). */
public double dtheta;
public Twist2d() {
}
public Twist2d() {}
/**
* Constructs a Twist2d with the given values.
*
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dtheta Change in angle relative to robot.

74
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
View File

@@ -4,85 +4,75 @@
package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of reference,
* this ChassisSpeeds struct represents a velocity w.r.t to the robot frame of
* reference.
* Represents the speed of a robot chassis. Although this struct contains similar members compared
* to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
* w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
* the robot frame of reference.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
@SuppressWarnings("MemberName")
public class ChassisSpeeds {
/**
* Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
*/
/** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
public double vxMetersPerSecond;
/**
* Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
*/
/** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
public double vyMetersPerSecond;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
/** Represents the angular velocity of the robot frame. (CCW is +) */
public double omegaRadiansPerSecond;
/**
* Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
*/
public ChassisSpeeds() {
}
/** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
public ChassisSpeeds() {}
/**
* Constructs a ChassisSpeeds object.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
*/
public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
double omegaRadiansPerSecond) {
public ChassisSpeeds(
double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
this.vxMetersPerSecond = vxMetersPerSecond;
this.vyMetersPerSecond = vyMetersPerSecond;
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omegaRadiansPerSecond The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's
* angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should
* be CCW positive.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond, double vyMetersPerSecond,
double omegaRadiansPerSecond, Rotation2d robotAngle) {
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
return new ChassisSpeeds(
vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
omegaRadiansPerSecond
);
omegaRadiansPerSecond);
}
@Override
public String toString() {
return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
return String.format(
"ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
}
}

43
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
View File

@@ -8,12 +8,12 @@ import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
* Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
* velocities for a differential drive.
*
* <p>Inverse kinematics converts a desired chassis speed into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
* <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
*/
@SuppressWarnings("MemberName")
public class DifferentialDriveKinematics {
@@ -22,10 +22,9 @@ public class DifferentialDriveKinematics {
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidthMeters The track width of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels.
* However, the empirical value may be larger than the physical
* measured value due to scrubbing effects.
* @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
* between the left wheels and right wheels. However, the empirical value may be larger than
* the physical measured value due to scrubbing effects.
*/
public DifferentialDriveKinematics(double trackWidthMeters) {
this.trackWidthMeters = trackWidthMeters;
@@ -33,34 +32,30 @@ public class DifferentialDriveKinematics {
}
/**
* Returns a chassis speed from left and right component velocities using
* forward kinematics.
* Returns a chassis speed from left and right component velocities using forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
/ trackWidthMeters
);
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
}
/**
* Returns left and right component velocities from a chassis speed using
* inverse kinematics.
* Returns left and right component velocities from a chassis speed using inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
* chassis' speed.
* @return The left and right velocities.
*/
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
* chassisSpeeds.omegaRadiansPerSecond,
chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
* chassisSpeeds.omegaRadiansPerSecond
);
chassisSpeeds.vxMetersPerSecond
- trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
chassisSpeeds.vxMetersPerSecond
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
}

45
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
View File

@@ -11,16 +11,14 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Twist2d;
/**
* Class for differential drive odometry. Odometry allows you to track the
* robot's position on the field over the course of a match using readings from
* 2 encoders and a gyroscope.
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
* field over the course of a match using readings from 2 encoders and a gyroscope.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*
* <p>It is important that you reset your encoders to zero before using this class.
* Any subsequent pose resets also require the encoders to be reset to zero.
* <p>It is important that you reset your encoders to zero before using this class. Any subsequent
* pose resets also require the encoders to be reset to zero.
*/
public class DifferentialDriveOdometry {
private Pose2d m_poseMeters;
@@ -34,11 +32,10 @@ public class DifferentialDriveOdometry {
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle,
Pose2d initialPoseMeters) {
public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
@@ -59,11 +56,11 @@ public class DifferentialDriveOdometry {
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
@@ -83,19 +80,18 @@ public class DifferentialDriveOdometry {
return m_poseMeters;
}
/**
* Updates the robot position on the field using distance measurements from encoders. This
* method is more numerically accurate than using velocities to integrate the pose and
* is also advantageous for teams that are using lower CPR encoders.
* Updates the robot position on the field using distance measurements from encoders. This method
* is more numerically accurate than using velocities to integrate the pose and is also
* advantageous for teams that are using lower CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
double rightDistanceMeters) {
public Pose2d update(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
@@ -105,8 +101,9 @@ public class DifferentialDriveOdometry {
double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
var angle = gyroAngle.plus(m_gyroOffset);
var newPose = m_poseMeters.exp(
new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
var newPose =
m_poseMeters.exp(
new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;

42
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
View File

@@ -4,31 +4,22 @@
package edu.wpi.first.wpilibj.kinematics;
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
/** Represents the wheel speeds for a differential drive drivetrain. */
@SuppressWarnings("MemberName")
public class DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
/** Speed of the left side of the robot. */
public double leftMetersPerSecond;
/**
* Speed of the right side of the robot.
*/
/** Speed of the right side of the robot. */
public double rightMetersPerSecond;
/**
* Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
*/
public DifferentialDriveWheelSpeeds() {
}
/** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
public DifferentialDriveWheelSpeeds() {}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param leftMetersPerSecond The left speed.
* @param leftMetersPerSecond The left speed.
* @param rightMetersPerSecond The right speed.
*/
public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
@@ -37,12 +28,11 @@ public class DifferentialDriveWheelSpeeds {
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
@@ -50,16 +40,16 @@ public class DifferentialDriveWheelSpeeds {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rightMetersPerSecond =
rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
@Override
public String toString() {
return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
return String.format(
"DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
leftMetersPerSecond, rightMetersPerSecond);
}
}

119
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
View File

@@ -4,33 +4,30 @@
package edu.wpi.first.wpilibj.kinematics;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import org.ejml.simple.SimpleMatrix;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* wheel speeds.
*
* <p>The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion maneuvers.
* <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
* uses the relative locations of the wheels with respect to the center of rotation. The center of
* rotation for inverse kinematics is also variable. This means that you can set your set your
* center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of wheel speeds into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
* <p>Forward kinematics (converting an array of wheel speeds into the overall chassis motion) is
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
* system (more equations than variables), we use a least-squares approximation.
*
* <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
* <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
* chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class MecanumDriveKinematics {
private SimpleMatrix m_inverseKinematics;
@@ -46,19 +43,20 @@ public class MecanumDriveKinematics {
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheelMeters The location of the front-left wheel relative to the
* physical center of the robot.
* @param frontRightWheelMeters The location of the front-right wheel relative to
* the physical center of the robot.
* @param rearLeftWheelMeters The location of the rear-left wheel relative to the
* physical center of the robot.
* @param rearRightWheelMeters The location of the rear-right wheel relative to the
* physical center of the robot.
* @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
* center of the robot.
* @param frontRightWheelMeters The location of the front-right wheel relative to the physical
* center of the robot.
* @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
* of the robot.
* @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
* center of the robot.
*/
public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
Translation2d frontRightWheelMeters,
Translation2d rearLeftWheelMeters,
Translation2d rearRightWheelMeters) {
public MecanumDriveKinematics(
Translation2d frontLeftWheelMeters,
Translation2d frontRightWheelMeters,
Translation2d rearLeftWheelMeters,
Translation2d rearRightWheelMeters) {
m_frontLeftWheelMeters = frontLeftWheelMeters;
m_frontRightWheelMeters = frontRightWheelMeters;
m_rearLeftWheelMeters = rearLeftWheelMeters;
@@ -66,8 +64,8 @@ public class MecanumDriveKinematics {
m_inverseKinematics = new SimpleMatrix(4, 3);
setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
rearLeftWheelMeters, rearRightWheelMeters);
setInverseKinematics(
frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
@@ -77,23 +75,21 @@ public class MecanumDriveKinematics {
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
*
* <p>This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
* <p>This function also supports variable centers of rotation. During normal operations, the
* center of rotation is usually the same as the physical center of the robot; therefore, the
* argument is defaulted to that use case. However, if you wish to change the center of rotation
* for evasive maneuvers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide
* a chassis speed that only has a dtheta component, the robot
* will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user
* input may cause one of the wheel speeds to go above the attainable max velocity. Use
* the {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the center of
* rotation at one corner of the robot and provide a chassis speed that only has a dtheta
* component, the robot will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
* may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
* MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
// We have a new center of rotation. We need to compute the matrix again.
if (!centerOfRotationMeters.equals(m_prevCoR)) {
var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
@@ -106,8 +102,11 @@ public class MecanumDriveKinematics {
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0,
chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
@@ -115,8 +114,7 @@ public class MecanumDriveKinematics {
wheelsMatrix.get(0, 0),
wheelsMatrix.get(1, 0),
wheelsMatrix.get(2, 0),
wheelsMatrix.get(3, 0)
);
wheelsMatrix.get(3, 0));
}
/**
@@ -140,13 +138,18 @@ public class MecanumDriveKinematics {
*/
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
wheelSpeedsMatrix.setColumn(0, 0,
wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
);
wheelSpeedsMatrix.setColumn(
0,
0,
wheelSpeeds.frontLeftMetersPerSecond,
wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond,
wheelSpeeds.rearRightMetersPerSecond);
var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
@@ -158,8 +161,8 @@ public class MecanumDriveKinematics {
* @param rl The location of the rear-left wheel relative to the physical center of the robot.
* @param rr The location of the rear-right wheel relative to the physical center of the robot.
*/
private void setInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) {
private void setInverseKinematics(
Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());

49
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
View File

@@ -4,49 +4,37 @@
package edu.wpi.first.wpilibj.kinematics;
/**
* Represents the motor voltages for a mecanum drive drivetrain.
*/
/** Represents the motor voltages for a mecanum drive drivetrain. */
@SuppressWarnings("MemberName")
public class MecanumDriveMotorVoltages {
/**
* Voltage of the front left motor.
*/
/** Voltage of the front left motor. */
public double frontLeftVoltage;
/**
* Voltage of the front right motor.
*/
/** Voltage of the front right motor. */
public double frontRightVoltage;
/**
* Voltage of the rear left motor.
*/
/** Voltage of the rear left motor. */
public double rearLeftVoltage;
/**
* Voltage of the rear right motor.
*/
/** Voltage of the rear right motor. */
public double rearRightVoltage;
/**
* Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
*/
public MecanumDriveMotorVoltages() {
}
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
public MecanumDriveMotorVoltages() {}
/**
* Constructs a MecanumDriveMotorVoltages.
*
* @param frontLeftVoltage Voltage of the front left motor.
* @param frontLeftVoltage Voltage of the front left motor.
* @param frontRightVoltage Voltage of the front right motor.
* @param rearLeftVoltage Voltage of the rear left motor.
* @param rearRightVoltage Voltage of the rear right motor.
* @param rearLeftVoltage Voltage of the rear left motor.
* @param rearRightVoltage Voltage of the rear right motor.
*/
public MecanumDriveMotorVoltages(double frontLeftVoltage,
double frontRightVoltage,
double rearLeftVoltage,
double rearRightVoltage) {
public MecanumDriveMotorVoltages(
double frontLeftVoltage,
double frontRightVoltage,
double rearLeftVoltage,
double rearRightVoltage) {
this.frontLeftVoltage = frontLeftVoltage;
this.frontRightVoltage = frontRightVoltage;
this.rearLeftVoltage = rearLeftVoltage;
@@ -55,8 +43,9 @@ public class MecanumDriveMotorVoltages {
@Override
public String toString() {
return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
rearLeftVoltage, rearRightVoltage);
return String.format(
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
}
}

74
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
View File

@@ -12,13 +12,11 @@ import edu.wpi.first.wpilibj.geometry.Twist2d;
import edu.wpi.first.wpiutil.WPIUtilJNI;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* mecanum wheel encoders.
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your mecanum wheel encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
@@ -31,12 +29,12 @@ public class MecanumDriveOdometry {
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
Pose2d initialPoseMeters) {
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
@@ -48,7 +46,7 @@ public class MecanumDriveOdometry {
* Constructs a MecanumDriveOdometry object with the default pose at the origin.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
@@ -57,11 +55,11 @@ public class MecanumDriveOdometry {
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
@@ -79,30 +77,31 @@ public class MecanumDriveOdometry {
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
@@ -110,20 +109,17 @@ public class MecanumDriveOdometry {
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the
* current time to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method automatically calculates the current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle,
wheelSpeeds);
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
}
}

86
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -8,44 +8,34 @@ import java.util.stream.DoubleStream;
@SuppressWarnings("MemberName")
public class MecanumDriveWheelSpeeds {
/**
* Speed of the front left wheel.
*/
/** Speed of the front left wheel. */
public double frontLeftMetersPerSecond;
/**
* Speed of the front right wheel.
*/
/** Speed of the front right wheel. */
public double frontRightMetersPerSecond;
/**
* Speed of the rear left wheel.
*/
/** Speed of the rear left wheel. */
public double rearLeftMetersPerSecond;
/**
* Speed of the rear right wheel.
*/
/** Speed of the rear right wheel. */
public double rearRightMetersPerSecond;
/**
* Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
*/
public MecanumDriveWheelSpeeds() {
}
/** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
public MecanumDriveWheelSpeeds() {}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeftMetersPerSecond Speed of the front left wheel.
* @param frontLeftMetersPerSecond Speed of the front left wheel.
* @param frontRightMetersPerSecond Speed of the front right wheel.
* @param rearLeftMetersPerSecond Speed of the rear left wheel.
* @param rearRightMetersPerSecond Speed of the rear right wheel.
* @param rearLeftMetersPerSecond Speed of the rear left wheel.
* @param rearRightMetersPerSecond Speed of the rear right wheel.
*/
public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
double frontRightMetersPerSecond,
double rearLeftMetersPerSecond,
double rearRightMetersPerSecond) {
public MecanumDriveWheelSpeeds(
double frontLeftMetersPerSecond,
double frontRightMetersPerSecond,
double rearLeftMetersPerSecond,
double rearRightMetersPerSecond) {
this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
this.frontRightMetersPerSecond = frontRightMetersPerSecond;
this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
@@ -53,36 +43,44 @@ public class MecanumDriveWheelSpeeds {
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
.max().getAsDouble();
double realMaxSpeed =
DoubleStream.of(
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond)
.max()
.getAsDouble();
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
frontLeftMetersPerSecond =
frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
frontRightMetersPerSecond =
frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearLeftMetersPerSecond =
rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearRightMetersPerSecond =
rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
@Override
public String toString() {
return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
return String.format(
"MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond);
}
}

150
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
View File

@@ -4,37 +4,33 @@
package edu.wpi.first.wpilibj.kinematics;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* module states (speed and angle).
*
* <p>The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion maneuvers.
* <p>The inverse kinematics (converting from a desired chassis velocity to individual module
* states) uses the relative locations of the modules with respect to the center of rotation. The
* center of rotation for inverse kinematics is also variable. This means that you can set your set
* your center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of module states into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
* <p>Forward kinematics (converting an array of module states into the overall chassis motion) is
* performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
* system (more equations than variables), we use a least-squares approximation.
*
* <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
* <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our
* chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class SwerveDriveKinematics {
private final SimpleMatrix m_inverseKinematics;
@@ -45,15 +41,12 @@ public class SwerveDriveKinematics {
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a swerve drive kinematics object. This takes in a variable
* number of wheel locations as Translation2ds. The order in which you pass in
* the wheel locations is the same order that you will receive the module
* states when performing inverse kinematics. It is also expected that you
* pass in the module states in the same order when calling the forward
* kinematics methods.
* Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
* as Translation2ds. The order in which you pass in the wheel locations is the same order that
* you will receive the module states when performing inverse kinematics. It is also expected that
* you pass in the module states in the same order when calling the forward kinematics methods.
*
* @param wheelsMeters The locations of the wheels relative to the physical center
* of the robot.
* @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
*/
public SwerveDriveKinematics(Translation2d... wheelsMeters) {
if (wheelsMeters.length < 2) {
@@ -73,43 +66,50 @@ public class SwerveDriveKinematics {
}
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
* Performs inverse kinematics to return the module states from a desired chassis velocity. This
* method is often used to convert joystick values into module speeds and angles.
*
* <p>This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
* <p>This function also supports variable centers of rotation. During normal operations, the
* center of rotation is usually the same as the physical center of the robot; therefore, the
* argument is defaulted to that use case. However, if you wish to change the center of rotation
* for evasive maneuvers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide
* a chassis speed that only has a dtheta component, the robot
* will rotate around that corner.
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds}
* function to rectify this issue.
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the center of
* rotation at one corner of the robot and provide a chassis speed that only has a dtheta
* component, the robot will rotate around that corner.
* @return An array containing the module states. Use caution because these module states are not
* normalized. Sometimes, a user input may cause one of the module speeds to go above the
* attainable max velocity. Use the {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
* normalizeWheelSpeeds} function to rectify this issue.
*/
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
Translation2d centerOfRotationMeters) {
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
if (!centerOfRotationMeters.equals(m_prevCoR)) {
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
m_inverseKinematics.setRow(
i * 2 + 0,
0, /* Start Data */
1,
0,
-m_modules[i].getY() + centerOfRotationMeters.getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
m_inverseKinematics.setRow(
i * 2 + 1,
0, /* Start Data */
0,
1,
+m_modules[i].getX() - centerOfRotationMeters.getX());
}
m_prevCoR = centerOfRotationMeters;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(0, 0,
chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
@@ -140,22 +140,20 @@ public class SwerveDriveKinematics {
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
* Performs forward kinematics to return the resulting chassis state from the given module states.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed and angle of each module on the robot.
*
* @param wheelStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
* @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from
* respective encoders and gyros. The order of the swerve module states should be same as
* passed into the constructor of this class.
* @return The resulting chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
if (wheelStates.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor"
);
+ "constructor");
}
var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
@@ -166,30 +164,30 @@ public class SwerveDriveKinematics {
}
var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
* this issue, one can "normalize" all the wheel speeds to make sure that all
* requested module speeds are below the absolute threshold, while maintaining
* the ratio of speeds between modules.
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
*/
public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates,
double attainableMaxSpeedMetersPerSecond) {
public static void normalizeWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
* attainableMaxSpeedMetersPerSecond;
moduleState.speedMetersPerSecond =
moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
}

75
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
View File

@@ -12,13 +12,12 @@ import edu.wpi.first.wpilibj.geometry.Twist2d;
import edu.wpi.first.wpiutil.WPIUtilJNI;
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's
* position on the field over a course of a match using readings from your
* swerve drive encoders and swerve azimuth encoders.
* Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your swerve drive encoders and swerve azimuth
* encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class SwerveDriveOdometry {
private final SwerveDriveKinematics m_kinematics;
@@ -31,12 +30,12 @@ public class SwerveDriveOdometry {
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
Pose2d initialPose) {
public SwerveDriveOdometry(
SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
@@ -48,7 +47,7 @@ public class SwerveDriveOdometry {
* Constructs a SwerveDriveOdometry object with the default pose at the origin.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param gyroAngle The angle reported by the gyroscope.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
@@ -57,10 +56,10 @@ public class SwerveDriveOdometry {
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
@@ -79,32 +78,32 @@ public class SwerveDriveOdometry {
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide the states in the
* same order in which you instantiated your SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
SwerveModuleState... moduleStates) {
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
@@ -113,17 +112,15 @@ public class SwerveDriveOdometry {
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the
* current time to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the angular
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method automatically calculates the current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide the states in the
* same order in which you instantiated your SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {

23
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
View File

@@ -6,27 +6,18 @@ package edu.wpi.first.wpilibj.kinematics;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/**
* Represents the state of one swerve module.
*/
/** Represents the state of one swerve module. */
@SuppressWarnings("MemberName")
public class SwerveModuleState implements Comparable<SwerveModuleState> {
/**
* Speed of the wheel of the module.
*/
/** Speed of the wheel of the module. */
public double speedMetersPerSecond;
/**
* Angle of the module.
*/
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
/**
* Constructs a SwerveModuleState with zeros for speed and angle.
*/
public SwerveModuleState() {
}
/** Constructs a SwerveModuleState with zeros for speed and angle. */
public SwerveModuleState() {}
/**
* Constructs a SwerveModuleState.
@@ -54,7 +45,7 @@ public class SwerveModuleState implements Comparable<SwerveModuleState> {
@Override
public String toString() {
return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
angle);
return String.format(
"SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
}
}

96
wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java
View File

@@ -4,12 +4,11 @@
package edu.wpi.first.wpilibj.math;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.Pair;
import org.ejml.simple.SimpleMatrix;
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public final class Discretization {
@@ -20,8 +19,8 @@ public final class Discretization {
/**
* Discretizes the given continuous A matrix.
*
* @param <States> Num representing the number of states.
* @param contA Continuous system matrix.
* @param <States> Num representing the number of states.
* @param contA Continuous system matrix.
* @param dtSeconds Discretization timestep.
* @return the discrete matrix system.
*/
@@ -33,26 +32,25 @@ public final class Discretization {
/**
* Discretizes the given continuous A and B matrices.
*
* <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix
* exponential like in DiscretizeAB(), we take advantage of the structure of the
* block matrix of A and B.
* <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix exponential like in
* DiscretizeAB(), we take advantage of the structure of the block matrix of A and B.
*
* <p>1) The exponential of A*t, which is only N x N, is relatively cheap.
* 2) The upper-right quarter of the (States + Inputs) x (States + Inputs)
* matrix, which we can approximate using a taylor series to several terms
* and still be substantially cheaper than taking the big exponential.
* <p>1) The exponential of A*t, which is only N x N, is relatively cheap. 2) The upper-right
* quarter of the (States + Inputs) x (States + Inputs) matrix, which we can approximate using a
* taylor series to several terms and still be substantially cheaper than taking the big
* exponential.
*
* @param states Nat representing the states of the system.
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param states Nat representing the states of the system.
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param dtseconds Discretization timestep.
*/
public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
Matrix<States, Inputs>>
discretizeABTaylor(Nat<States> states,
Matrix<States, States> contA,
Matrix<States, Inputs> contB,
double dtseconds) {
public static <States extends Num, Inputs extends Num>
Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeABTaylor(
Nat<States> states,
Matrix<States, States> contA,
Matrix<States, Inputs> contB,
double dtseconds) {
Matrix<States, States> lastTerm = Matrix.eye(states);
double lastCoeff = dtseconds;
@@ -76,29 +74,25 @@ public final class Discretization {
/**
* Discretizes the given continuous A and Q matrices.
*
* <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which
* is expensive), we take advantage of the structure of the block matrix of A
* and Q.
* <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
* we take advantage of the structure of the block matrix of A and Q.
*
* <p>The exponential of A*t, which is only N x N, is relatively cheap.
* 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
* using a taylor series to several terms and still be substantially cheaper
* than taking the big exponential.
* <p>The exponential of A*t, which is only N x N, is relatively cheap. 2) The upper-right quarter
* of the 2N x 2N matrix, which we can approximate using a taylor series to several terms and
* still be substantially cheaper than taking the big exponential.
*
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param dtSeconds Discretization timestep.
* @return a pair representing the discrete system matrix and process noise covariance matrix.
*/
@SuppressWarnings("LocalVariableName")
public static <States extends Num> Pair<Matrix<States, States>,
Matrix<States, States>> discretizeAQTaylor(Matrix<States, States> contA,
Matrix<States, States> contQ,
double dtSeconds) {
public static <States extends Num>
Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
Matrix<States, States> lastTerm = Q.copy();
double lastCoeff = dtSeconds;
@@ -126,11 +120,11 @@ public final class Discretization {
}
/**
* Returns a discretized version of the provided continuous measurement noise
* covariance matrix. Note that dt=0.0 divides R by zero.
* Returns a discretized version of the provided continuous measurement noise covariance matrix.
* Note that dt=0.0 divides R by zero.
*
* @param <O> Nat representing the number of outputs.
* @param R Continuous measurement noise covariance matrix.
* @param <O> Nat representing the number of outputs.
* @param R Continuous measurement noise covariance matrix.
* @param dtSeconds Discretization timestep.
* @return Discretized version of the provided continuous measurement noise covariance matrix.
*/
@@ -141,19 +135,17 @@ public final class Discretization {
/**
* Discretizes the given continuous A and B matrices.
*
* @param <States> Nat representing the states of the system.
* @param <Inputs> Nat representing the inputs to the system.
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param <States> Nat representing the states of the system.
* @param <Inputs> Nat representing the inputs to the system.
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param dtSeconds Discretization timestep.
* @return a Pair representing discA and diskB.
*/
@SuppressWarnings("LocalVariableName")
public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
Matrix<States, Inputs>> discretizeAB(
Matrix<States, States> contA,
Matrix<States, Inputs> contB,
double dtSeconds) {
public static <States extends Num, Inputs extends Num>
Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
var scaledA = contA.times(dtSeconds);
var scaledB = contB.times(dtSeconds);
@@ -163,10 +155,10 @@ public final class Discretization {
Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
var Mdisc = Mcont.exp();
var discA = new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(),
contB.getNumRows()));
var discB = new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(),
contB.getNumCols()));
var discA =
new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(), contB.getNumRows()));
var discB =
new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(), contB.getNumCols()));
discA.extractFrom(0, 0, Mdisc);
discB.extractFrom(0, contB.getNumRows(), Mdisc);

117
wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java
View File

@@ -4,10 +4,6 @@
package edu.wpi.first.wpilibj.math;
import java.util.Random;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpiutil.math.MathUtil;
@@ -18,6 +14,8 @@ import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N3;
import edu.wpi.first.wpiutil.math.numbers.N4;
import java.util.Random;
import org.ejml.simple.SimpleMatrix;
@SuppressWarnings("ParameterName")
public final class StateSpaceUtil {
@@ -26,23 +24,20 @@ public final class StateSpaceUtil {
}
/**
* Creates a covariance matrix from the given vector for use with Kalman
* filters.
* Creates a covariance matrix from the given vector for use with Kalman filters.
*
* <p>Each element is squared and placed on the covariance matrix diagonal.
*
* @param <States> Num representing the states of the system.
* @param states A Nat representing the states of the system.
* @param stdDevs For a Q matrix, its elements are the standard deviations of
* each state from how the model behaves. For an R matrix, its
* elements are the standard deviations for each output
* measurement.
* @param <States> Num representing the states of the system.
* @param states A Nat representing the states of the system.
* @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how
* the model behaves. For an R matrix, its elements are the standard deviations for each
* output measurement.
* @return Process noise or measurement noise covariance matrix.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
Nat<States> states, Matrix<States, N1> stdDevs
) {
Nat<States> states, Matrix<States, N1> stdDevs) {
var result = new Matrix<>(states, states);
for (int i = 0; i < states.getNum(); i++) {
result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
@@ -51,17 +46,15 @@ public final class StateSpaceUtil {
}
/**
* Creates a vector of normally distributed white noise with the given noise
* intensities for each element.
* Creates a vector of normally distributed white noise with the given noise intensities for each
* element.
*
* @param <N> Num representing the dimensionality of the noise vector to create.
* @param stdDevs A matrix whose elements are the standard deviations of each
* element of the noise vector.
* @param <N> Num representing the dimensionality of the noise vector to create.
* @param stdDevs A matrix whose elements are the standard deviations of each element of the noise
* vector.
* @return White noise vector.
*/
public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(
Matrix<N, N1> stdDevs
) {
public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(Matrix<N, N1> stdDevs) {
var rand = new Random();
Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
@@ -74,19 +67,18 @@ public final class StateSpaceUtil {
/**
* Creates a cost matrix from the given vector for use with LQR.
*
* <p>The cost matrix is constructed using Bryson's rule. The inverse square of
* each element in the input is taken and placed on the cost matrix diagonal.
* <p>The cost matrix is constructed using Bryson's rule. The inverse square of each element in
* the input is taken and placed on the cost matrix diagonal.
*
* @param <States> Nat representing the states of the system.
* @param costs An array. For a Q matrix, its elements are the maximum allowed
* excursions of the states from the reference. For an R matrix,
* its elements are the maximum allowed excursions of the control
* inputs from no actuation.
* @param <States> Nat representing the states of the system.
* @param costs An array. For a Q matrix, its elements are the maximum allowed excursions of the
* states from the reference. For an R matrix, its elements are the maximum allowed excursions
* of the control inputs from no actuation.
* @return State excursion or control effort cost matrix.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num> Matrix<States, States>
makeCostMatrix(Matrix<States, N1> costs) {
public static <States extends Num> Matrix<States, States> makeCostMatrix(
Matrix<States, N1> costs) {
Matrix<States, States> result =
new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
result.fill(0.0);
@@ -101,21 +93,20 @@ public final class StateSpaceUtil {
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank(lambda * I - A, B) %3C n where n is number of states.
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
* %3C n where n is number of states.
*
* @param <States> Num representing the size of A.
* @param <Inputs> Num representing the columns of B.
* @param A System matrix.
* @param B Input matrix.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
@SuppressWarnings("MethodTypeParameterName")
public static <States extends Num, Inputs extends Num> boolean isStabilizable(
Matrix<States, States> A, Matrix<States, Inputs> B) {
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(),
A.getData(), B.getData());
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
}
/**
@@ -125,32 +116,24 @@ public final class StateSpaceUtil {
* @return The given pose in vector form, with the third element, theta, in radians.
*/
public static Matrix<N3, N1> poseToVector(Pose2d pose) {
return VecBuilder.fill(
pose.getX(),
pose.getY(),
pose.getRotation().getRadians()
);
return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
}
/**
* Clamp the input u to the min and max.
*
* @param u The input to clamp.
* @param u The input to clamp.
* @param umin The minimum input magnitude.
* @param umax The maximum input magnitude.
* @param <I> The number of inputs.
* @return The clamped input.
* @param <I> The number of inputs.
* @return The clamped input.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(Matrix<I, N1> u,
Matrix<I, N1> umin,
Matrix<I, N1> umax) {
public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
for (int i = 0; i < u.getNumRows(); i++) {
result.set(i, 0, MathUtil.clamp(
u.get(i, 0),
umin.get(i, 0),
umax.get(i, 0)));
result.set(i, 0, MathUtil.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
}
return result;
}
@@ -159,13 +142,13 @@ public final class StateSpaceUtil {
* Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
* differential drivetrains.
*
* @param u The input vector.
* @param u The input vector.
* @param maxMagnitude The maximum magnitude any input can have.
* @param <I> The number of inputs.
* @param <I> The number of inputs.
* @return The normalizedInput
*/
public static <I extends Num> Matrix<I, N1> normalizeInputVector(Matrix<I, N1> u,
double maxMagnitude) {
public static <I extends Num> Matrix<I, N1> normalizeInputVector(
Matrix<I, N1> u, double maxMagnitude) {
double maxValue = u.maxAbs();
boolean isCapped = maxValue > maxMagnitude;
@@ -176,18 +159,17 @@ public final class StateSpaceUtil {
}
/**
* Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)],
* where theta is in radians.
* Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)], where theta is in
* radians.
*
* @param pose A pose to convert to a vector.
*/
public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getCos(),
pose.getRotation().getSin()
);
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getCos(),
pose.getRotation().getSin());
}
/**
@@ -198,9 +180,8 @@ public final class StateSpaceUtil {
*/
public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getRadians()
);
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getRotation().getRadians());
}
}

57
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
View File

@@ -11,22 +11,20 @@ public class CubicHermiteSpline extends Spline {
private final SimpleMatrix m_coefficients;
/**
* Constructs a cubic hermite spline with the specified control vectors. Each
* control vector contains info about the location of the point and its first
* derivative.
* Constructs a cubic hermite spline with the specified control vectors. Each control vector
* contains info about the location of the point and its first derivative.
*
* @param xInitialControlVector The control vector for the initial point in
* the x dimension.
* @param xFinalControlVector The control vector for the final point in
* the x dimension.
* @param yInitialControlVector The control vector for the initial point in
* the y dimension.
* @param yFinalControlVector The control vector for the final point in
* the y dimension.
* @param xInitialControlVector The control vector for the initial point in the x dimension.
* @param xFinalControlVector The control vector for the final point in the x dimension.
* @param yInitialControlVector The control vector for the initial point in the y dimension.
* @param yFinalControlVector The control vector for the final point in the y dimension.
*/
@SuppressWarnings("ParameterName")
public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
double[] yInitialControlVector, double[] yFinalControlVector) {
public CubicHermiteSpline(
double[] xInitialControlVector,
double[] xFinalControlVector,
double[] yInitialControlVector,
double[] yFinalControlVector) {
super(3);
// Populate the coefficients for the actual spline equations.
@@ -63,7 +61,6 @@ public class CubicHermiteSpline extends Spline {
m_coefficients.set(4, i, m_coefficients.get(2, i) * (2 - i));
m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
}
}
/**
@@ -83,30 +80,38 @@ public class CubicHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
+2.0, +1.0, -2.0, +1.0,
-3.0, -2.0, +3.0, -1.0,
+0.0, +1.0, +0.0, +0.0,
+1.0, +0.0, +0.0, +0.0
});
hermiteBasis =
new SimpleMatrix(
4,
4,
true,
new double[] {
+2.0, +1.0, -2.0, +1.0, -3.0, -2.0, +3.0, -1.0, +0.0, +1.0, +0.0, +0.0, +1.0, +0.0,
+0.0, +0.0
});
}
return hermiteBasis;
}
/**
* Returns the control vector for each dimension as a matrix from the
* user-provided arrays in the constructor.
* Returns the control vector for each dimension as a matrix from the user-provided arrays in the
* constructor.
*
* @param initialVector The control vector for the initial point.
* @param finalVector The control vector for the final point.
* @param finalVector The control vector for the final point.
* @return The control vector matrix for a dimension.
*/
private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
if (initialVector.length != 2 || finalVector.length != 2) {
throw new IllegalArgumentException("Size of vectors must be 2");
}
return new SimpleMatrix(4, 1, true, new double[]{
initialVector[0], initialVector[1],
finalVector[0], finalVector[1]});
return new SimpleMatrix(
4,
1,
true,
new double[] {
initialVector[0], initialVector[1],
finalVector[0], finalVector[1]
});
}
}

10
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
View File

@@ -6,9 +6,7 @@ package edu.wpi.first.wpilibj.spline;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* Represents a pair of a pose and a curvature.
*/
/** Represents a pair of a pose and a curvature. */
@SuppressWarnings("MemberName")
public class PoseWithCurvature {
// Represents the pose.
@@ -20,7 +18,7 @@ public class PoseWithCurvature {
/**
* Constructs a PoseWithCurvature.
*
* @param poseMeters The pose.
* @param poseMeters The pose.
* @param curvatureRadPerMeter The curvature.
*/
public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
@@ -28,9 +26,7 @@ public class PoseWithCurvature {
this.curvatureRadPerMeter = curvatureRadPerMeter;
}
/**
* Constructs a PoseWithCurvature with default values.
*/
/** Constructs a PoseWithCurvature with default values. */
public PoseWithCurvature() {
poseMeters = new Pose2d();
}

59
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
View File

@@ -11,22 +11,20 @@ public class QuinticHermiteSpline extends Spline {
private final SimpleMatrix m_coefficients;
/**
* Constructs a quintic hermite spline with the specified control vectors.
* Each control vector contains into about the location of the point, its
* first derivative, and its second derivative.
* Constructs a quintic hermite spline with the specified control vectors. Each control vector
* contains into about the location of the point, its first derivative, and its second derivative.
*
* @param xInitialControlVector The control vector for the initial point in
* the x dimension.
* @param xFinalControlVector The control vector for the final point in
* the x dimension.
* @param yInitialControlVector The control vector for the initial point in
* the y dimension.
* @param yFinalControlVector The control vector for the final point in
* the y dimension.
* @param xInitialControlVector The control vector for the initial point in the x dimension.
* @param xFinalControlVector The control vector for the final point in the x dimension.
* @param yInitialControlVector The control vector for the initial point in the y dimension.
* @param yFinalControlVector The control vector for the final point in the y dimension.
*/
@SuppressWarnings("ParameterName")
public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
double[] yInitialControlVector, double[] yFinalControlVector) {
public QuinticHermiteSpline(
double[] xInitialControlVector,
double[] xFinalControlVector,
double[] yInitialControlVector,
double[] yFinalControlVector) {
super(5);
// Populate the coefficients for the actual spline equations.
@@ -82,32 +80,39 @@ public class QuinticHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+01.0, +00.0, +00.0, +00.0, +00.0, +00.0
});
hermiteBasis =
new SimpleMatrix(
6,
6,
true,
new double[] {
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
});
}
return hermiteBasis;
}
/**
* Returns the control vector for each dimension as a matrix from the
* user-provided arrays in the constructor.
* Returns the control vector for each dimension as a matrix from the user-provided arrays in the
* constructor.
*
* @param initialVector The control vector for the initial point.
* @param finalVector The control vector for the final point.
* @param finalVector The control vector for the final point.
* @return The control vector matrix for a dimension.
*/
private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
if (initialVector.length != 3 || finalVector.length != 3) {
throw new IllegalArgumentException("Size of vectors must be 3");
}
return new SimpleMatrix(6, 1, true, new double[]{
initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]});
return new SimpleMatrix(
6,
1,
true,
new double[] {
initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]
});
}
}

24
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
View File

@@ -4,17 +4,12 @@
package edu.wpi.first.wpilibj.spline;
import java.util.Arrays;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Arrays;
import org.ejml.simple.SimpleMatrix;
/**
* Represents a two-dimensional parametric spline that interpolates between two
* points.
*/
/** Represents a two-dimensional parametric spline that interpolates between two points. */
public abstract class Spline {
private final int m_degree;
@@ -79,20 +74,16 @@ public abstract class Spline {
}
// Find the curvature.
final double curvature =
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
return new PoseWithCurvature(
new Pose2d(x, y, new Rotation2d(dx, dy)),
curvature
);
return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
}
/**
* Represents a control vector for a spline.
*
* <p>Each element in each array represents the value of the derivative at the index. For
* example, the value of x[2] is the second derivative in the x dimension.
* <p>Each element in each array represents the value of the derivative at the index. For example,
* the value of x[2] is the second derivative in the x dimension.
*/
@SuppressWarnings("MemberName")
public static class ControlVector {
@@ -101,6 +92,7 @@ public abstract class Spline {
/**
* Instantiates a control vector.
*
* @param x The x dimension of the control vector.
* @param y The y dimension of the control vector.
*/

145
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
View File

@@ -4,31 +4,25 @@
package edu.wpi.first.wpilibj.spline;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.Arrays;
import java.util.List;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
public final class SplineHelper {
/**
* Private constructor because this is a utility class.
*/
private SplineHelper() {
}
/** Private constructor because this is a utility class. */
private SplineHelper() {}
/**
* Returns 2 cubic control vectors from a set of exterior waypoints and
* interior translations.
* Returns 2 cubic control vectors from a set of exterior waypoints and interior translations.
*
* @param start The starting pose.
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @param end The ending pose.
* @return 2 cubic control vectors.
*/
public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
) {
Pose2d start, Translation2d[] interiorWaypoints, Pose2d end) {
// Generate control vectors from poses.
Spline.ControlVector initialCV;
Spline.ControlVector endCV;
@@ -41,11 +35,11 @@ public final class SplineHelper {
} else {
double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
initialCV = getCubicControlVector(scalar, start);
scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
* 1.2;
scalar =
end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) * 1.2;
endCV = getCubicControlVector(scalar, end);
}
return new Spline.ControlVector[]{initialCV, endCV};
return new Spline.ControlVector[] {initialCV, endCV};
}
/**
@@ -67,27 +61,29 @@ public final class SplineHelper {
var controlVecA = getQuinticControlVector(scalar, p0);
var controlVecB = getQuinticControlVector(scalar, p1);
splines[i]
= new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
splines[i] =
new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
}
return splines;
}
/**
* Returns a set of cubic splines corresponding to the provided control vectors. The
* user is free to set the direction of the start and end point. The
* directions for the middle waypoints are determined automatically to ensure
* continuous curvature throughout the path.
* Returns a set of cubic splines corresponding to the provided control vectors. The user is free
* to set the direction of the start and end point. The directions for the middle waypoints are
* determined automatically to ensure continuous curvature throughout the path.
*
* @param start The starting control vector.
* @param waypoints The middle waypoints. This can be left blank if you only
* wish to create a path with two waypoints.
* @param end The ending control vector.
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints and control vectors.
* @param start The starting control vector.
* @param waypoints The middle waypoints. This can be left blank if you only wish to create a path
* with two waypoints.
* @param end The ending control vector.
* @return A vector of cubic hermite splines that interpolate through the provided waypoints and
* control vectors.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"})
@SuppressWarnings({
"LocalVariableName",
"PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"
})
public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
@@ -151,10 +147,12 @@ public final class SplineHelper {
}
}
dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
- newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
- newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
dx[dx.length - 1] =
3 * (newWaypts[newWaypts.length - 1].getX() - newWaypts[newWaypts.length - 3].getX())
- xFinal[1];
dy[dy.length - 1] =
3 * (newWaypts[newWaypts.length - 1].getY() - newWaypts[newWaypts.length - 3].getY())
- yFinal[1];
// Compute solution to tridiagonal system
thomasAlgorithm(a, b, c, dx, fx);
@@ -171,45 +169,44 @@ public final class SplineHelper {
newFy[newFy.length - 1] = yFinal[1];
for (int i = 0; i < newFx.length - 1; i++) {
splines[i] = new CubicHermiteSpline(
new double[]{newWaypts[i].getX(), newFx[i]},
new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
new double[]{newWaypts[i].getY(), newFy[i]},
new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
);
splines[i] =
new CubicHermiteSpline(
new double[] {newWaypts[i].getX(), newFx[i]},
new double[] {newWaypts[i + 1].getX(), newFx[i + 1]},
new double[] {newWaypts[i].getY(), newFy[i]},
new double[] {newWaypts[i + 1].getY(), newFy[i + 1]});
}
} else if (waypoints.length == 1) {
final var xDeriv = (3 * (xFinal[0]
- xInitial[0])
- xFinal[1] - xInitial[1])
/ 4.0;
final var yDeriv = (3 * (yFinal[0]
- yInitial[0])
- yFinal[1] - yInitial[1])
/ 4.0;
final var xDeriv = (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
final var yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
double[] midXControlVector = {waypoints[0].getX(), xDeriv};
double[] midYControlVector = {waypoints[0].getY(), yDeriv};
splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
yInitial, midYControlVector);
splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
midYControlVector, yFinal);
splines[0] =
new CubicHermiteSpline(
xInitial, midXControlVector,
yInitial, midYControlVector);
splines[1] =
new CubicHermiteSpline(
midXControlVector, xFinal,
midYControlVector, yFinal);
} else {
splines[0] = new CubicHermiteSpline(xInitial, xFinal,
yInitial, yFinal);
splines[0] =
new CubicHermiteSpline(
xInitial, xFinal,
yInitial, yFinal);
}
return splines;
}
/**
* Returns a set of quintic splines corresponding to the provided control vectors.
* The user is free to set the direction of all control vectors. Continuous
* curvature is guaranteed throughout the path.
* Returns a set of quintic splines corresponding to the provided control vectors. The user is
* free to set the direction of all control vectors. Continuous curvature is guaranteed throughout
* the path.
*
* @param controlVectors The control vectors.
* @return A vector of quintic hermite splines that interpolate through the
* provided waypoints.
* @return A vector of quintic hermite splines that interpolate through the provided waypoints.
*/
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
@@ -220,8 +217,10 @@ public final class SplineHelper {
var xFinal = controlVectors[i + 1].x;
var yInitial = controlVectors[i].y;
var yFinal = controlVectors[i + 1].y;
splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
yInitial, yFinal);
splines[i] =
new QuinticHermiteSpline(
xInitial, xFinal,
yInitial, yFinal);
}
return splines;
}
@@ -229,15 +228,15 @@ public final class SplineHelper {
/**
* Thomas algorithm for solving tridiagonal systems Af = d.
*
* @param a the values of A above the diagonal
* @param b the values of A on the diagonal
* @param c the values of A below the diagonal
* @param d the vector on the rhs
* @param a the values of A above the diagonal
* @param b the values of A on the diagonal
* @param c the values of A below the diagonal
* @param d the vector on the rhs
* @param solutionVector the unknown (solution) vector, modified in-place
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
private static void thomasAlgorithm(double[] a, double[] b,
double[] c, double[] d, double[] solutionVector) {
private static void thomasAlgorithm(
double[] a, double[] b, double[] c, double[] d, double[] solutionVector) {
int N = d.length;
double[] cStar = new double[N];
@@ -263,15 +262,13 @@ public final class SplineHelper {
private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
return new Spline.ControlVector(
new double[]{point.getX(), scalar * point.getRotation().getCos()},
new double[]{point.getY(), scalar * point.getRotation().getSin()}
);
new double[] {point.getX(), scalar * point.getRotation().getCos()},
new double[] {point.getY(), scalar * point.getRotation().getSin()});
}
private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
return new Spline.ControlVector(
new double[]{point.getX(), scalar * point.getRotation().getCos(), 0.0},
new double[]{point.getY(), scalar * point.getRotation().getSin(), 0.0}
);
new double[] {point.getX(), scalar * point.getRotation().getCos(), 0.0},
new double[] {point.getY(), scalar * point.getRotation().getSin(), 0.0});
}
}

50
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
View File

@@ -32,19 +32,17 @@ import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.List;
/**
* Class used to parameterize a spline by its arc length.
*/
/** Class used to parameterize a spline by its arc length. */
public final class SplineParameterizer {
private static final double kMaxDx = 0.127;
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
/**
* A malformed spline does not actually explode the LIFO stack size. Instead, the stack size
* stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must
* count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting
* this maximum should definitely indicate something has gone wrong.
* A malformed spline does not actually explode the LIFO stack size. Instead, the stack size stays
* at a relatively small number (e.g. 30) and never decreases. Because of this, we must count
* iterations. Even long, complex paths don't usually go over 300 iterations, so hitting this
* maximum should definitely indicate something has gone wrong.
*/
private static final int kMaxIterations = 5000;
@@ -71,35 +69,32 @@ public final class SplineParameterizer {
}
}
/**
* Private constructor because this is a utility class.
*/
private SplineParameterizer() {
}
/** Private constructor because this is a utility class. */
private SplineParameterizer() {}
/**
* Parameterizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
* Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
* dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
* @return A list of poses and curvatures that represents various points on the spline.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
* with approximately opposing headings)
*/
public static List<PoseWithCurvature> parameterize(Spline spline) {
return parameterize(spline, 0.0, 1.0);
}
/**
* Parameterizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
* Parameterizes the spline. This method breaks up the spline into various arcs until their dx,
* dy, and dtheta are within specific tolerances.
*
* @param spline The spline to parameterize.
* @param t0 Starting internal spline parameter. It is recommended to use 0.0.
* @param t1 Ending internal spline parameter. It is recommended to use 1.0.
* @return A list of poses and curvatures that represents various points on the spline.
* @param t0 Starting internal spline parameter. It is recommended to use 0.0.
* @param t1 Ending internal spline parameter. It is recommended to use 1.0.
* @return A list of poses and curvatures that represents various points on the spline.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
* with approximately opposing headings)
*/
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
@@ -124,11 +119,9 @@ public final class SplineParameterizer {
end = spline.getPoint(current.t1);
final var twist = start.poseMeters.log(end.poseMeters);
if (
Math.abs(twist.dy) > kMaxDy
if (Math.abs(twist.dy) > kMaxDy
|| Math.abs(twist.dx) > kMaxDx
|| Math.abs(twist.dtheta) > kMaxDtheta
) {
|| Math.abs(twist.dtheta) > kMaxDtheta) {
stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
} else {
@@ -138,10 +131,9 @@ public final class SplineParameterizer {
iterations++;
if (iterations >= kMaxIterations) {
throw new MalformedSplineException(
"Could not parameterize a malformed spline. "
+ "This means that you probably had two or more adjacent waypoints that were very close "
+ "together with headings in opposing directions."
);
"Could not parameterize a malformed spline. This means that you probably had two or "
+ " more adjacent waypoints that were very close together with headings in "
+ "opposing directions.");
}
}

57
wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java
View File

@@ -10,44 +10,38 @@ import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
@SuppressWarnings("ClassTypeParameterName")
public class LinearSystem<States extends Num, Inputs extends Num,
Outputs extends Num> {
public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
/**
* Continuous system matrix.
*/
/** Continuous system matrix. */
@SuppressWarnings("MemberName")
private final Matrix<States, States> m_A;
/**
* Continuous input matrix.
*/
/** Continuous input matrix. */
@SuppressWarnings("MemberName")
private final Matrix<States, Inputs> m_B;
/**
* Output matrix.
*/
/** Output matrix. */
@SuppressWarnings("MemberName")
private final Matrix<Outputs, States> m_C;
/**
* Feedthrough matrix.
*/
/** 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.
* @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) {
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;
@@ -138,17 +132,16 @@ public class LinearSystem<States extends Num, Inputs extends Num,
/**
* 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.
* <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 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) {
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));
@@ -157,23 +150,21 @@ public class LinearSystem<States extends Num, Inputs extends Num,
/**
* Computes the new y given the control input.
*
* <p>This is used by state observers directly to run updates based on state
* estimate.
* <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) {
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());
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());
}
}

166
wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
View File

@@ -4,11 +4,6 @@
package edu.wpi.first.wpilibj.system;
import java.util.function.Function;
import org.ejml.MatrixDimensionException;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
@@ -16,24 +11,25 @@ import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.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.
* 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 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> {
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;
@@ -42,81 +38,91 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
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.
* 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 plant State-space plant.
* @param controller State-space controller.
* @param observer State-space observer.
* @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,
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.
* 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 plant State-space plant.
* @param controller State-space controller.
* @param observer State-space observer.
* @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);
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.
* 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.
* @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));
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.
* 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 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) {
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;
@@ -157,7 +163,7 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
/**
* Set an element of the initial state estimate x-hat.
*
* @param row Row of x-hat.
* @param row Row of x-hat.
* @param value Value for element of x-hat.
*/
public void setXHat(int row, double value) {
@@ -199,10 +205,11 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
*/
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));
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));
}
@@ -254,9 +261,9 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
}
/**
* 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.
* 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.
*/
@@ -288,15 +295,14 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
/**
* 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.
*/
/** Set the clamping function used to clamp inputs. */
public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
this.m_clampFunction = clampFunction;
}
@@ -312,18 +318,19 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
}
/**
* Sets new controller output, projects model forward, and runs observer
* prediction.
* 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.
* <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)));
var u =
clampInput(
m_controller
.calculate(getObserver().getXhat(), m_nextR)
.plus(m_feedforward.calculate(m_nextR)));
getObserver().predict(u, dtSeconds);
}
@@ -331,10 +338,9 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
* Clamp the input u to the min and max.
*
* @param unclampedU The input to clamp.
* @return The clamped input.
* @return The clamped input.
*/
public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
return m_clampFunction.apply(unclampedU);
}
}

62
wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java
View File

@@ -4,13 +4,12 @@
package edu.wpi.first.wpilibj.system;
import java.util.function.BiFunction;
import java.util.function.Function;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.Function;
public final class NumericalJacobian {
private NumericalJacobian() {
@@ -22,23 +21,22 @@ public final class NumericalJacobian {
/**
* Computes the numerical Jacobian with respect to x for f(x).
*
* @param <Rows> Number of rows in the result of 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.
* @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(
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
) {
Matrix<Cols, N1> x) {
var result = new Matrix<>(rows, cols);
for (int i = 0; i < cols.getNum(); i++) {
@@ -58,15 +56,15 @@ public final class NumericalJacobian {
/**
* 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 <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.
* @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"})
@@ -76,8 +74,7 @@ public final class NumericalJacobian {
Nat<States> states,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u
) {
Matrix<Inputs, N1> u) {
return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
}
@@ -86,23 +83,22 @@ public final class NumericalJacobian {
*
* @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.
* @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(
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
) {
Matrix<Inputs, N1> u) {
return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
}
}

51
wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
View File

@@ -4,13 +4,12 @@
package edu.wpi.first.wpilibj.system;
import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
public final class RungeKutta {
private RungeKutta() {
@@ -20,17 +19,13 @@ public final class RungeKutta {
/**
* 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 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 rungeKutta(
DoubleFunction<Double> f,
double x,
double dtSeconds
) {
public static double rungeKutta(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);
@@ -42,17 +37,15 @@ public final class RungeKutta {
/**
* 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 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 rungeKutta(
BiFunction<Double, Double, Double> f,
double x, Double u, double dtSeconds
) {
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);
@@ -64,18 +57,20 @@ public final class RungeKutta {
/**
* 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 <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> rungeKutta(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x, Matrix<Inputs, N1> u, double dtSeconds) {
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
final var halfDt = 0.5 * dtSeconds;
Matrix<States, N1> k1 = f.apply(x, u);
@@ -88,16 +83,15 @@ public final class RungeKutta {
/**
* 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 <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> rungeKutta(
Function<Matrix<States, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x, double dtSeconds) {
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);
@@ -106,5 +100,4 @@ public final class RungeKutta {
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));
}
}

98
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
View File

@@ -6,37 +6,38 @@ package edu.wpi.first.wpilibj.system.plant;
import edu.wpi.first.wpilibj.util.Units;
/**
* Holds the constants for a DC motor.
*/
/** Holds the constants for a DC motor. */
public class DCMotor {
public final double m_nominalVoltageVolts;
public final double m_stallTorqueNewtonMeters;
public final double m_stallCurrentAmps;
public final double m_freeCurrentAmps;
public final double m_freeSpeedRadPerSec;
@SuppressWarnings("MemberName")
public final double m_rOhms;
@SuppressWarnings("MemberName")
public final double m_KvRadPerSecPerVolt;
@SuppressWarnings("MemberName")
public final double m_KtNMPerAmp;
/**
* Constructs a DC motor.
*
* @param nominalVoltageVolts Voltage at which the motor constants were measured.
* @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 stallCurrentAmps Current draw when stalled.
* @param freeCurrentAmps Current draw under no load.
* @param freeSpeedRadPerSec Angular velocity under no load.
*/
public DCMotor(double nominalVoltageVolts,
double stallTorqueNewtonMeters,
double stallCurrentAmps,
double freeCurrentAmps,
double freeSpeedRadPerSec) {
public DCMotor(
double nominalVoltageVolts,
double stallTorqueNewtonMeters,
double stallCurrentAmps,
double freeCurrentAmps,
double freeSpeedRadPerSec) {
this.m_nominalVoltageVolts = nominalVoltageVolts;
this.m_stallTorqueNewtonMeters = stallTorqueNewtonMeters;
this.m_stallCurrentAmps = stallCurrentAmps;
@@ -44,8 +45,8 @@ public class DCMotor {
this.m_freeSpeedRadPerSec = freeSpeedRadPerSec;
this.m_rOhms = nominalVoltageVolts / stallCurrentAmps;
this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms
* freeCurrentAmps);
this.m_KvRadPerSecPerVolt =
freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms * freeCurrentAmps);
this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps;
}
@@ -53,11 +54,11 @@ public class DCMotor {
* Estimate the current being drawn by this motor.
*
* @param speedRadiansPerSec The speed of the rotor.
* @param voltageInputVolts The input voltage.
* @param voltageInputVolts The input voltage.
*/
public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec
+ 1.0 / m_rOhms * voltageInputVolts;
+ 1.0 / m_rOhms * voltageInputVolts;
}
/**
@@ -66,9 +67,8 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getCIM(int numMotors) {
return new DCMotor(12,
2.42 * numMotors, 133,
2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
return new DCMotor(
12, 2.42 * numMotors, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
}
/**
@@ -77,9 +77,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getVex775Pro(int numMotors) {
return gearbox(new DCMotor(12,
0.71, 134,
0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors);
return gearbox(
new DCMotor(12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)),
numMotors);
}
/**
@@ -88,8 +88,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getNEO(int numMotors) {
return gearbox(new DCMotor(12, 2.6,
105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors);
return gearbox(
new DCMotor(12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)),
numMotors);
}
/**
@@ -98,8 +99,8 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getMiniCIM(int numMotors) {
return gearbox(new DCMotor(12, 1.41, 89, 3,
Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
return gearbox(
new DCMotor(12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
}
/**
@@ -108,8 +109,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getBag(int numMotors) {
return gearbox(new DCMotor(12, 0.43, 53, 1.8,
Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors);
return gearbox(
new DCMotor(12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180)),
numMotors);
}
/**
@@ -118,8 +120,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getAndymarkRs775_125(int numMotors) {
return gearbox(new DCMotor(12, 0.28, 18, 1.6,
Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors);
return gearbox(
new DCMotor(12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0)),
numMotors);
}
/**
@@ -128,8 +131,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getBanebotsRs775(int numMotors) {
return gearbox(new DCMotor(12, 0.72, 97, 2.7,
Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors);
return gearbox(
new DCMotor(12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0)),
numMotors);
}
/**
@@ -138,8 +142,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getAndymark9015(int numMotors) {
return gearbox(new DCMotor(12, 0.36, 71, 3.7,
Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors);
return gearbox(
new DCMotor(12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0)),
numMotors);
}
/**
@@ -148,8 +153,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getBanebotsRs550(int numMotors) {
return gearbox(new DCMotor(12, 0.38, 84, 0.4,
Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors);
return gearbox(
new DCMotor(12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0)),
numMotors);
}
/**
@@ -158,8 +164,9 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getNeo550(int numMotors) {
return gearbox(new DCMotor(12, 0.97, 100, 1.4,
Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors);
return gearbox(
new DCMotor(12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0)),
numMotors);
}
/**
@@ -168,12 +175,17 @@ public class DCMotor {
* @param numMotors Number of motors in the gearbox.
*/
public static DCMotor getFalcon500(int numMotors) {
return gearbox(new DCMotor(12, 4.69, 257, 1.5,
Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors);
return gearbox(
new DCMotor(12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0)),
numMotors);
}
private static DCMotor gearbox(DCMotor motor, double numMotors) {
return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors,
motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec);
return new DCMotor(
motor.m_nominalVoltageVolts,
motor.m_stallTorqueNewtonMeters * numMotors,
motor.m_stallCurrentAmps,
motor.m_freeCurrentAmps,
motor.m_freeSpeedRadPerSec);
}
}

194
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
View File

@@ -19,85 +19,85 @@ public final class LinearSystemId {
/**
* Create a state-space model of an elevator system.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param massKg The mass of the elevator carriage, in kilograms.
* @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.
* @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) {
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.m_KtNMPerAmp
/ (motor.m_rOhms * radiusMeters * radiusMeters * massKg
* motor.m_KvRadPerSecPerVolt)),
VecBuilder.fill(
0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * radiusMeters * massKg)),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-Math.pow(G, 2)
* motor.m_KtNMPerAmp
/ (motor.m_rOhms
* radiusMeters
* radiusMeters
* massKg
* motor.m_KvRadPerSecPerVolt)),
VecBuilder.fill(0, G * motor.m_KtNMPerAmp / (motor.m_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.
*
* @param motor The motor (or gearbox) attached to the arm.
* @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.
* @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) {
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
DCMotor motor, double jKgMetersSquared, double G) {
return new LinearSystem<>(
VecBuilder.fill(
-G * G * motor.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)),
VecBuilder.fill(G * motor.m_KtNMPerAmp
/ (motor.m_rOhms * jKgMetersSquared)),
Matrix.eye(Nat.N1()),
new Matrix<>(Nat.N1(), Nat.N1()));
-G
* G
* motor.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)),
VecBuilder.fill(G * motor.m_KtNMPerAmp / (motor.m_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.
* 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 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.
* @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) {
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
DCMotor motor,
double massKg,
double rMeters,
double rbMeters,
double JKgMetersSquared,
double G) {
var C1 =
-(G * G) * motor.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters);
-(G * G)
* motor.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters);
var C2 = G * motor.m_KtNMPerAmp / (motor.m_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 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);
@@ -107,87 +107,89 @@ public final class LinearSystemId {
/**
* Create a state-space model of a single jointed arm system.
*
* @param motor The motor (or gearbox) attached to the arm.
* @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.
* @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) {
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.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)),
VecBuilder.fill(0, G * motor.m_KtNMPerAmp
/ (motor.m_rOhms * jKgSquaredMeters)),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-Math.pow(G, 2)
* motor.m_KtNMPerAmp
/ (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)),
VecBuilder.fill(0, G * motor.m_KtNMPerAmp / (motor.m_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.
* 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.
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
* the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.wpilibj.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)
* @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>
* 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));
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.
* 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.
*
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
* the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
* {@link edu.wpi.first.wpilibj.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)
* @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>
* 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));
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.
* 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.
*
* @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).
* @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>
* https://github.com/wpilibsuite/frc-characterization</a>
*/
@SuppressWarnings("ParameterName")
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
final double c = 0.5 / (kALinear * kAAngular);
final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
@@ -197,8 +199,8 @@ public final class LinearSystemId {
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));
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));
}
}

153
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
View File

@@ -4,28 +4,23 @@
package edu.wpi.first.wpilibj.trajectory;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
/**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
* Represents a time-parameterized trajectory. The trajectory contains of various States that
* represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
*/
public class Trajectory {
private final double m_totalTimeSeconds;
private final List<State> m_states;
/**
* Constructs an empty trajectory.
*/
/** Constructs an empty trajectory. */
public Trajectory() {
m_states = new ArrayList<>();
m_totalTimeSeconds = 0.0;
@@ -45,8 +40,8 @@ public class Trajectory {
* Linearly interpolates between two values.
*
* @param startValue The start value.
* @param endValue The end value.
* @param t The fraction for interpolation.
* @param endValue The end value.
* @param t The fraction for interpolation.
* @return The interpolated value.
*/
@SuppressWarnings("ParameterName")
@@ -58,8 +53,8 @@ public class Trajectory {
* Linearly interpolates between two poses.
*
* @param startValue The start pose.
* @param endValue The end pose.
* @param t The fraction for interpolation.
* @param endValue The end pose.
* @param t The fraction for interpolation.
* @return The interpolated pose.
*/
@SuppressWarnings("ParameterName")
@@ -144,14 +139,15 @@ public class Trajectory {
return sample;
}
// Interpolate between the two states for the state that we want.
return prevSample.interpolate(sample,
return prevSample.interpolate(
sample,
(timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
}
/**
* Transforms all poses in the trajectory by the given transform. This is
* useful for converting a robot-relative trajectory into a field-relative
* trajectory. This works with respect to the first pose in the trajectory.
* Transforms all poses in the trajectory by the given transform. This is useful for converting a
* robot-relative trajectory into a field-relative trajectory. This works with respect to the
* first pose in the trajectory.
*
* @param transform The transform to transform the trajectory by.
* @return The transformed trajectory.
@@ -165,44 +161,54 @@ public class Trajectory {
var newFirstPose = firstPose.plus(transform);
List<State> newStates = new ArrayList<>();
newStates.add(new State(
firstState.timeSeconds, firstState.velocityMetersPerSecond,
firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
));
newStates.add(
new State(
firstState.timeSeconds,
firstState.velocityMetersPerSecond,
firstState.accelerationMetersPerSecondSq,
newFirstPose,
firstState.curvatureRadPerMeter));
for (int i = 1; i < m_states.size(); i++) {
var state = m_states.get(i);
// We are transforming relative to the coordinate frame of the new initial pose.
newStates.add(new State(
state.timeSeconds, state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
state.curvatureRadPerMeter
));
newStates.add(
new State(
state.timeSeconds,
state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq,
newFirstPose.plus(state.poseMeters.minus(firstPose)),
state.curvatureRadPerMeter));
}
return new Trajectory(newStates);
}
/**
* Transforms all poses in the trajectory so that they are relative to the
* given pose. This is useful for converting a field-relative trajectory
* into a robot-relative trajectory.
* Transforms all poses in the trajectory so that they are relative to the given pose. This is
* useful for converting a field-relative trajectory into a robot-relative trajectory.
*
* @param pose The pose that is the origin of the coordinate frame that
* the current trajectory will be transformed into.
* @param pose The pose that is the origin of the coordinate frame that the current trajectory
* will be transformed into.
* @return The transformed trajectory.
*/
public Trajectory relativeTo(Pose2d pose) {
return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
.collect(Collectors.toList()));
return new Trajectory(
m_states.stream()
.map(
state ->
new State(
state.timeSeconds,
state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq,
state.poseMeters.relativeTo(pose),
state.curvatureRadPerMeter))
.collect(Collectors.toList()));
}
/**
* Represents a time-parameterized trajectory. The trajectory contains of
* various States that represent the pose, curvature, time elapsed, velocity,
* and acceleration at that point.
* Represents a time-parameterized trajectory. The trajectory contains of various States that
* represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
*/
@SuppressWarnings("MemberName")
public static class State {
@@ -233,15 +239,18 @@ public class Trajectory {
/**
* Constructs a State with the specified parameters.
*
* @param timeSeconds The time elapsed since the beginning of the trajectory.
* @param velocityMetersPerSecond The speed at that point of the trajectory.
* @param timeSeconds The time elapsed since the beginning of the trajectory.
* @param velocityMetersPerSecond The speed at that point of the trajectory.
* @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
* @param poseMeters The pose at that point of the trajectory.
* @param curvatureRadPerMeter The curvature at that point of the trajectory.
* @param poseMeters The pose at that point of the trajectory.
* @param curvatureRadPerMeter The curvature at that point of the trajectory.
*/
public State(double timeSeconds, double velocityMetersPerSecond,
double accelerationMetersPerSecondSq, Pose2d poseMeters,
double curvatureRadPerMeter) {
public State(
double timeSeconds,
double velocityMetersPerSecond,
double accelerationMetersPerSecondSq,
Pose2d poseMeters,
double curvatureRadPerMeter) {
this.timeSeconds = timeSeconds;
this.velocityMetersPerSecond = velocityMetersPerSecond;
this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
@@ -253,7 +262,7 @@ public class Trajectory {
* Interpolates between two States.
*
* @param endValue The end value for the interpolation.
* @param i The interpolant (fraction).
* @param i The interpolant (fraction).
* @return The interpolated state.
*/
@SuppressWarnings("ParameterName")
@@ -270,8 +279,9 @@ public class Trajectory {
}
// Check whether the robot is reversing at this stage.
final boolean reversing = velocityMetersPerSecond < 0
|| Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
final boolean reversing =
velocityMetersPerSecond < 0
|| Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
// Calculate the new velocity
// v_f = v_0 + at
@@ -279,29 +289,35 @@ public class Trajectory {
// Calculate the change in position.
// delta_s = v_0 t + 0.5 at^2
final double newS = (velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
final double newS =
(velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
* (reversing ? -1.0 : 1.0);
// Return the new state. To find the new position for the new state, we need
// to interpolate between the two endpoint poses. The fraction for
// interpolation is the change in position (delta s) divided by the total
// distance between the two endpoints.
final double interpolationFrac = newS
/ endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
final double interpolationFrac =
newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
return new State(
newT, newV, accelerationMetersPerSecondSq,
newT,
newV,
accelerationMetersPerSecondSq,
lerp(poseMeters, endValue.poseMeters, interpolationFrac),
lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
);
lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac));
}
@Override
public String toString() {
return String.format(
"State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
poseMeters, curvatureRadPerMeter);
"State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
timeSeconds,
velocityMetersPerSecond,
accelerationMetersPerSecondSq,
poseMeters,
curvatureRadPerMeter);
}
@Override
@@ -314,17 +330,20 @@ public class Trajectory {
}
State state = (State) obj;
return Double.compare(state.timeSeconds, timeSeconds) == 0
&& Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
&& Double.compare(state.accelerationMetersPerSecondSq,
accelerationMetersPerSecondSq) == 0
&& Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
&& Objects.equals(poseMeters, state.poseMeters);
&& Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
&& Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0
&& Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
&& Objects.equals(poseMeters, state.poseMeters);
}
@Override
public int hashCode() {
return Objects.hash(timeSeconds, velocityMetersPerSecond,
accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
return Objects.hash(
timeSeconds,
velocityMetersPerSecond,
accelerationMetersPerSecondSq,
poseMeters,
curvatureRadPerMeter);
}
}

52
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
View File

@@ -4,9 +4,6 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
@@ -14,14 +11,16 @@ import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsCo
import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
import java.util.ArrayList;
import java.util.List;
/**
* Represents the configuration for generating a trajectory. This class stores the start velocity,
* end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
*
* <p>The class must be constructed with a max velocity and max acceleration. The other parameters
* (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable
* values (0, 0, {}, false). These values can be changed via the setXXX methods.
* (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable values
* (0, 0, {}, false). These values can be changed via the setXXX methods.
*/
public class TrajectoryConfig {
private final double m_maxVelocity;
@@ -34,11 +33,11 @@ public class TrajectoryConfig {
/**
* Constructs the trajectory configuration class.
*
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
*/
public TrajectoryConfig(double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq) {
public TrajectoryConfig(
double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) {
m_maxVelocity = maxVelocityMetersPerSecond;
m_maxAcceleration = maxAccelerationMetersPerSecondSq;
m_constraints = new ArrayList<>();
@@ -57,6 +56,7 @@ public class TrajectoryConfig {
/**
* Adds all user-defined constraints from a list to the trajectory.
*
* @param constraints List of user-defined constraints.
* @return Instance of the current config object.
*/
@@ -66,8 +66,8 @@ public class TrajectoryConfig {
}
/**
* Adds a differential drive kinematics constraint to ensure that
* no wheel velocity of a differential drive goes above the max velocity.
* Adds a differential drive kinematics constraint to ensure that no wheel velocity of a
* differential drive goes above the max velocity.
*
* @param kinematics The differential drive kinematics.
* @return Instance of the current config object.
@@ -78,34 +78,34 @@ public class TrajectoryConfig {
}
/**
* Adds a mecanum drive kinematics constraint to ensure that
* no wheel velocity of a mecanum drive goes above the max velocity.
*
* @param kinematics The mecanum drive kinematics.
* @return Instance of the current config object.
*/
* Adds a mecanum drive kinematics constraint to ensure that no wheel velocity of a mecanum drive
* goes above the max velocity.
*
* @param kinematics The mecanum drive kinematics.
* @return Instance of the current config object.
*/
public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
return this;
}
/**
* Adds a swerve drive kinematics constraint to ensure that
* no wheel velocity of a swerve drive goes above the max velocity.
*
* @param kinematics The swerve drive kinematics.
* @return Instance of the current config object.
*/
* Adds a swerve drive kinematics constraint to ensure that no wheel velocity of a swerve drive
* goes above the max velocity.
*
* @param kinematics The swerve drive kinematics.
* @return Instance of the current config object.
*/
public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
return this;
}
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
public double getStartVelocity() {
return m_startVelocity;
}

132
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
View File

@@ -4,12 +4,6 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.function.BiConsumer;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -20,17 +14,19 @@ import edu.wpi.first.wpilibj.spline.Spline;
import edu.wpi.first.wpilibj.spline.SplineHelper;
import edu.wpi.first.wpilibj.spline.SplineParameterizer;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.function.BiConsumer;
public final class TrajectoryGenerator {
private static final Trajectory kDoNothingTrajectory =
new Trajectory(Arrays.asList(new Trajectory.State()));
private static BiConsumer<String, StackTraceElement[]> errorFunc;
/**
* Private constructor because this is a utility class.
*/
private TrajectoryGenerator() {
}
/** Private constructor because this is a utility class. */
private TrajectoryGenerator() {}
private static void reportError(String error, StackTraceElement[] stackTrace) {
if (errorFunc != null) {
@@ -51,22 +47,21 @@ public final class TrajectoryGenerator {
/**
* Generates a trajectory from the given control vectors and config. This method uses clamped
* cubic splines -- a method in which the exterior control vectors and interior waypoints
* are provided. The headings are automatically determined at the interior points to
* ensure continuous curvature.
* cubic splines -- a method in which the exterior control vectors and interior waypoints are
* provided. The headings are automatically determined at the interior points to ensure continuous
* curvature.
*
* @param initial The initial control vector.
* @param initial The initial control vector.
* @param interiorWaypoints The interior waypoints.
* @param end The ending control vector.
* @param config The configuration for the trajectory.
* @param end The ending control vector.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(
Spline.ControlVector initial,
List<Translation2d> interiorWaypoints,
Spline.ControlVector end,
TrajectoryConfig config
) {
TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
// Clone the control vectors.
@@ -84,8 +79,10 @@ public final class TrajectoryGenerator {
// Get the spline points
List<PoseWithCurvature> points;
try {
points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
interiorWaypoints.toArray(new Translation2d[0]), newEnd));
points =
splinePointsFromSplines(
SplineHelper.getCubicSplinesFromControlVectors(
newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd));
} catch (MalformedSplineException ex) {
reportError(ex.getMessage(), ex.getStackTrace());
return kDoNothingTrajectory;
@@ -100,49 +97,50 @@ public final class TrajectoryGenerator {
}
// Generate and return trajectory.
return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
config.getMaxAcceleration(), config.isReversed());
return TrajectoryParameterizer.timeParameterizeTrajectory(
points,
config.getConstraints(),
config.getStartVelocity(),
config.getEndVelocity(),
config.getMaxVelocity(),
config.getMaxAcceleration(),
config.isReversed());
}
/**
* Generates a trajectory from the given waypoints and config. This method uses clamped
* cubic splines -- a method in which the initial pose, final pose, and interior waypoints
* are provided. The headings are automatically determined at the interior points to
* ensure continuous curvature.
* Generates a trajectory from the given waypoints and config. This method uses clamped cubic
* splines -- a method in which the initial pose, final pose, and interior waypoints are provided.
* The headings are automatically determined at the interior points to ensure continuous
* curvature.
*
* @param start The starting pose.
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @param config The configuration for the trajectory.
* @param end The ending pose.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
TrajectoryConfig config
) {
var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
start, interiorWaypoints.toArray(new Translation2d[0]), end
);
Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config) {
var controlVectors =
SplineHelper.getCubicControlVectorsFromWaypoints(
start, interiorWaypoints.toArray(new Translation2d[0]), end);
// Return the generated trajectory.
return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], config);
}
/**
* Generates a trajectory from the given quintic control vectors and config. This method
* uses quintic hermite splines -- therefore, all points must be represented by control
* vectors. Continuous curvature is guaranteed in this method.
* Generates a trajectory from the given quintic control vectors and config. This method uses
* quintic hermite splines -- therefore, all points must be represented by control vectors.
* Continuous curvature is guaranteed in this method.
*
* @param controlVectors List of quintic control vectors.
* @param config The configuration for the trajectory.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public static Trajectory generateTrajectory(
ControlVectorList controlVectors,
TrajectoryConfig config
) {
ControlVectorList controlVectors, TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
@@ -159,9 +157,10 @@ public final class TrajectoryGenerator {
// Get the spline points
List<PoseWithCurvature> points;
try {
points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
newControlVectors.toArray(new Spline.ControlVector[]{})
));
points =
splinePointsFromSplines(
SplineHelper.getQuinticSplinesFromControlVectors(
newControlVectors.toArray(new Spline.ControlVector[] {})));
} catch (MalformedSplineException ex) {
reportError(ex.getMessage(), ex.getStackTrace());
return kDoNothingTrajectory;
@@ -176,19 +175,23 @@ public final class TrajectoryGenerator {
}
// Generate and return trajectory.
return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
config.getMaxAcceleration(), config.isReversed());
return TrajectoryParameterizer.timeParameterizeTrajectory(
points,
config.getConstraints(),
config.getStartVelocity(),
config.getEndVelocity(),
config.getMaxVelocity(),
config.getMaxAcceleration(),
config.isReversed());
}
/**
* Generates a trajectory from the given waypoints and config. This method
* uses quintic hermite splines -- therefore, all points must be represented by Pose2d
* objects. Continuous curvature is guaranteed in this method.
* Generates a trajectory from the given waypoints and config. This method uses quintic hermite
* splines -- therefore, all points must be represented by Pose2d objects. Continuous curvature is
* guaranteed in this method.
*
* @param waypoints List of waypoints..
* @param config The configuration for the trajectory.
* @param config The configuration for the trajectory.
* @return The generated trajectory.
*/
@SuppressWarnings("LocalVariableName")
@@ -222,22 +225,25 @@ public final class TrajectoryGenerator {
}
// Generate and return trajectory.
return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
config.getMaxAcceleration(), config.isReversed());
return TrajectoryParameterizer.timeParameterizeTrajectory(
points,
config.getConstraints(),
config.getStartVelocity(),
config.getEndVelocity(),
config.getMaxVelocity(),
config.getMaxAcceleration(),
config.isReversed());
}
/**
* Generate spline points from a vector of splines by parameterizing the
* splines.
* Generate spline points from a vector of splines by parameterizing the splines.
*
* @param splines The splines to parameterize.
* @return The spline points for use in time parameterization of a trajectory.
* @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
* with approximately opposing headings)
* with approximately opposing headings)
*/
public static List<PoseWithCurvature> splinePointsFromSplines(
Spline[] splines) {
public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines) {
// Create the vector of spline points.
var splinePoints = new ArrayList<PoseWithCurvature>();

215
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
View File

@@ -28,45 +28,38 @@
package edu.wpi.first.wpilibj.trajectory;
import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
/**
* Class used to parameterize a trajectory by time.
*/
/** Class used to parameterize a trajectory by time. */
public final class TrajectoryParameterizer {
/**
* Private constructor because this is a utility class.
*/
private TrajectoryParameterizer() {
}
/** Private constructor because this is a utility class. */
private TrajectoryParameterizer() {}
/**
* Parameterize the trajectory by time. This is where the velocity profile is
* generated.
* Parameterize the trajectory by time. This is where the velocity profile is generated.
*
* <p>The derivation of the algorithm used can be found
* <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
* here</a>.
* <p>The derivation of the algorithm used can be found <a
* href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">here</a>.
*
* @param points Reference to the spline points.
* @param constraints A vector of various velocity and acceleration.
* constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param points Reference to the spline points.
* @param constraints A vector of various velocity and acceleration. constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param reversed Whether the robot should move backwards.
* Note that the robot will still move from
* a -&gt; b -&gt; ... -&gt; z as defined in the
* waypoints.
* @param reversed Whether the robot should move backwards. Note that the robot will still move
* from a -&gt; b -&gt; ... -&gt; z as defined in the waypoints.
* @return The trajectory.
*/
@SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
"PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops"})
@SuppressWarnings({
"PMD.ExcessiveMethodLength",
"PMD.CyclomaticComplexity",
"PMD.NPathComplexity",
"PMD.AvoidInstantiatingObjectsInLoops"
})
public static Trajectory timeParameterizeTrajectory(
List<PoseWithCurvature> points,
List<TrajectoryConstraint> constraints,
@@ -74,11 +67,15 @@ public final class TrajectoryParameterizer {
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
boolean reversed
) {
boolean reversed) {
var constrainedStates = new ArrayList<ConstrainedState>(points.size());
var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
var predecessor =
new ConstrainedState(
points.get(0),
0,
startVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq,
maxAccelerationMetersPerSecondSq);
// Forward pass
for (int i = 0; i < points.size(); i++) {
@@ -87,8 +84,12 @@ public final class TrajectoryParameterizer {
constrainedState.pose = points.get(i);
// Begin constraining based on predecessor.
double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
predecessor.pose.poseMeters.getTranslation());
double ds =
constrainedState
.pose
.poseMeters
.getTranslation()
.getDistance(predecessor.pose.poseMeters.getTranslation());
constrainedState.distanceMeters = predecessor.distanceMeters + ds;
// We may need to iterate to find the maximum end velocity and common
@@ -96,12 +97,12 @@ public final class TrajectoryParameterizer {
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
constrainedState.maxVelocityMetersPerSecond = Math.min(
maxVelocityMetersPerSecond,
Math.sqrt(predecessor.maxVelocityMetersPerSecond
* predecessor.maxVelocityMetersPerSecond
+ predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
);
constrainedState.maxVelocityMetersPerSecond =
Math.min(
maxVelocityMetersPerSecond,
Math.sqrt(
predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond
+ predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0));
constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
@@ -109,12 +110,13 @@ public final class TrajectoryParameterizer {
// At this point, the constrained state is fully constructed apart from
// all the custom-defined user constraints.
for (final var constraint : constraints) {
constrainedState.maxVelocityMetersPerSecond = Math.min(
constrainedState.maxVelocityMetersPerSecond,
constraint.getMaxVelocityMetersPerSecond(
constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
constrainedState.maxVelocityMetersPerSecond)
);
constrainedState.maxVelocityMetersPerSecond =
Math.min(
constrainedState.maxVelocityMetersPerSecond,
constraint.getMaxVelocityMetersPerSecond(
constrainedState.pose.poseMeters,
constrainedState.pose.curvatureRadPerMeter,
constrainedState.maxVelocityMetersPerSecond));
}
// Now enforce all acceleration limits.
@@ -127,16 +129,18 @@ public final class TrajectoryParameterizer {
// If the actual acceleration for this state is higher than the max
// acceleration that we applied, then we need to reduce the max
// acceleration of the predecessor and try again.
double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
double actualAcceleration =
(constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- predecessor.maxVelocityMetersPerSecond
* predecessor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
// If we violate the max acceleration constraint, let's modify the
// predecessor.
if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
predecessor.maxAccelerationMetersPerSecondSq
= constrainedState.maxAccelerationMetersPerSecondSq;
predecessor.maxAccelerationMetersPerSecondSq =
constrainedState.maxAccelerationMetersPerSecondSq;
} else {
// Constrain the predecessor's max acceleration to the current
// acceleration.
@@ -151,10 +155,13 @@ public final class TrajectoryParameterizer {
predecessor = constrainedState;
}
var successor = new ConstrainedState(points.get(points.size() - 1),
constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
endVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
var successor =
new ConstrainedState(
points.get(points.size() - 1),
constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
endVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq,
maxAccelerationMetersPerSecondSq);
// Backward pass
for (int i = points.size() - 1; i >= 0; i--) {
@@ -164,10 +171,10 @@ public final class TrajectoryParameterizer {
while (true) {
// Enforce max velocity limit (reverse)
// vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
double newMaxVelocity = Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+ successor.minAccelerationMetersPerSecondSq * ds * 2.0
);
double newMaxVelocity =
Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+ successor.minAccelerationMetersPerSecondSq * ds * 2.0);
// No more limits to impose! This state can be finalized.
if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
@@ -186,14 +193,15 @@ public final class TrajectoryParameterizer {
// If the actual acceleration for this state is lower than the min
// acceleration, then we need to lower the min acceleration of the
// successor and try again.
double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
double actualAcceleration =
(constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
/ (ds * 2.0);
if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
successor.minAccelerationMetersPerSecondSq
= constrainedState.minAccelerationMetersPerSecondSq;
successor.minAccelerationMetersPerSecondSq =
constrainedState.minAccelerationMetersPerSecondSq;
} else {
successor.minAccelerationMetersPerSecondSq = actualAcceleration;
break;
@@ -218,8 +226,10 @@ public final class TrajectoryParameterizer {
// Calculate the acceleration between the current state and the previous
// state.
double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
- velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
double accel =
(state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
- velocityMetersPerSecond * velocityMetersPerSecond)
/ (ds * 2);
// Calculate dt
double dt = 0.0;
@@ -232,8 +242,8 @@ public final class TrajectoryParameterizer {
// delta_x = v * t
dt = ds / velocityMetersPerSecond;
} else {
throw new TrajectoryGenerationException("Something went wrong at iteration " + i
+ " of time parameterization.");
throw new TrajectoryGenerationException(
"Something went wrong at iteration " + i + " of time parameterization.");
}
}
@@ -242,44 +252,53 @@ public final class TrajectoryParameterizer {
timeSeconds += dt;
states.add(new Trajectory.State(
timeSeconds,
reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
reversed ? -accel : accel,
state.pose.poseMeters, state.pose.curvatureRadPerMeter
));
states.add(
new Trajectory.State(
timeSeconds,
reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
reversed ? -accel : accel,
state.pose.poseMeters,
state.pose.curvatureRadPerMeter));
}
return new Trajectory(states);
}
private static void enforceAccelerationLimits(boolean reverse,
List<TrajectoryConstraint> constraints,
ConstrainedState state) {
private static void enforceAccelerationLimits(
boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) {
for (final var constraint : constraints) {
double factor = reverse ? -1.0 : 1.0;
final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
state.pose.poseMeters, state.pose.curvatureRadPerMeter,
state.maxVelocityMetersPerSecond * factor);
final var minMaxAccel =
constraint.getMinMaxAccelerationMetersPerSecondSq(
state.pose.poseMeters,
state.pose.curvatureRadPerMeter,
state.maxVelocityMetersPerSecond * factor);
if (minMaxAccel.minAccelerationMetersPerSecondSq
> minMaxAccel.maxAccelerationMetersPerSecondSq) {
throw new TrajectoryGenerationException("The constraint's min acceleration "
+ "was greater than its max acceleration.\n Offending Constraint: "
+ constraint.getClass().getName()
+ "\n If the offending constraint was packaged with WPILib, please file a bug report.");
throw new TrajectoryGenerationException(
"The constraint's min acceleration "
+ "was greater than its max acceleration.\n Offending Constraint: "
+ constraint.getClass().getName()
+ "\n If the offending constraint was packaged with WPILib, please file a bug"
+ " report.");
}
state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
: minMaxAccel.minAccelerationMetersPerSecondSq);
state.minAccelerationMetersPerSecondSq =
Math.max(
state.minAccelerationMetersPerSecondSq,
reverse
? -minMaxAccel.maxAccelerationMetersPerSecondSq
: minMaxAccel.minAccelerationMetersPerSecondSq);
state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
: minMaxAccel.maxAccelerationMetersPerSecondSq);
state.maxAccelerationMetersPerSecondSq =
Math.min(
state.maxAccelerationMetersPerSecondSq,
reverse
? -minMaxAccel.minAccelerationMetersPerSecondSq
: minMaxAccel.maxAccelerationMetersPerSecondSq);
}
}
@SuppressWarnings("MemberName")
@@ -290,10 +309,12 @@ public final class TrajectoryParameterizer {
double minAccelerationMetersPerSecondSq;
double maxAccelerationMetersPerSecondSq;
ConstrainedState(PoseWithCurvature pose, double distanceMeters,
double maxVelocityMetersPerSecond,
double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
ConstrainedState(
PoseWithCurvature pose,
double distanceMeters,
double maxVelocityMetersPerSecond,
double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
this.pose = pose;
this.distanceMeters = distanceMeters;
this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;

13
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj.trajectory;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.ObjectReader;
import com.fasterxml.jackson.databind.ObjectWriter;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.IOException;
@@ -11,11 +15,6 @@ import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Arrays;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.ObjectReader;
import com.fasterxml.jackson.databind.ObjectWriter;
public final class TrajectoryUtil {
private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
@@ -26,6 +25,7 @@ public final class TrajectoryUtil {
/**
* Imports a Trajectory from a PathWeaver-style JSON file.
*
* @param path the path of the json file to import from
* @return The trajectory represented by the file.
* @throws IOException if reading from the file fails
@@ -39,6 +39,7 @@ public final class TrajectoryUtil {
/**
* Exports a Trajectory to a PathWeaver-style JSON file.
*
* @param trajectory the trajectory to export
* @param path the path of the file to export to
* @throws IOException if writing to the file fails
@@ -52,6 +53,7 @@ public final class TrajectoryUtil {
/**
* Deserializes a Trajectory from PathWeaver-style JSON.
*
* @param json the string containing the serialized JSON
* @return the trajectory represented by the JSON
* @throws JsonProcessingException if deserializing the JSON fails
@@ -63,6 +65,7 @@ public final class TrajectoryUtil {
/**
* Serializes a Trajectory to PathWeaver-style JSON.
*
* @param trajectory the trajectory to export
* @return the string containing the serialized JSON
* @throws JsonProcessingException if serializing the Trajectory fails

79
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
View File

@@ -4,20 +4,19 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.Objects;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import java.util.Objects;
/**
* A trapezoid-shaped velocity profile.
*
* <p>While this class can be used for a profiled movement from start to finish,
* the intended usage is to filter a reference's dynamics based on trapezoidal
* velocity constraints. To compute the reference obeying this constraint, do
* the following.
* <p>While this class can be used for a profiled movement from start to finish, the intended usage
* is to filter a reference's dynamics based on trapezoidal velocity constraints. To compute the
* reference obeying this constraint, do the following.
*
* <p>Initialization:
*
* <pre><code>
* TrapezoidProfile.Constraints constraints =
* new TrapezoidProfile.Constraints(kMaxV, kMaxA);
@@ -26,19 +25,18 @@ import edu.wpi.first.math.MathUsageId;
* </code></pre>
*
* <p>Run on update:
*
* <pre><code>
* TrapezoidProfile profile =
* new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
* previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
* </code></pre>
*
* <p>where `unprofiledReference` is free to change between calls. Note that when
* the unprofiled reference is within the constraints, `calculate()` returns the
* unprofiled reference unchanged.
* <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
* reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
*
* <p>Otherwise, a timer can be started to provide monotonic values for
* `calculate()` and to determine when the profile has completed via
* `isFinished()`.
* <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
* determine when the profile has completed via `isFinished()`.
*/
public class TrapezoidProfile {
// The direction of the profile, either 1 for forwards or -1 for inverted
@@ -55,6 +53,7 @@ public class TrapezoidProfile {
public static class Constraints {
@SuppressWarnings("MemberName")
public double maxVelocity;
@SuppressWarnings("MemberName")
public double maxAcceleration;
@@ -78,11 +77,11 @@ public class TrapezoidProfile {
public static class State {
@SuppressWarnings("MemberName")
public double position;
@SuppressWarnings("MemberName")
public double velocity;
public State() {
}
public State() {}
public State(double position, double velocity) {
this.position = position;
@@ -109,8 +108,8 @@ public class TrapezoidProfile {
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
*/
public TrapezoidProfile(Constraints constraints, State goal, State initial) {
m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
@@ -134,12 +133,12 @@ public class TrapezoidProfile {
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
+ cutoffDistEnd;
double fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
* m_constraints.maxAcceleration;
double fullSpeedDist =
fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < 0) {
@@ -156,15 +155,15 @@ public class TrapezoidProfile {
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param goal The desired state when the profile is complete.
*/
public TrapezoidProfile(Constraints constraints, State goal) {
this(constraints, goal, new State(0, 0));
}
/**
* Calculate the correct position and velocity for the profile at a time t
* where the beginning of the profile was at time t = 0.
* Calculate the correct position and velocity for the profile at a time t where the beginning of
* the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
*/
@@ -176,13 +175,15 @@ public class TrapezoidProfile {
result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
/ 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
result.position +=
(m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
double timeLeft = m_endDeccel - t;
result.position = m_goal.position - (m_goal.velocity + timeLeft
* m_constraints.maxAcceleration / 2.0) * timeLeft;
result.position =
m_goal.position
- (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
} else {
result = m_goal;
}
@@ -247,20 +248,22 @@ public class TrapezoidProfile {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
* accelDist))) / acceleration;
double accelTime =
(-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)))
/ acceleration;
double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
+ 2 * decceleration * deccelDist))) / decceleration;
double deccelTime =
(-deccelVelocity
+ Math.sqrt(
Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist)))
/ decceleration;
double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + deccelTime;
}
/**
* Returns the total time the profile takes to reach the goal.
*/
/** Returns the total time the profile takes to reach the goal. */
public double totalTime() {
return m_endDeccel;
}
@@ -268,8 +271,8 @@ public class TrapezoidProfile {
/**
* Returns true if the profile has reached the goal.
*
* <p>The profile has reached the goal if the time since the profile started
* has exceeded the profile's total time.
* <p>The profile has reached the goal if the time since the profile started has exceeded the
* profile's total time.
*
* @param t The time since the beginning of the profile.
*/
@@ -282,8 +285,8 @@ public class TrapezoidProfile {
*
* <p>The profile is inverted if goal position is less than the initial position.
*
* @param initial The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
*/
private static boolean shouldFlipAcceleration(State initial, State goal) {
return initial.position > goal.position;

38
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
View File

@@ -7,13 +7,12 @@ package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* A constraint on the maximum absolute centripetal acceleration allowed when
* traversing a trajectory. The centripetal acceleration of a robot is defined
* as the velocity squared divided by the radius of curvature.
* A constraint on the maximum absolute centripetal acceleration allowed when traversing a
* trajectory. The centripetal acceleration of a robot is defined as the velocity squared divided by
* the radius of curvature.
*
* <p>Effectively, limiting the maximum centripetal acceleration will cause the
* robot to slow down around tight turns, making it easier to track trajectories
* with sharp turns.
* <p>Effectively, limiting the maximum centripetal acceleration will cause the robot to slow down
* around tight turns, making it easier to track trajectories with sharp turns.
*/
public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
private final double m_maxCentripetalAccelerationMetersPerSecondSq;
@@ -30,15 +29,15 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// ac = v^2 / r
// k (curvature) = 1 / r
@@ -46,23 +45,22 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
// ac / k = v^2
// v = std::sqrt(ac / k)
return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
/ Math.abs(curvatureRadPerMeter));
return Math.sqrt(
m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return new MinMax();

40
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
View File

@@ -9,10 +9,9 @@ import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
/**
* A class that enforces constraints on the differential drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for both sides of the drivetrain stay below a certain
* limit.
* A class that enforces constraints on the differential drive kinematics. This can be used to
* ensure that the trajectory is constructed so that the commanded velocities for both sides of the
* drivetrain stay below a certain limit.
*/
public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
@@ -24,28 +23,28 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
* @param kinematics A kinematics component describing the drive geometry.
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
public DifferentialDriveKinematicsConstraint(
final DifferentialDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
0, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds =
new ChassisSpeeds(
velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
@@ -56,18 +55,17 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
return new MinMax();
}
}

71
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
View File

@@ -4,18 +4,18 @@
package edu.wpi.first.wpilibj.trajectory.constraint;
import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;
/**
* A class that enforces constraints on differential drive voltage expenditure based on the motor
* dynamics and the drive kinematics. Ensures that the acceleration of any wheel of the robot
* while following the trajectory is never higher than what can be achieved with the given
* maximum voltage.
* dynamics and the drive kinematics. Ensures that the acceleration of any wheel of the robot while
* following the trajectory is never higher than what can be achieved with the given maximum
* voltage.
*/
public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
private final SimpleMotorFeedforward m_feedforward;
@@ -26,40 +26,41 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
* Creates a new DifferentialDriveVoltageConstraint.
*
* @param feedforward A feedforward component describing the behavior of the drive.
* @param kinematics A kinematics component describing the drive geometry.
* @param maxVoltage The maximum voltage available to the motors while following the path.
* Should be somewhat less than the nominal battery voltage (12V) to account
* for "voltage sag" due to current draw.
* @param kinematics A kinematics component describing the drive geometry.
* @param maxVoltage The maximum voltage available to the motors while following the path. Should
* be somewhat less than the nominal battery voltage (12V) to account for "voltage sag" due to
* current draw.
*/
public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
double maxVoltage) {
m_feedforward = requireNonNullParam(feedforward, "feedforward",
"DifferentialDriveVoltageConstraint");
m_kinematics = requireNonNullParam(kinematics, "kinematics",
"DifferentialDriveVoltageConstraint");
public DifferentialDriveVoltageConstraint(
SimpleMotorFeedforward feedforward,
DifferentialDriveKinematics kinematics,
double maxVoltage) {
m_feedforward =
requireNonNullParam(feedforward, "feedforward", "DifferentialDriveVoltageConstraint");
m_kinematics =
requireNonNullParam(kinematics, "kinematics", "DifferentialDriveVoltageConstraint");
m_maxVoltage = maxVoltage;
}
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
return Double.POSITIVE_INFINITY;
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
velocityMetersPerSecond
* curvatureRadPerMeter));
var wheelSpeeds =
m_kinematics.toWheelSpeeds(
new ChassisSpeeds(
velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter));
double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.rightMetersPerSecond);
double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.rightMetersPerSecond);
double maxWheelSpeed =
Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
double minWheelSpeed =
Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
@@ -97,12 +98,18 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
} else {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
/ (1
+ m_kinematics.trackWidthMeters
* Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond)
/ 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond) / 2);
/ (1
- m_kinematics.trackWidthMeters
* Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond)
/ 2);
}
// When turning about a point inside of the wheelbase (i.e. radius less than half

48
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
View File

@@ -8,9 +8,7 @@ import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
* Enforces a particular constraint only within an elliptical region.
*/
/** Enforces a particular constraint only within an elliptical region. */
public class EllipticalRegionConstraint implements TrajectoryConstraint {
private final Translation2d m_center;
private final Translation2d m_radii;
@@ -19,46 +17,49 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint {
/**
* Constructs a new EllipticalRegionConstraint.
*
* @param center The center of the ellipse in which to enforce the constraint.
* @param xWidth The width of the ellipse in which to enforce the constraint.
* @param yWidth The height of the ellipse in which to enforce the constraint.
* @param rotation The rotation to apply to all radii around the origin.
* @param center The center of the ellipse in which to enforce the constraint.
* @param xWidth The width of the ellipse in which to enforce the constraint.
* @param yWidth The height of the ellipse in which to enforce the constraint.
* @param rotation The rotation to apply to all radii around the origin.
* @param constraint The constraint to enforce when the robot is within the region.
*/
@SuppressWarnings("ParameterName")
public EllipticalRegionConstraint(Translation2d center, double xWidth, double yWidth,
Rotation2d rotation, TrajectoryConstraint constraint) {
public EllipticalRegionConstraint(
Translation2d center,
double xWidth,
double yWidth,
Rotation2d rotation,
TrajectoryConstraint constraint) {
m_center = center;
m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
m_constraint = constraint;
}
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (isPoseInRegion(poseMeters)) {
return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
velocityMetersPerSecond);
return m_constraint.getMaxVelocityMetersPerSecond(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
} else {
return Double.POSITIVE_INFINITY;
}
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (isPoseInRegion(poseMeters)) {
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
curvatureRadPerMeter, velocityMetersPerSecond);
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
} else {
return new MinMax();
}
}
/**
* Returns whether the specified robot pose is within the region that the constraint
* is enforced in.
* Returns whether the specified robot pose is within the region that the constraint is enforced
* in.
*
* @param robotPose The robot pose.
* @return Whether the robot pose is within the constraint region.
@@ -69,9 +70,8 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint {
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
// Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
return Math.pow(robotPose.getX() - m_center.getX(), 2)
* Math.pow(m_radii.getY(), 2)
+ Math.pow(robotPose.getY() - m_center.getY(), 2)
* Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
+ Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
<= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
}
}

10
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
View File

@@ -7,9 +7,9 @@ package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* Represents a constraint that enforces a max velocity. This can be composed with the
* {@link EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce
* a max velocity in a region.
* Represents a constraint that enforces a max velocity. This can be composed with the {@link
* EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce a max velocity in a
* region.
*/
public class MaxVelocityConstraint implements TrajectoryConstraint {
private final double m_maxVelocity;
@@ -24,8 +24,8 @@ public class MaxVelocityConstraint implements TrajectoryConstraint {
}
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
return m_maxVelocity;
}

39
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
View File

@@ -9,10 +9,9 @@ import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
/**
* A class that enforces constraints on the mecanum drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for all 4 wheels of the drivetrain stay below a certain
* limit.
* A class that enforces constraints on the mecanum drive kinematics. This can be used to ensure
* that the trajectory is constructed so that the commanded velocities for all 4 wheels of the
* drivetrain stay below a certain limit.
*/
public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
@@ -23,25 +22,24 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
public MecanumDriveKinematicsConstraint(
final MecanumDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
@@ -49,8 +47,8 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity,
ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds =
new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
@@ -64,18 +62,17 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
return new MinMax();
}
}

39
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
View File

@@ -7,9 +7,7 @@ package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
/**
* Enforces a particular constraint only within a rectangular region.
*/
/** Enforces a particular constraint only within a rectangular region. */
public class RectangularRegionConstraint implements TrajectoryConstraint {
private final Translation2d m_bottomLeftPoint;
private final Translation2d m_topRightPoint;
@@ -18,45 +16,44 @@ public class RectangularRegionConstraint implements TrajectoryConstraint {
/**
* Constructs a new RectangularRegionConstraint.
*
* @param bottomLeftPoint The bottom left point of the rectangular region in which to
* enforce the constraint.
* @param topRightPoint The top right point of the rectangular region in which to enforce
* the constraint.
* @param constraint The constraint to enforce when the robot is within the region.
* @param bottomLeftPoint The bottom left point of the rectangular region in which to enforce the
* constraint.
* @param topRightPoint The top right point of the rectangular region in which to enforce the
* constraint.
* @param constraint The constraint to enforce when the robot is within the region.
*/
public RectangularRegionConstraint(Translation2d bottomLeftPoint, Translation2d topRightPoint,
TrajectoryConstraint constraint) {
public RectangularRegionConstraint(
Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) {
m_bottomLeftPoint = bottomLeftPoint;
m_topRightPoint = topRightPoint;
m_constraint = constraint;
}
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (isPoseInRegion(poseMeters)) {
return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
velocityMetersPerSecond);
return m_constraint.getMaxVelocityMetersPerSecond(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
} else {
return Double.POSITIVE_INFINITY;
}
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (isPoseInRegion(poseMeters)) {
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
curvatureRadPerMeter, velocityMetersPerSecond);
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
} else {
return new MinMax();
}
}
/**
* Returns whether the specified robot pose is within the region that the constraint
* is enforced in.
* Returns whether the specified robot pose is within the region that the constraint is enforced
* in.
*
* @param robotPose The robot pose.
* @return Whether the robot pose is within the constraint region.

39
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
View File

@@ -9,10 +9,9 @@ import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
/**
* A class that enforces constraints on the swerve drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for all 4 wheels of the drivetrain stay below a certain
* limit.
* A class that enforces constraints on the swerve drive kinematics. This can be used to ensure that
* the trajectory is constructed so that the commanded velocities for all 4 wheels of the drivetrain
* stay below a certain limit.
*/
public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
@@ -23,25 +22,24 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
public SwerveDriveKinematicsConstraint(
final SwerveDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
@@ -49,8 +47,8 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity,
ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds =
new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
@@ -64,18 +62,17 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
return new MinMax();
}
}

41
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
View File

@@ -7,38 +7,36 @@ package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
/**
* An interface for defining user-defined velocity and acceleration constraints
* while generating trajectories.
* An interface for defining user-defined velocity and acceleration constraints while generating
* trajectories.
*/
public interface TrajectoryConstraint {
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* constraints are applied.
* @return The absolute maximum velocity.
*/
double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond);
double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond);
MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
/**
* Represents a minimum and maximum acceleration.
*/
/** Represents a minimum and maximum acceleration. */
@SuppressWarnings("MemberName")
class MinMax {
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
@@ -50,16 +48,13 @@ public interface TrajectoryConstraint {
* @param minAccelerationMetersPerSecondSq The minimum acceleration.
* @param maxAccelerationMetersPerSecondSq The maximum acceleration.
*/
public MinMax(double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
public MinMax(
double minAccelerationMetersPerSecondSq, double maxAccelerationMetersPerSecondSq) {
this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
}
/**
* Constructs a MinMax with default values.
*/
public MinMax() {
}
/** Constructs a MinMax with default values. */
public MinMax() {}
}
}

8
wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
View File

@@ -4,17 +4,13 @@
package edu.wpi.first.wpilibj.util;
/**
* Utility class that converts between commonly used units in FRC.
*/
/** Utility class that converts between commonly used units in FRC. */
public final class Units {
private static final double kInchesPerFoot = 12.0;
private static final double kMetersPerInch = 0.0254;
private static final double kSecondsPerMinute = 60;
/**
* Utility class, so constructor is private.
*/
/** Utility class, so constructor is private. */
private Units() {
throw new UnsupportedOperationException("This is a utility class!");
}

16
wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.wpiutil.math;
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
/**
@@ -19,8 +18,8 @@ public class MatBuilder<R extends Num, C extends Num> {
final Nat<C> m_cols;
/**
* Fills the matrix with the given data, encoded in row major form.
* (The matrix is filled row by row, left to right with the given data).
* Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
* row, left to right with the given data).
*
* @param data The data to fill the matrix with.
* @return The constructed matrix.
@@ -28,8 +27,14 @@ public class MatBuilder<R extends Num, C extends Num> {
@SuppressWarnings("LineLength")
public final Matrix<R, C> fill(double... data) {
if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
throw new IllegalArgumentException("Invalid matrix data provided. Wanted " + this.m_rows.getNum()
+ " x " + this.m_cols.getNum() + " matrix, but got " + data.length + " elements");
throw new IllegalArgumentException(
"Invalid matrix data provided. Wanted "
+ this.m_rows.getNum()
+ " x "
+ this.m_cols.getNum()
+ " matrix, but got "
+ data.length
+ " elements");
} else {
return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
}
@@ -37,6 +42,7 @@ public class MatBuilder<R extends Num, C extends Num> {
/**
* Creates a new {@link MatBuilder} with the given dimensions.
*
* @param rows The number of rows of the matrix.
* @param cols The number of columns of the matrix.
*/

8
wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
View File

@@ -13,8 +13,8 @@ public final class MathUtil {
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
public static int clamp(int value, int low, int high) {
return Math.max(low, Math.min(value, high));
@@ -24,8 +24,8 @@ public final class MathUtil {
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));

207
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
View File

@@ -4,8 +4,9 @@
package edu.wpi.first.wpiutil.math;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.Objects;
import org.ejml.MatrixDimensionException;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
@@ -15,9 +16,6 @@ import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.wpiutil.math.numbers.N1;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
@@ -33,24 +31,22 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Constructs an empty zero matrix of the given dimensions.
*
* @param rows The number of rows of the matrix.
* @param rows The number of rows of the matrix.
* @param columns The number of columns of the matrix.
*/
public Matrix(Nat<R> rows, Nat<C> columns) {
this.m_storage = new SimpleMatrix(
Objects.requireNonNull(rows).getNum(),
Objects.requireNonNull(columns).getNum()
);
this.m_storage =
new SimpleMatrix(
Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
}
/**
* Constructs a new {@link Matrix} with the given storage.
* Caller should make sure that the provided generic bounds match
* the shape of the provided {@link Matrix}.
* Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Matrix}.
*
* <p>NOTE:It is not recommend to use this constructor unless the
* {@link SimpleMatrix} API is absolutely necessary due to the desired
* function not being accessible through the {@link Matrix} wrapper.
* <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
* absolutely necessary due to the desired function not being accessible through the {@link
* Matrix} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this value.
*/
@@ -70,10 +66,9 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
*
* <p>NOTE:The use of this method is heavily discouraged as this removes any
* guarantee of type safety. This should only be called if the {@link SimpleMatrix}
* API is absolutely necessary due to the desired function not being accessible through
* the {@link Matrix} wrapper.
* <p>NOTE:The use of this method is heavily discouraged as this removes any guarantee of type
* safety. This should only be called if the {@link SimpleMatrix} API is absolutely necessary due
* to the desired function not being accessible through the {@link Matrix} wrapper.
*
* @return The underlying {@link SimpleMatrix} storage.
*/
@@ -113,8 +108,8 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Sets the value at the given indices.
*
* @param row The row of the element.
* @param col The column of the element.
* @param row The row of the element.
* @param col The column of the element.
* @param value The value to insert at the given location.
*/
public final void set(int row, int col, double value) {
@@ -128,22 +123,19 @@ public class Matrix<R extends Num, C extends Num> {
* @param val The row vector to set the given row to.
*/
public final void setRow(int row, Matrix<N1, C> val) {
this.m_storage.setRow(row, 0,
Objects.requireNonNull(val).m_storage.getDDRM().getData());
this.m_storage.setRow(row, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
}
/**
* Sets a column to a given column vector.
*
* @param column The column to set.
* @param val The column vector to set the given row to.
* @param val The column vector to set the given row to.
*/
public final void setColumn(int column, Matrix<R, N1> val) {
this.m_storage.setColumn(column, 0,
Objects.requireNonNull(val).m_storage.getDDRM().getData());
this.m_storage.setColumn(column, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
}
/**
* Sets all the elements in "this" matrix equal to the specified value.
*
@@ -156,8 +148,8 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Returns the diagonal elements inside a vector or square matrix.
*
* <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this"
* {@link Matrix} is a matrix then a vector of diagonal elements is returned.
* <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this" {@link
* Matrix} is a matrix then a vector of diagonal elements is returned.
*
* @return The diagonal elements inside a vector or a square matrix.
*/
@@ -183,7 +175,6 @@ public class Matrix<R extends Num, C extends Num> {
return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
}
/**
* Returns the smallest element of this matrix.
*
@@ -205,12 +196,12 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Multiplies this matrix with another that has C rows.
*
* <p>As matrix multiplication is only defined if the number of columns
* in the first matrix matches the number of rows in the second,
* this operation will fail to compile under any other circumstances.
* <p>As matrix multiplication is only defined if the number of columns in the first matrix
* matches the number of rows in the second, this operation will fail to compile under any other
* circumstances.
*
* @param other The other matrix to multiply by.
* @param <C2> The number of columns in the second matrix.
* @param <C2> The number of columns in the second matrix.
* @return The result of the matrix multiplication between "this" and the given matrix.
*/
public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
@@ -228,12 +219,11 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Returns a matrix which is the result of an element by element multiplication of
* "this" and other.
* Returns a matrix which is the result of an element by element multiplication of "this" and
* other.
*
* <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
*
*
* @param other The other {@link Matrix} to preform element multiplication on.
* @return The element by element multiplication of "this" and other.
*/
@@ -251,7 +241,6 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(this.m_storage.minus(value));
}
/**
* Subtracts the given matrix from this matrix.
*
@@ -262,7 +251,6 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
}
/**
* Adds the given value to all the elements of this matrix.
*
@@ -312,7 +300,6 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(this.m_storage.transpose());
}
/**
* Returns a copy of this matrix.
*
@@ -322,7 +309,6 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(this.m_storage.copy());
}
/**
* Returns the inverse matrix of "this" matrix.
*
@@ -336,8 +322,8 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Returns the solution x to the equation Ax = b, where A is "this" matrix.
*
* <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the
* pseudo inverse is used if A is not square.
* <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
* is used if A is not square.
*
* @param b The right-hand side of the equation to solve.
* @return The solution to the linear system.
@@ -348,39 +334,50 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Computes the matrix exponential using Eigen's solver.
* This method only works for square matrices, and will
* otherwise throw an {@link MatrixDimensionException}.
* Computes the matrix exponential using Eigen's solver. This method only works for square
* matrices, and will otherwise throw an {@link MatrixDimensionException}.
*
* @return The exponential of A.
*/
public final Matrix<R, C> exp() {
if (this.getNumRows() != this.getNumCols()) {
throw new MatrixDimensionException("Non-square matrices cannot be exponentiated! "
+ "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
throw new MatrixDimensionException(
"Non-square matrices cannot be exponentiated! "
+ "This matrix is "
+ this.getNumRows()
+ " x "
+ this.getNumCols());
}
Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
WPIMathJNI.exp(this.m_storage.getDDRM().getData(), this.getNumRows(),
toReturn.m_storage.getDDRM().getData());
WPIMathJNI.exp(
this.m_storage.getDDRM().getData(),
this.getNumRows(),
toReturn.m_storage.getDDRM().getData());
return toReturn;
}
/**
* Computes the matrix power using Eigen's solver.
* This method only works for square matrices, and will
* otherwise throw an {@link MatrixDimensionException}.
* Computes the matrix power using Eigen's solver. This method only works for square matrices, and
* will otherwise throw an {@link MatrixDimensionException}.
*
* @param exponent The exponent.
* @return The exponential of A.
*/
public final Matrix<R, C> pow(double exponent) {
if (this.getNumRows() != this.getNumCols()) {
throw new MatrixDimensionException("Non-square matrices cannot be raised to a power! "
+ "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
throw new MatrixDimensionException(
"Non-square matrices cannot be raised to a power! "
+ "This matrix is "
+ this.getNumRows()
+ " x "
+ this.getNumCols());
}
Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
WPIMathJNI.pow(this.m_storage.getDDRM().getData(), this.getNumRows(), exponent,
toReturn.m_storage.getDDRM().getData());
WPIMathJNI.pow(
this.m_storage.getDDRM().getData(),
this.getNumRows(),
exponent,
toReturn.m_storage.getDDRM().getData());
return toReturn;
}
@@ -396,7 +393,7 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Computes the Frobenius normal of the matrix.
*
* <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
* <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
*
* @return The matrix's Frobenius normal.
*/
@@ -480,37 +477,35 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Extracts a matrix of a given size and start position with new underlying
* storage.
* Extracts a matrix of a given size and start position with new underlying storage.
*
* @param height The number of rows of the extracted matrix.
* @param width The number of columns of the extracted matrix.
* @param width The number of columns of the extracted matrix.
* @param startingRow The starting row of the extracted matrix.
* @param startingCol The starting column of the extracted matrix.
* @return The extracted matrix.
*/
public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
return new Matrix<>(this.m_storage.extractMatrix(
startingRow,
Objects.requireNonNull(height).getNum() + startingRow,
startingCol,
Objects.requireNonNull(width).getNum() + startingCol));
return new Matrix<>(
this.m_storage.extractMatrix(
startingRow,
Objects.requireNonNull(height).getNum() + startingRow,
startingCol,
Objects.requireNonNull(width).getNum() + startingCol));
}
/**
* Assign a matrix of a given size and start position.
*
* @param startingRow The row to start at.
* @param startingCol The column to start at.
* @param other The matrix to assign the block to.
* @param startingCol The column to start at.
* @param other The matrix to assign the block to.
*/
public <R2 extends Num, C2 extends Num> void assignBlock(int startingRow, int startingCol,
Matrix<R2, C2> other) {
public <R2 extends Num, C2 extends Num> void assignBlock(
int startingRow, int startingCol, Matrix<R2, C2> other) {
this.m_storage.insertIntoThis(
startingRow,
startingCol,
Objects.requireNonNull(other).m_storage);
startingRow, startingCol, Objects.requireNonNull(other).m_storage);
}
/**
@@ -519,30 +514,30 @@ public class Matrix<R extends Num, C extends Num> {
*
* @param startingRow The starting row in the supplied matrix to extract the submatrix.
* @param startingCol The starting column in the supplied matrix to extract the submatrix.
* @param other The matrix to extract the submatrix from.
* @param other The matrix to extract the submatrix from.
*/
public <R2 extends Num, C2 extends Num> void extractFrom(int startingRow, int startingCol,
Matrix<R2, C2> other) {
CommonOps_DDRM.extract(other.m_storage.getDDRM(), startingRow, startingCol,
this.m_storage.getDDRM());
public <R2 extends Num, C2 extends Num> void extractFrom(
int startingRow, int startingCol, Matrix<R2, C2> other) {
CommonOps_DDRM.extract(
other.m_storage.getDDRM(), startingRow, startingCol, this.m_storage.getDDRM());
}
/**
* Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it
* will return the zero matrix.
* Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will
* return the zero matrix.
*
* @param lowerTriangular Whether or not we want to decompose to the lower triangular
* Cholesky matrix.
* @param lowerTriangular Whether or not we want to decompose to the lower triangular Cholesky
* matrix.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
* semidefinite).
* semidefinite).
*/
@SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
SimpleMatrix temp = m_storage.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
@@ -555,8 +550,8 @@ public class Matrix<R extends Num, C extends Num> {
return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
}
throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n"
+ m_storage.toString());
throw new RuntimeException(
"Cholesky decomposition failed! Input matrix:\n" + m_storage.toString());
}
return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
@@ -594,8 +589,8 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Entrypoint to the {@link MatBuilder} class for creation
* of custom matrices with the given dimensions and contents.
* Entrypoint to the {@link MatBuilder} class for creation of custom matrices with the given
* dimensions and contents.
*
* @param rows The number of rows of the desired matrix.
* @param cols The number of columns of the desired matrix.
@@ -608,8 +603,8 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Reassigns dimensions of a {@link Matrix} to allow for operations with
* other matrices that have wildcard dimensions.
* Reassigns dimensions of a {@link Matrix} to allow for operations with other matrices that have
* wildcard dimensions.
*
* @param mat The {@link Matrix} to remove the dimensions from.
* @return The matrix with reassigned dimensions.
@@ -622,40 +617,40 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
*
* <p>This will check if each element is in tolerance of the corresponding element
* from the other {@link Matrix} or if the elements have the same symbolic meaning. For two
* elements to have the same symbolic meaning they both must be either Double.NaN,
* Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY.
* <p>This will check if each element is in tolerance of the corresponding element from the other
* {@link Matrix} or if the elements have the same symbolic meaning. For two elements to have the
* same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or
* Double.NEGATIVE_INFINITY.
*
* <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this
* method when checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)}
* will return false if an element is uncountable. This method should only be used when
* uncountable elements need to compared.
* <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this method when
* checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} will return false
* if an element is uncountable. This method should only be used when uncountable elements need to
* compared.
*
* @param other The {@link Matrix} to check against this one.
* @param other The {@link Matrix} to check against this one.
* @param tolerance The tolerance to check equality with.
* @return true if this matrix is identical to the one supplied.
*/
public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
return MatrixFeatures_DDRM.isIdentical(this.m_storage.getDDRM(),
other.m_storage.getDDRM(), tolerance);
return MatrixFeatures_DDRM.isIdentical(
this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
}
/**
* Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
*
* <p>This will check if each element is in tolerance of the corresponding element
* from the other {@link Matrix}.
* <p>This will check if each element is in tolerance of the corresponding element from the other
* {@link Matrix}.
*
* <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
*
* @param other The {@link Matrix} to check against this one.
* @param other The {@link Matrix} to check against this one.
* @param tolerance The tolerance to check equality with.
* @return true if this matrix is equal to the one supplied.
*/
public boolean isEqual(Matrix<?, ?> other, double tolerance) {
return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(),
other.m_storage.getDDRM(), tolerance);
return MatrixFeatures_DDRM.isEquals(
this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
}
@Override

17
wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
View File

@@ -4,11 +4,9 @@
package edu.wpi.first.wpiutil.math;
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
@Deprecated
public final class MatrixUtils {
@@ -28,7 +26,8 @@ public final class MatrixUtils {
@SuppressWarnings("LineLength")
public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
return new Matrix<>(
new SimpleMatrix(Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
new SimpleMatrix(
Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
}
/**
@@ -54,8 +53,8 @@ public final class MatrixUtils {
}
/**
* Entrypoint to the MatBuilder class for creation
* of custom matrices with the given dimensions and contents.
* Entrypoint to the MatBuilder class for creation of custom matrices with the given dimensions
* and contents.
*
* @param rows The number of rows of the desired matrix.
* @param cols The number of columns of the desired matrix.
@@ -68,8 +67,8 @@ public final class MatrixUtils {
}
/**
* Entrypoint to the VecBuilder class for creation
* of custom vectors with the given size and contents.
* Entrypoint to the VecBuilder class for creation of custom vectors with the given size and
* contents.
*
* @param dim The dimension of the vector.
* @param <D> The dimension of the vector as a generic.

4
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
View File

@@ -4,9 +4,7 @@
package edu.wpi.first.wpiutil.math;
/**
* A number expressed as a java class.
*/
/** A number expressed as a java class. */
public abstract class Num {
/**
* The number this is backing.

85
wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
View File

@@ -4,8 +4,8 @@
package edu.wpi.first.wpiutil.math;
import edu.wpi.first.math.WPIMathJNI;
import java.util.function.BiFunction;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.NormOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
@@ -13,11 +13,8 @@ import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
import org.ejml.simple.SimpleBase;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.math.WPIMathJNI;
public final class SimpleMatrixUtils {
private SimpleMatrixUtils() {
}
private SimpleMatrixUtils() {}
/**
* Compute the matrix exponential, e^M of the given matrix.
@@ -55,8 +52,11 @@ public final class SimpleMatrixUtils {
}
@SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
private static SimpleMatrix dispatchPade(SimpleMatrix U, SimpleMatrix V,
int nSquarings, BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
private static SimpleMatrix dispatchPade(
SimpleMatrix U,
SimpleMatrix V,
int nSquarings,
BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
SimpleMatrix P = U.plus(V);
SimpleMatrix Q = U.negative().plus(V);
@@ -71,7 +71,7 @@ public final class SimpleMatrixUtils {
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
double[] b = new double[]{120, 60, 12, 1};
double[] b = new double[] {120, 60, 12, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
@@ -82,7 +82,7 @@ public final class SimpleMatrixUtils {
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
double[] b = new double[]{30240, 15120, 3360, 420, 30, 1};
double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
@@ -95,24 +95,26 @@ public final class SimpleMatrixUtils {
@SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
double[] b = new double[]{17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U =
A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V =
A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
double[] b = new double[]{17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240,
2162160, 110880, 3960, 90, 1};
double[] b =
new double[] {
17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
SimpleMatrix A4 = A2.mult(A2);
@@ -120,18 +122,41 @@ public final class SimpleMatrixUtils {
SimpleMatrix A8 = A6.mult(A2);
SimpleMatrix U =
A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
A.mult(
A8.scale(b[9])
.plus(A6.scale(b[7]))
.plus(A4.scale(b[5]))
.plus(A2.scale(b[3]))
.plus(ident.scale(b[1])));
SimpleMatrix V =
A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
A8.scale(b[8])
.plus(A6.scale(b[6]))
.plus(A4.scale(b[4]))
.plus(A2.scale(b[2]))
.plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
double[] b = new double[]{64764752532480000.0, 32382376266240000.0, 7771770303897600.0,
1187353796428800.0, 129060195264000.0, 10559470521600.0, 670442572800.0,
33522128640.0, 1323241920, 40840800, 960960, 16380, 182, 1};
double[] b =
new double[] {
64764752532480000.0,
32382376266240000.0,
7771770303897600.0,
1187353796428800.0,
129060195264000.0,
10559470521600.0,
670442572800.0,
33522128640.0,
1323241920,
40840800,
960960,
16380,
182,
1
};
SimpleMatrix ident = eye(A.numRows(), A.numCols());
SimpleMatrix A2 = A.mult(A);
@@ -139,9 +164,17 @@ public final class SimpleMatrixUtils {
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U =
A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
A.mult(
A6.scale(b[13])
.plus(A4.scale(b[11]))
.plus(A2.scale(b[9]))
.plus(A6.scale(b[7]))
.plus(A4.scale(b[5]))
.plus(A2.scale(b[3]))
.plus(ident.scale(b[1])));
SimpleMatrix V =
A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8])))
.plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
return new Pair<>(U, V);
}
@@ -168,7 +201,7 @@ public final class SimpleMatrixUtils {
* @param src The matrix to decompose.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
* semidefinite).
* semidefinite).
*/
public static SimpleMatrix lltDecompose(SimpleMatrix src) {
return lltDecompose(src, false);
@@ -182,14 +215,14 @@ public final class SimpleMatrixUtils {
* @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
* semidefinite).
* semidefinite).
*/
@SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
SimpleMatrix temp = src.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
@@ -215,11 +248,9 @@ public final class SimpleMatrixUtils {
* @return the exponential of A.
*/
@SuppressWarnings("ParameterName")
public static SimpleMatrix exp(
SimpleMatrix A) {
public static SimpleMatrix exp(SimpleMatrix A) {
SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
return toReturn;
}
}

38
wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
View File

@@ -15,8 +15,6 @@ import edu.wpi.first.wpiutil.math.numbers.N7;
import edu.wpi.first.wpiutil.math.numbers.N8;
import edu.wpi.first.wpiutil.math.numbers.N9;
/**
* A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
*
@@ -96,8 +94,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @param n5 the fifth element.
* @param n6 the sixth element.
*/
public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5,
double n6) {
public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5, double n6) {
return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
}
@@ -112,8 +109,8 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @param n6 the sixth element.
* @param n7 the seventh element.
*/
public static Vector<N7> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7) {
public static Vector<N7> fill(
double n1, double n2, double n3, double n4, double n5, double n6, double n7) {
return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
}
@@ -129,8 +126,8 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @param n7 the seventh element.
* @param n8 the eighth element.
*/
public static Vector<N8> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8) {
public static Vector<N8> fill(
double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) {
return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
}
@@ -147,8 +144,16 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @param n8 the eighth element.
* @param n9 the ninth element.
*/
public static Vector<N9> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8, double n9) {
public static Vector<N9> fill(
double n1,
double n2,
double n3,
double n4,
double n5,
double n6,
double n7,
double n8,
double n9) {
return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
}
@@ -167,8 +172,17 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @param n10 the tenth element.
*/
@SuppressWarnings("PMD.ExcessiveParameterList")
public static Vector<N10> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8, double n9, double n10) {
public static Vector<N10> fill(
double n1,
double n2,
double n3,
double n4,
double n5,
double n6,
double n7,
double n8,
double n9,
double n10) {
return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
}
}

16
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java
View File

@@ -4,9 +4,8 @@
package edu.wpi.first.wpiutil.math;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.simple.SimpleMatrix;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
@@ -20,20 +19,19 @@ public class Vector<R extends Num> extends Matrix<R, N1> {
/**
* Constructs an empty zero vector of the given dimensions.
*
* @param rows The number of rows of the vector.
* @param rows The number of rows of the vector.
*/
public Vector(Nat<R> rows) {
super(rows, Nat.N1());
}
/**
* Constructs a new {@link Vector} with the given storage.
* Caller should make sure that the provided generic bounds match
* the shape of the provided {@link Vector}.
* Constructs a new {@link Vector} with the given storage. Caller should make sure that the
* provided generic bounds match the shape of the provided {@link Vector}.
*
* <p>NOTE:It is not recommended to use this constructor unless the
* {@link SimpleMatrix} API is absolutely necessary due to the desired
* function not being accessible through the {@link Vector} wrapper.
* <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
* absolutely necessary due to the desired function not being accessible through the {@link
* Vector} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this vector.
*/