Skip to content

Commit

Permalink
Fix wpimath test compilation
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Jun 12, 2024
1 parent c6058d5 commit ab2a350
Show file tree
Hide file tree
Showing 8 changed files with 277 additions and 205 deletions.
141 changes: 72 additions & 69 deletions wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,14 +4,12 @@

package edu.wpi.first.math.controller;

import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.N1;
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 {
Expand All @@ -24,14 +22,8 @@ public class SimpleMotorFeedforward {
/** The acceleration gain. */
public final double ka;

/** The feedforward. */
private final LinearPlantInversionFeedforward<N1, N1, N1> feedforward;

/** The current reference. */
private final Matrix<N1, N1> r;

/** The next reference. */
private Matrix<N1, N1> nextR;
/** The period. */
private double m_dt;

/**
* Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain
Expand All @@ -40,12 +32,12 @@ public class SimpleMotorFeedforward {
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param periodSeconds The period in seconds.
* @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, double periodSeconds) {
public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) {
this.ks = ks;
this.kv = kv;
this.ka = ka;
Expand All @@ -55,21 +47,11 @@ public SimpleMotorFeedforward(double ks, double kv, double ka, double periodSeco
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (periodSeconds <= 0.0) {
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + periodSeconds + "!");
}
if (ka != 0.0) {
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
this.feedforward = new LinearPlantInversionFeedforward<>(plant, periodSeconds);

this.r = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
this.nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
} else {
this.feedforward = null;
this.r = null;
this.nextR = null;
"period must be a positive number, got " + dtSeconds + "!");
}
m_dt = dtSeconds;
}

/**
Expand Down Expand Up @@ -104,6 +86,7 @@ public SimpleMotorFeedforward(double ks, double kv) {
* @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")
Expand All @@ -117,6 +100,7 @@ public double calculate(double velocity, double acceleration) {
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
* @deprecated Use the current/next velocity overload instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
Expand All @@ -132,50 +116,69 @@ public double calculate(double velocity) {
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit<U>> double calculate(
public <U extends Unit<U>> Measure<Voltage> calculate(
Measure<Velocity<U>> currentVelocity, Measure<Velocity<U>> nextVelocity) {
if (ka == 0.0) {
return ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude();
// 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ₖ₊₁
// 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 {
r.set(0, 0, currentVelocity.magnitude());
nextR.set(0, 0, nextVelocity.magnitude());

return ks * Math.signum(currentVelocity.magnitude())
+ feedforward.calculate(r, nextR).get(0, 0);
// 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()));
}
}

Expand Down
120 changes: 102 additions & 18 deletions wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,15 @@

#pragma once

#include <gcem.hpp>
#include <wpi/MathExtras.h>

#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
#include "units/voltage.h"
#include "wpimath/MathShared.h"

namespace frc {

/**
* A helper class that computes feedforward voltages for a simple
* permanent-magnet DC motor.
Expand All @@ -40,16 +39,47 @@ class SimpleMotorFeedforward {
SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t periodSeconds = units::second_t(0.020));

units::second_t dt = 20_ms)
: kS(kS),
kV([&] {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
return units::unit_t<kv_unit>{0};
} else {
return kV;
}
}()),
kA([&] {
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
return units::unit_t<ka_unit>{0};
} else {
return kA;
}
}()) {
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"period must be a positive number, got {}!", dt.value());
m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
} else {
m_dt = dt;
}
}

/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
[[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)) const {
Expand All @@ -64,15 +94,72 @@ class SimpleMotorFeedforward {
* @param nextVelocity The next velocity setpoint, in distance per second.
* @return The computed feedforward, in volts.
*/
units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
if (kA.value() == 0.0) {
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
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 {
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate({currentVelocity.value()}, {nextVelocity.value()})(0)};
// 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());
double B_d = 1.0 / A * (A_d - 1.0) * B;
return 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
Expand Down Expand Up @@ -155,12 +242,9 @@ class SimpleMotorFeedforward {
/** The acceleration gain. */
const units::unit_t<ka_unit> kA;

/** The plant. */
const frc::LinearSystem<1, 1, 1> plant;

/** The feedforward. */
const frc::LinearPlantInversionFeedforward<1, 1> feedforward;


private:
/** The period. */
units::second_t m_dt;
};

} // namespace frc
10 changes: 6 additions & 4 deletions wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,25 +26,27 @@ void testCalculate() {
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);

var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt);

var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
var currentVelocity = MutableMeasure.ofBaseUnits(2.0, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(3.0, RadiansPerSecond);

assertEquals(
37.52499583432516 + 0.5, simpleMotor.calculate(currentVelocity, nextVelocity), 0.002);
37.52499583432516 + 0.5,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(currentVelocity, nextVelocity),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);

// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(currentVelocity, nextVelocity),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
2.0);
}
}
Loading

0 comments on commit ab2a350

Please sign in to comment.