Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +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

67
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -37,11 +37,16 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per radian.
* @param kA The acceleration gain, in volt seconds² per radian.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: kS(kS), kG(kG), kV(kV), kA(kA) {
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
@@ -54,45 +59,84 @@ class WPILIB_DLLEXPORT ArmFeedforward {
this->kA = units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
}
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"period must be a positive number, got {}!", dt.value());
this->m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
}
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param angle The angle setpoint, 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 your encoder does not follow this convention, an
* offset should be added.
* @param velocity The velocity setpoint, in radians per second.
* @param acceleration The acceleration setpoint, in radians per second².
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Angle> angle,
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
/**
* 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 your encoder does not follow this
* convention, an offset should be added.
* @param currentVelocity The current velocity setpoint in radians per second.
* @param nextVelocity The next velocity setpoint in radians per second.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const;
/**
* Calculates the feedforward from the gains and setpoint assuming discrete
* control. Use this method 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.
*/
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const;
/**
* 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.
* @param nextVelocity The next velocity.
* @return The computed feedforward in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
@@ -232,6 +276,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
/// The acceleration gain, in V/(rad/s²).
units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
};
} // namespace frc

104
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -37,11 +37,16 @@ class ElevatorFeedforward {
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ElevatorFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: kS(kS), kG(kG), kV(kV), kA(kA) {
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
@@ -54,55 +59,43 @@ class ElevatorFeedforward {
this->kA = units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
}
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"period must be a positive number, got {}!", dt.value());
this->m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
}
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second².
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) {
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param currentVelocity The current velocity setpoint, in distance per
* second.
* @param nextVelocity The next velocity setpoint, in distance per second.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward, in volts.
*/
[[deprecated("Use the current/next velocity overload instead.")]]
units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
// 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)
// See wpimath/algorithms.md#Elevator_feedforward for derivation
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
@@ -113,6 +106,46 @@ class ElevatorFeedforward {
units::volt_t{feedforward.Calculate(r, nextR)(0)};
}
/**
* Calculates the feedforward from the gains and setpoint assuming discrete
* control. Use this method when the setpoint does not change.
*
* @param currentVelocity The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity) const {
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.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Elevator_feedforward for derivation
if (kA == decltype(kA)(0)) {
return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
} else {
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = 1.0 / A * (A_d - 1.0) * B;
return kG + kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *
(nextVelocity.value() - A_d * currentVelocity.value())};
}
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
@@ -222,6 +255,9 @@ class ElevatorFeedforward {
/// The acceleration gain.
units::unit_t<ka_unit> kA;
/** The period. */
units::second_t m_dt;
};
} // namespace frc

83
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -7,6 +7,8 @@
#include <gcem.hpp>
#include <wpi/MathExtras.h>
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
#include "units/voltage.h"
#include "wpimath/MathShared.h"
@@ -18,6 +20,8 @@ namespace frc {
* permanent-magnet DC motor.
*/
template <class Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
class SimpleMotorFeedforward {
public:
using Velocity =
@@ -35,6 +39,9 @@ class SimpleMotorFeedforward {
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
@@ -62,10 +69,11 @@ class SimpleMotorFeedforward {
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming continuous
* control.
*
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second².
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
@@ -77,11 +85,10 @@ class SimpleMotorFeedforward {
}
/**
* Calculates the feedforward from the gains and setpoint.
* Use this method when the setpoint does not change.
* Calculates the feedforward from the gains and setpoint assuming discrete
* control. Use this method when the setpoint does not change.
*
* @param setpoint The velocity setpoint, in distance per
* second.
* @param setpoint The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> setpoint) const {
@@ -89,70 +96,22 @@ class SimpleMotorFeedforward {
}
/**
* Calculates the feedforward from the gains and setpoints.
* Calculates the feedforward from the gains and setpoints assuming discrete
* control.
*
* @param currentVelocity The current velocity setpoint, in distance per
* second.
* @param nextVelocity The next velocity setpoint, in distance per second.
* <p>Note this method is inaccurate when the velocity crosses 0.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
// See wpimath/algorithms.md#Simple_motor_feedforward for derivation
if (kA == decltype(kA)(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 = std::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 kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
} 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.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());

5
wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.h
View File

@@ -10,7 +10,8 @@
#include "units/length.h"
// Everything is converted into units for
// frc::SimpleMotorFeedforward<units::meters>
// frc::SimpleMotorFeedforward<units::meters> or
// frc::SimpleMotorFeedforward<units::radians>
template <class Distance>
struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
@@ -31,6 +32,6 @@ struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
static_assert(
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
wpi::StructSerializable<frc::SimpleMotorFeedforward<units::radians>>);
#include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc"