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] Fully discretized ElevatorFF and ArmFF (#7024)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-10-11 01:10:45 -04:00
committed by GitHub
parent 5d9a553104
commit 4adfa8bf64
31 changed files with 1004 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

193
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -4,9 +4,17 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
import edu.wpi.first.math.jni.ArmFeedforwardJNI;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -27,24 +35,25 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
/** The acceleration gain, in V/(rad/s²). */
private final double ka;
/** Arm feedforward protobuf for serialization. */
public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
/** The period, in seconds. */
private final double m_dt;
/** Arm feedforward struct for serialization. */
public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
/** The calculated output voltage measure. */
private final MutVoltage output = Volts.mutable(0.0);
/**
* 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 and period.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(rad/s).
* @param ka The acceleration gain in V/(rad/s²).
* @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 ArmFeedforward(double ks, double kg, double kv, double ka) {
public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
this.ks = ks;
this.kg = kg;
this.kv = kv;
@@ -55,56 +64,84 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
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 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. The period is defaulted to 20 ms.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(rad/s).
* @param ka The acceleration gain in V/(rad/s²).
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
*/
public ArmFeedforward(double ks, double kg, double kv, double ka) {
this(ks, kg, kv, ka, 0.020);
}
/**
* Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms.
*
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(rad/s).
* @throws IllegalArgumentException for kv &lt; zero.
*/
public ArmFeedforward(double ks, double kg, double kv) {
this(ks, kg, kv, 0);
}
/**
* Returns the static gain.
* Returns the static gain in volts.
*
* @return The static gain, in volts.
* @return The static gain in volts.
*/
public double getKs() {
return ks;
}
/**
* Returns the gravity gain.
* Returns the gravity gain in volts.
*
* @return The gravity gain, in volts.
* @return The gravity gain in volts.
*/
public double getKg() {
return kg;
}
/**
* Returns the velocity gain.
* Returns the velocity gain in V/(rad/s).
*
* @return The velocity gain, in V/(rad/s).
* @return The velocity gain.
*/
public double getKv() {
return kv;
}
/**
* Returns the acceleration gain.
* Returns the acceleration gain in V/(rad/s²).
*
* @return The acceleration gain, in V/(rad/s²).
* @return The acceleration gain.
*/
public double getKa() {
return ka;
}
/**
* Returns the period in seconds.
*
* @return The period in seconds.
*/
public double getDt() {
return m_dt;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
@@ -115,6 +152,7 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @param accelRadPerSecSquared The acceleration setpoint.
* @return The computed feedforward.
*/
@Deprecated(forRemoval = true, since = "2025")
public double calculate(
double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
return ks * Math.signum(velocityRadPerSec)
@@ -124,8 +162,8 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
* Calculates the feedforward from the gains and velocity setpoint assuming continuous control
* (acceleration is assumed to be zero).
*
* @param positionRadians The position (angle) setpoint. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
@@ -133,12 +171,14 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double positionRadians, double velocity) {
return calculate(positionRadians, velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous control.
*
* @param currentAngle The current angle in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If
@@ -148,12 +188,59 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward in volts.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
return ArmFeedforwardJNI.calculate(
ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control when the
* velocity does not change.
*
* @param currentAngle The current angle. This angle should be measured from the horizontal (i.e.
* if the provided angle is 0, the arm should be parallel to the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param currentVelocity The current velocity.
* @return The computed feedforward in volts.
*/
public Voltage calculate(Angle currentAngle, AngularVelocity currentVelocity) {
output.mut_replace(
kg * Math.cos(currentAngle.in(Radians))
+ ks * Math.signum(currentVelocity.in(RadiansPerSecond))
+ kv * currentVelocity.in(RadiansPerSecond),
Volts);
return output;
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* @param currentAngle The current angle. This angle should be measured from the horizontal (i.e.
* if the provided angle is 0, the arm should be parallel to the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward in volts.
*/
public Voltage calculate(
Angle currentAngle, AngularVelocity currentVelocity, AngularVelocity nextVelocity) {
output.mut_replace(
ArmFeedforwardJNI.calculate(
ks,
kv,
ka,
kg,
currentAngle.in(Radians),
currentVelocity.in(RadiansPerSecond),
nextVelocity.in(RadiansPerSecond),
m_dt),
Volts);
return output;
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
@@ -164,11 +251,11 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* 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. This angle should be measured from the horizontal (i.e. if
* the provided angle is 0, the arm should be parallel with the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param acceleration The acceleration of the arm.
* @return The maximum possible velocity at the given acceleration and angle.
* @param angle The angle of the arm, in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param acceleration The acceleration of the arm, in (rad/s²).
* @return The maximum possible velocity in (rad/s) at the given acceleration and angle.
*/
public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
// Assume max velocity is positive
@@ -181,12 +268,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* 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. This angle should be measured from the horizontal (i.e. if
* the provided angle is 0, the arm should be parallel with the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param acceleration The acceleration of the arm.
* @return The minimum possible velocity at the given acceleration and angle.
* @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
* @param angle The angle of the arm, in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param acceleration The acceleration of the arm, in (rad/s²).
* @return The minimum possible velocity in (rad/s) at the given acceleration and angle.
*/
public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
// Assume min velocity is negative, ks flips sign
@@ -199,12 +286,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* 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. This angle should be measured from the horizontal (i.e. if
* the provided angle is 0, the arm should be parallel with the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param velocity The velocity of the arm.
* @return The maximum possible acceleration at the given velocity.
* @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
* @param angle The angle of the arm, in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param velocity The velocity of the elevator, in (rad/s)
* @return The maximum possible acceleration in (rad/s²) at the given velocity.
*/
public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
@@ -216,14 +303,20 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* 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. This angle should be measured from the horizontal (i.e. if
* the provided angle is 0, the arm should be parallel with the floor). If your encoder does
* not follow this convention, an offset should be added.
* @param velocity The velocity of the arm.
* @return The minimum possible acceleration at the given velocity.
* @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
* @param angle The angle of the arm, in radians. This angle should be measured from the
* horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
* your encoder does not follow this convention, an offset should be added.
* @param velocity The velocity of the elevator, in (rad/s)
* @return The maximum possible acceleration in (rad/s²) at the given velocity.
*/
public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
return maxAchievableAcceleration(-maxVoltage, angle, velocity);
}
/** Arm feedforward struct for serialization. */
public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
/** Arm feedforward protobuf for serialization. */
public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
}

214
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -4,11 +4,14 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -17,36 +20,37 @@ import edu.wpi.first.util.struct.StructSerializable;
* against the force of gravity).
*/
public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
/** The static gain. */
/** The static gain, in volts. */
private final double ks;
/** The gravity gain. */
/** The gravity gain, in volts. */
private final double kg;
/** The velocity gain. */
/** The velocity gain, in V/(m/s). */
private final double kv;
/** The acceleration gain. */
/** The acceleration gain, in V/(m/s²). */
private final double ka;
/** ElevatorFeedforward protobuf for serialization. */
public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
/** The period, in seconds. */
private final double m_dt;
/** ElevatorFeedforward struct for serialization. */
public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
/** The calculated output voltage measure. */
private final MutVoltage output = Volts.mutable(0.0);
/**
* 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 and period.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(m/s).
* @param ka The acceleration gain in V/(m/s²).
* @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 ElevatorFeedforward(double ks, double kg, double kv, double ka) {
public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
this.ks = ks;
this.kg = kg;
this.kv = kv;
@@ -57,40 +61,60 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
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 ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms.
*
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(m/s).
* @param ka The acceleration gain in V/(m/s²).
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
*/
public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
this(ks, kg, kv, ka, 0.020);
}
/**
* 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.
* zero. The period is defaulted to 20 ms.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
* @param ks The static gain in volts.
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(m/s).
* @throws IllegalArgumentException for kv &lt; zero.
*/
public ElevatorFeedforward(double ks, double kg, double kv) {
this(ks, kg, kv, 0);
}
/**
* Returns the static gain.
* Returns the static gain in volts.
*
* @return The static gain.
* @return The static gain in volts.
*/
public double getKs() {
return ks;
}
/**
* Returns the gravity gain.
* Returns the gravity gain in volts.
*
* @return The gravity gain.
* @return The gravity gain in volts.
*/
public double getKg() {
return kg;
}
/**
* Returns the velocity gain.
* Returns the velocity gain in V/(m/s).
*
* @return The velocity gain.
*/
@@ -99,7 +123,7 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
}
/**
* Returns the acceleration gain.
* Returns the acceleration gain in V/(m/s²).
*
* @return The acceleration gain.
*/
@@ -108,67 +132,83 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
}
/**
* Calculates the feedforward from the gains and setpoints.
* Returns the period in seconds.
*
* @return The period in seconds.
*/
public double getDt() {
return m_dt;
}
/**
* Calculates the feedforward from the gains and setpoints assuming continuous control.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity, double acceleration) {
return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and velocity setpoint assuming continuous control
* (acceleration is assumed to be zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
@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 currentVelocity The velocity setpoint.
* @return The computed feedforward.
*/
public Voltage calculate(LinearVelocity currentVelocity) {
return calculate(currentVelocity, currentVelocity);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @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) {
// Discretize the affine model.
//
// dx/dt = Ax + Bu + c
// dx/dt = Ax + B(u + B⁺c)
// xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
// xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
//
// Solve for uₖ.
//
// B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
//
// For an elevator with the model
// dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
// A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
// assuming sgn(x) is a constant for the duration of the step.
//
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
// uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
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 kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double velocity) {
return calculate(velocity, 0);
public Voltage calculate(LinearVelocity currentVelocity, LinearVelocity nextVelocity) {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (ka == 0.0) {
output.mut_replace(
ks * Math.signum(nextVelocity.in(MetersPerSecond))
+ kg
+ kv * nextVelocity.in(MetersPerSecond),
Volts);
return output;
} else {
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;
output.mut_replace(
kg
+ ks * Math.signum(currentVelocity.magnitude())
+ 1.0
/ B_d
* (nextVelocity.in(MetersPerSecond) - A_d * currentVelocity.in(MetersPerSecond)),
Volts);
return output;
}
}
// Rearranging the main equation from the calculate() method yields the
@@ -180,9 +220,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* 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.
* @return The maximum possible velocity at the given acceleration.
* @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
* @param acceleration The acceleration of the elevator, in (m/s²).
* @return The maximum possible velocity in (m/s) at the given acceleration.
*/
public double maxAchievableVelocity(double maxVoltage, double acceleration) {
// Assume max velocity is positive
@@ -195,9 +235,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* 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.
* @return The minimum possible velocity at the given acceleration.
* @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
* @param acceleration The acceleration of the elevator, in (m/s²).
* @return The maximum possible velocity in (m/s) at the given acceleration.
*/
public double minAchievableVelocity(double maxVoltage, double acceleration) {
// Assume min velocity is negative, ks flips sign
@@ -210,9 +250,9 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* 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.
* @return The maximum possible acceleration at the given velocity.
* @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
* @param velocity The velocity of the elevator, in (m/s)
* @return The maximum possible acceleration in (m/s²) at the given velocity.
*/
public double maxAchievableAcceleration(double maxVoltage, double velocity) {
return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
@@ -224,11 +264,17 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* 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.
* @return The minimum possible acceleration at the given velocity.
* @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
* @param velocity The velocity of the elevator, in (m/s)
* @return The maximum possible acceleration in (m/s²) at the given velocity.
*/
public double minAchievableAcceleration(double maxVoltage, double velocity) {
return maxAchievableAcceleration(-maxVoltage, velocity);
}
/** ElevatorFeedforward struct for serialization. */
public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
/** ElevatorFeedforward protobuf for serialization. */
public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
}

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

@@ -12,31 +12,36 @@ import edu.wpi.first.units.Measure;
import edu.wpi.first.units.PerUnit;
import edu.wpi.first.units.TimeUnit;
import edu.wpi.first.units.Unit;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
public class SimpleMotorFeedforward implements ProtobufSerializable, StructSerializable {
/** The static gain. */
/** The static gain, in volts. */
private final double ks;
/** The velocity gain. */
/** The velocity gain, in V/(units/s). */
private final double kv;
/** The acceleration gain. */
/** The acceleration gain, in V/(units/s²). */
private final double ka;
/** The period. */
/** The period, in seconds. */
private final double m_dt;
// ** The calculated output voltage measure */
private final MutVoltage output = Volts.mutable(0.0);
/**
* Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain
* values will dictate units of the computed feedforward.
* Creates a new SimpleMotorFeedforward with the specified gains and period.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param ks The static gain in volts.
* @param kv The velocity gain in V/(units/s).
* @param ka The acceleration gain in V/(units/s²).
* @param dtSeconds The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
@@ -61,11 +66,13 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
/**
* 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.
* defaulted to 20 ms.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param ks The static gain in volts.
* @param kv The velocity gain in V/(units/s).
* @param ka The acceleration gain in V/(units/s²).
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
*/
@@ -75,45 +82,51 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
* to zero. The period is defaulted to 20 ms. Units of the gain values will dictate units of the
* computed feedforward.
* to zero. The period is defaulted to 20 ms.
*
* @param ks The static gain.
* @param kv The velocity gain.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param ks The static gain in volts.
* @param kv The velocity gain in V/(units/s).
* @throws IllegalArgumentException for kv &lt; zero.
*/
public SimpleMotorFeedforward(double ks, double kv) {
this(ks, kv, 0, 0.020);
this(ks, kv, 0);
}
/**
* Returns the static gain.
* Returns the static gain in volts.
*
* @return The static gain.
* @return The static gain in volts.
*/
public double getKs() {
return ks;
}
/**
* Returns the velocity gain.
* Returns the velocity gain in V/(units/s).
*
* @return The velocity gain.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @return The velocity gain in V/(units/s).
*/
public double getKv() {
return kv;
}
/**
* Returns the acceleration gain.
* Returns the acceleration gain in V/(units/s²).
*
* @return The acceleration gain.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @return The acceleration gain in V/(units/s²).
*/
public double getKa() {
return ka;
}
/**
* Returns the period.
* Returns the period in seconds.
*
* @return The period in seconds.
*/
@@ -122,7 +135,7 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous control.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
@@ -136,8 +149,8 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
* Calculates the feedforward from the gains and velocity setpoint assuming continuous control
* (acceleration is assumed to be zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
@@ -164,6 +177,8 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @param <U> The velocity parameter either as distance or angle.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
@@ -172,67 +187,21 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
public <U extends Unit> Voltage calculate(
Measure<? extends PerUnit<U, TimeUnit>> currentVelocity,
Measure<? extends PerUnit<U, TimeUnit>> nextVelocity) {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
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());
output.mut_replace(
ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude(), Volts);
return output;
} 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(
output.mut_replace(
ks * Math.signum(currentVelocity.magnitude())
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()));
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()),
Volts);
return output;
}
}
@@ -242,9 +211,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
* 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.
* @return The maximum possible velocity at the given acceleration.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
* @param acceleration The acceleration of the motor, in (units/s²).
* @return The maximum possible velocity in (units/s) at the given acceleration.
*/
public double maxAchievableVelocity(double maxVoltage, double acceleration) {
// Assume max velocity is positive
@@ -257,9 +228,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
* 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.
* @return The minimum possible velocity at the given acceleration.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
* @param acceleration The acceleration of the motor, in (units/s²).
* @return The maximum possible velocity in (units/s) at the given acceleration.
*/
public double minAchievableVelocity(double maxVoltage, double acceleration) {
// Assume min velocity is negative, ks flips sign
@@ -272,9 +245,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
* 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.
* @return The maximum possible acceleration at the given velocity.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
* @param velocity The velocity of the motor, in (units/s).
* @return The maximum possible acceleration in (units/s²) at the given velocity.
*/
public double maxAchievableAcceleration(double maxVoltage, double velocity) {
return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
@@ -286,9 +261,11 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
* 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.
* @return The minimum possible acceleration at the given velocity.
* <p>The units should be radians for angular systems and meters for linear systems.
*
* @param maxVoltage The maximum voltage that can be supplied to the motor, in volts.
* @param velocity The velocity of the motor, in (units/s).
* @return The maximum possible acceleration in (units/s²) at the given velocity.
*/
public double minAchievableAcceleration(double maxVoltage, double velocity) {
return maxAchievableAcceleration(-maxVoltage, velocity);

3
wpimath/src/main/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProto.java
View File

@@ -27,7 +27,7 @@ public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArm
@Override
public ArmFeedforward unpack(ProtobufArmFeedforward msg) {
return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
return new ArmFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
}
@Override
@@ -36,5 +36,6 @@ public class ArmFeedforwardProto implements Protobuf<ArmFeedforward, ProtobufArm
msg.setKg(value.getKg());
msg.setKv(value.getKv());
msg.setKa(value.getKa());
msg.setDt(value.getDt());
}
}

3
wpimath/src/main/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProto.java
View File

@@ -28,7 +28,7 @@ public class ElevatorFeedforwardProto
@Override
public ElevatorFeedforward unpack(ProtobufElevatorFeedforward msg) {
return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa());
return new ElevatorFeedforward(msg.getKs(), msg.getKg(), msg.getKv(), msg.getKa(), msg.getDt());
}
@Override
@@ -37,5 +37,6 @@ public class ElevatorFeedforwardProto
msg.setKg(value.getKg());
msg.setKv(value.getKv());
msg.setKa(value.getKa());
msg.setDt(value.getDt());
}
}

8
wpimath/src/main/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStruct.java
View File

@@ -21,12 +21,12 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
@Override
public int getSize() {
return kSizeDouble * 4;
return kSizeDouble * 5;
}
@Override
public String getSchema() {
return "double ks;double kg;double kv;double ka";
return "double ks;double kg;double kv;double ka;double dt";
}
@Override
@@ -35,7 +35,8 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
double kg = bb.getDouble();
double kv = bb.getDouble();
double ka = bb.getDouble();
return new ArmFeedforward(ks, kg, kv, ka);
double dt = bb.getDouble();
return new ArmFeedforward(ks, kg, kv, ka, dt);
}
@Override
@@ -44,5 +45,6 @@ public class ArmFeedforwardStruct implements Struct<ArmFeedforward> {
bb.putDouble(value.getKg());
bb.putDouble(value.getKv());
bb.putDouble(value.getKa());
bb.putDouble(value.getDt());
}
}

8
wpimath/src/main/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStruct.java
View File

@@ -21,12 +21,12 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
@Override
public int getSize() {
return kSizeDouble * 4;
return kSizeDouble * 5;
}
@Override
public String getSchema() {
return "double ks;double kg;double kv;double ka";
return "double ks;double kg;double kv;double ka;double dt";
}
@Override
@@ -35,7 +35,8 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
double kg = bb.getDouble();
double kv = bb.getDouble();
double ka = bb.getDouble();
return new ElevatorFeedforward(ks, kg, kv, ka);
double dt = bb.getDouble();
return new ElevatorFeedforward(ks, kg, kv, ka, dt);
}
@Override
@@ -44,5 +45,6 @@ public class ElevatorFeedforwardStruct implements Struct<ElevatorFeedforward> {
bb.putDouble(value.getKg());
bb.putDouble(value.getKv());
bb.putDouble(value.getKa());
bb.putDouble(value.getDt());
}
}