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

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -4,9 +4,12 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.system.plant.LinearSystemId;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Unit;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
public class SimpleMotorFeedforward {
@@ -19,17 +22,22 @@ public class SimpleMotorFeedforward {
/** The acceleration gain. */
public final double ka;
/** The period. */
private double m_dt;
/**
* 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 and period. Units of the gain
* values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param dtSeconds The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka) {
public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) {
this.ks = ks;
this.kv = kv;
this.ka = ka;
@@ -39,17 +47,37 @@ public class SimpleMotorFeedforward {
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + dtSeconds + "!");
}
m_dt = dtSeconds;
}
/**
* Creates a new SimpleMotorFeedforward with the specified gains and period. The period is
* defaulted to 20 ms. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka) {
this(ks, kv, ka, 0.020);
}
/**
* 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.
* to zero. The period is defaulted to 20 ms. Units of the gain values will dictate units of the
* computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
*/
public SimpleMotorFeedforward(double ks, double kv) {
this(ks, kv, 0);
this(ks, kv, 0, 0.020);
}
/**
@@ -58,43 +86,114 @@ public class SimpleMotorFeedforward {
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
* @deprecated Use the current/next velocity overload instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity, double acceleration) {
return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dtSeconds Time between velocity setpoints in seconds.
* @return The computed feedforward.
*/
public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
* @deprecated Use the current/next velocity overload instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity) {
return calculate(velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control when the
* setpoint does not change.
*
* @param <U> The velocity parameter either as distance or angle.
* @param setpoint The velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit<U>> Measure<Voltage> calculate(Measure<Velocity<U>> setpoint) {
return calculate(setpoint, setpoint);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* @param <U> The velocity parameter either as distance or angle.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit<U>> Measure<Voltage> calculate(
Measure<Velocity<U>> currentVelocity, Measure<Velocity<U>> nextVelocity) {
if (ka == 0.0) {
// Given the following discrete feedforward model
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
//
// We want the feedforward model when kₐ = 0.
//
// Simplify A.
//
// A = −kᵥ/kₐ
//
// As kₐ approaches zero, A approaches -∞.
//
// A = −∞
//
// Simplify A_d.
//
// A_d = eᴬᵀ
// A_d = exp(−∞)
// A_d = 0
//
// Simplify B_d.
//
// B_d = A⁻¹(eᴬᵀ - I)B
// B_d = A⁻¹((0) - I)B
// B_d = A⁻¹(-I)B
// B_d = -A⁻¹B
// B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = kₐ/kᵥ(1/kₐ)
// B_d = 1/kᵥ
//
// Substitute these into the feedforward equation.
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
// uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ)
// uₖ = kᵥrₖ₊₁
return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude());
} else {
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
return Volts.of(
ks * Math.signum(currentVelocity.magnitude())
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()));
}
}
/**
* 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