Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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

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);
}