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

[wpimath] Add ArmFeedforward calculate() overload that takes current and next velocity instead of acceleration (#6540)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-04-28 15:01:08 -04:00
committed by GitHub
parent 1727c74b80
commit 1ec089c7f9
8 changed files with 355 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
wpimath/CMakeLists.txt
View File

@@ -19,6 +19,7 @@ protobuf_generate_cpp(
file(
GLOB wpimath_jni_src
src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp

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

@@ -44,6 +44,33 @@ public final class WPIMathJNI {
libraryLoaded = true;
}
// ArmFeedforward wrappers
/**
* Obtain a feedforward voltage from a single jointed arm feedforward object.
*
* <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
*
* @param ks The ArmFeedforward's static gain in volts.
* @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
* @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
* @param kg The ArmFeedforward's gravity gain in volts.
* @param currentAngle The current angle in the calculation in radians.
* @param currentVelocity The current velocity in the calculation in radians per second.
* @param nextVelocity The next velocity in the calculation in radians per second.
* @param dt The time between velocity setpoints in seconds.
* @return The calculated feedforward in volts.
*/
public static native double calculate(
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double nextVelocity,
double dt);
// DARE wrappers
/**

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

@@ -4,6 +4,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -100,6 +101,22 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
return calculate(positionRadians, velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @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 dt Time between velocity setpoints in seconds.
* @return The computed feedforward in volts.
*/
public double calculate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:

100
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp Normal file
View File

@@ -0,0 +1,100 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/ArmFeedforward.h"
#include <limits>
#include <sleipnir/autodiff/Gradient.hpp>
#include <sleipnir/autodiff/Hessian.hpp>
#include "frc/EigenCore.h"
#include "frc/system/NumericalIntegration.h"
using namespace frc;
units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
using VarMat = sleipnir::VariableMatrix;
// Arm dynamics
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
const auto& f = [&](const VarMat& x, const VarMat& u) -> VarMat {
VarMat c{{0.0},
{-(kS / kA).value() * sleipnir::sign(x(1)) -
(kG / kA).value() * sleipnir::cos(x(0))}};
return A * x + B * u + c;
};
Vectord<2> r_k{currentAngle.value(), currentVelocity.value()};
sleipnir::Variable u_k;
// Initial guess
auto acceleration = (nextVelocity - currentVelocity) / dt;
u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * units::math::cos(currentAngle))
.value());
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, dt);
// Minimize difference between desired and actual next velocity
auto cost =
(nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1));
// Refine solution via Newton's method
{
auto xAD = u_k;
double x = xAD.Value();
sleipnir::Gradient gradientF{cost, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
sleipnir::Hessian hessianF{cost, xAD};
Eigen::SparseMatrix<double> H = hessianF.Value();
double error = std::numeric_limits<double>::infinity();
while (error > 1e-8) {
// Iterate via Newton's method.
//
// xₖ₊₁ = xₖ − H⁻¹g
//
// The Hessian is regularized to at least 1e-4.
double p_x = -g.coeff(0) / std::max(H.coeff(0, 0), 1e-4);
// Shrink step until cost goes down
{
double oldCost = cost.Value();
double α = 1.0;
double trial_x = x + α * p_x;
xAD.SetValue(trial_x);
cost.Update();
while (cost.Value() > oldCost) {
α *= 0.5;
trial_x = x + α * p_x;
xAD.SetValue(trial_x);
cost.Update();
}
x = trial_x;
}
xAD.SetValue(x);
g = gradientF.Value();
H = hessianF.Value();
error = std::abs(g.coeff(0));
}
}
return units::volt_t{u_k.Value()};
}

36
wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp Normal file
View File

@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <jni.h>
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "frc/controller/ArmFeedforward.h"
using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: calculate
* Signature: (DDDDDDDD)D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_math_WPIMathJNI_calculate
(JNIEnv* env, jclass, jdouble ks, jdouble kv, jdouble ka, jdouble kg,
jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
jdouble dt)
{
return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg},
units::unit_t<frc::ArmFeedforward::kv_unit>{kv},
units::unit_t<frc::ArmFeedforward::ka_unit>{ka}}
.Calculate(units::radian_t{currentAngle},
units::radians_per_second_t{currentVelocity},
units::radians_per_second_t{nextVelocity}, units::second_t{dt})
.value();
}
} // extern "C"

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

@@ -76,6 +76,23 @@ class WPILIB_DLLEXPORT ArmFeedforward {
kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @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 dt Time between velocity setpoints in seconds.
* @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,
units::second_t dt) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:

64
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
View File

@@ -6,6 +6,13 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
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.numbers.N2;
import edu.wpi.first.math.system.NumericalIntegration;
import java.util.function.BiFunction;
import org.junit.jupiter.api.Test;
class ArmFeedforwardTest {
@@ -15,12 +22,69 @@ class ArmFeedforwardTest {
private static final double ka = 2;
private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
* @param dt The simulation time in seconds.
* @return The final state as a 2-vector of angle and angular velocity.
*/
private Matrix<N2, N1> simulate(
double currentAngle, double currentVelocity, double input, double dt) {
final Matrix<N2, N2> A =
new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
final BiFunction<Matrix<N2, N1>, Matrix<N1, N1>, Matrix<N2, N1>> f =
(x, u) -> {
Matrix<N2, N1> c =
MatBuilder.fill(
Nat.N2(),
Nat.N1(),
0.0,
Math.signum(x.get(1, 0)) * (-ks / ka) - (kg / ka) * Math.cos(x.get(0, 0)));
return A.times(x).plus(B.times(u)).plus(c);
};
return NumericalIntegration.rk4(
f,
MatBuilder.fill(Nat.N2(), Nat.N1(), currentAngle, currentVelocity),
MatBuilder.fill(Nat.N1(), Nat.N1(), input),
dt);
}
/**
* Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
* then simulates an arm using that voltage to verify correctness.
*
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
* @param dt The simulation time in seconds.
*/
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity, dt);
assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
}
@Test
void testCalculate() {
// calculate(angle, angular velocity)
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1), 0.002);
// calculate(angle, angular velocity, angular acceleration)
assertEquals(6.5, m_armFF.calculate(Math.PI / 3, 1, 2), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, -1, 2), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020);
}
@Test

121
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -7,46 +7,111 @@
#include <gtest/gtest.h>
#include "frc/EigenCore.h"
#include "frc/controller/ArmFeedforward.h"
#include "units/acceleration.h"
#include "units/length.h"
#include "frc/system/NumericalIntegration.h"
#include "units/angular_acceleration.h"
#include "units/angular_velocity.h"
#include "units/time.h"
#include "units/voltage.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V * 1_s / 1_rad;
static constexpr auto Ka = 2_V * 1_s * 1_s / 1_rad;
static constexpr auto Kv = 1.5_V / 1_rad_per_s;
static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
static constexpr auto Kg = 1_V;
namespace {
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
const frc::Matrixd<1, 1>& u) -> frc::Matrixd<2, 1> {
frc::Matrixd<2, 1> c{0.0, -Ks.value() / Ka.value() * wpi::sgn(x(1)) -
Kg.value() / Ka.value() * std::cos(x(0))};
return A * x + B * u + c;
},
frc::Matrixd<2, 1>{currentAngle.value(), currentVelocity.value()},
frc::Matrixd<1, 1>{input.value()}, dt);
}
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param armFF The feedforward object.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity, dt);
EXPECT_NEAR(nextVelocity.value(),
Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
}
} // namespace
TEST(ArmFeedforwardTest, Calculate) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi * 1_rad / 3, 0_rad / 1_s).value(), 0.5,
armFF.Calculate(std::numbers::pi / 3 * 1_rad, 0_rad_per_s).value(), 0.5,
0.002);
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s).value(), 2.5,
armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s).value(), 2.5,
0.002);
EXPECT_NEAR(armFF
.Calculate(std::numbers::pi * 1_rad / 3, 1_rad / 1_s,
2_rad / 1_s / 1_s)
.value(),
6.5, 0.002);
EXPECT_NEAR(armFF
.Calculate(std::numbers::pi * 1_rad / 3, -1_rad / 1_s,
2_rad / 1_s / 1_s)
.value(),
2.5, 0.002);
// Calculate(angle, angular velocity, angular acceleration)
EXPECT_NEAR(
armFF.Calculate(std::numbers::pi / 3 * 1_rad, 1_rad_per_s, 2_rad_per_s_sq)
.value(),
6.5, 0.002);
EXPECT_NEAR(
armFF
.Calculate(std::numbers::pi / 3 * 1_rad, -1_rad_per_s, 2_rad_per_s_sq)
.value(),
2.5, 0.002);
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, AchievableVelocity) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s / 1_s)
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s_sq)
.value(),
6, 0.002);
EXPECT_NEAR(armFF
.MinAchievableVelocity(11.5_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s / 1_s)
.MinAchievableVelocity(11.5_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s_sq)
.value(),
-9, 0.002);
}
@@ -54,23 +119,23 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
TEST(ArmFeedforwardTest, AchievableAcceleration) {
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s)
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s)
.value(),
4.75, 0.002);
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
-1_rad / 1_s)
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
-1_rad_per_s)
.value(),
6.75, 0.002);
EXPECT_NEAR(armFF
.MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
1_rad / 1_s)
.MinAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s)
.value(),
-7.25, 0.002);
EXPECT_NEAR(armFF
.MinAchievableAcceleration(12_V, std::numbers::pi * 1_rad / 3,
-1_rad / 1_s)
.MinAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
-1_rad_per_s)
.value(),
-5.25, 0.002);
}