mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
[wpimath] Add Exponential motion profile (#5720)
This commit is contained in:
Jordan McMichael
@@ -0,0 +1,364 @@
|
||||
// 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.
|
||||
|
||||
package edu.wpi.first.math.trajectory;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class ExponentialProfileTest {
|
||||
private static final double kDt = 0.01;
|
||||
private static final SimpleMotorFeedforward feedforward =
|
||||
new SimpleMotorFeedforward(0, 2.5629, 0.43277);
|
||||
private static final ExponentialProfile.Constraints constraints =
|
||||
ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
|
||||
|
||||
/**
|
||||
* Asserts "val1" is within "eps" of "val2".
|
||||
*
|
||||
* @param val1 First operand in comparison.
|
||||
* @param val2 Second operand in comparison.
|
||||
* @param eps Tolerance for whether values are near to each other.
|
||||
*/
|
||||
private static void assertNear(double val1, double val2, double eps) {
|
||||
assertTrue(
|
||||
Math.abs(val1 - val2) <= eps,
|
||||
"Difference between " + val1 + " and " + val2 + " is greater than " + eps);
|
||||
}
|
||||
|
||||
private static void assertNear(
|
||||
ExponentialProfile.State val1, ExponentialProfile.State val2, double eps) {
|
||||
assertAll(
|
||||
() -> assertNear(val1.position, val2.position, eps),
|
||||
() -> assertNear(val1.position, val2.position, eps));
|
||||
}
|
||||
|
||||
private static ExponentialProfile.State checkDynamics(
|
||||
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
|
||||
var next = profile.calculate(kDt, current, goal);
|
||||
|
||||
var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
|
||||
|
||||
assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
|
||||
|
||||
return next;
|
||||
}
|
||||
|
||||
@Test
|
||||
void reachesGoal() {
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
for (int i = 0; i < 450; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// Tests that decreasing the maximum velocity in the middle when it is already
|
||||
// moving faster than the new max is handled correctly
|
||||
@Test
|
||||
void posContinuousUnderVelChange() {
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(10, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
if (i == 150) {
|
||||
profile =
|
||||
new ExponentialProfile(
|
||||
ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
|
||||
}
|
||||
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// Tests that decreasing the maximum velocity in the middle when it is already
|
||||
// moving faster than the new max is handled correctly
|
||||
@Test
|
||||
void posContinuousUnderVelChangeBackward() {
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
if (i == 150) {
|
||||
profile =
|
||||
new ExponentialProfile(
|
||||
ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B));
|
||||
}
|
||||
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// There is some somewhat tricky code for dealing with going backwards
|
||||
@Test
|
||||
void backwards() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
for (int i = 0; i < 400; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void switchGoalInMiddle() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
for (int i = 0; i < 50; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertNotEquals(state, goal);
|
||||
|
||||
goal = new ExponentialProfile.State(0.0, 0.0);
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed
|
||||
@Test
|
||||
void topSpeed() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
double maxSpeed = 0;
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
maxSpeed = Math.max(maxSpeed, state.velocity);
|
||||
}
|
||||
|
||||
assertNear(constraints.maxVelocity(), maxSpeed, 10e-5);
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void topSpeedBackward() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
double maxSpeed = 0;
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
maxSpeed = Math.min(maxSpeed, state.velocity);
|
||||
}
|
||||
|
||||
assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5);
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void largeInitialVelocity() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 8);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void largeNegativeInitialVelocity() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, -8);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
}
|
||||
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@SuppressWarnings("PMD.TestClassWithoutTestCases")
|
||||
static class TestCase {
|
||||
public final ExponentialProfile.State initial;
|
||||
public final ExponentialProfile.State goal;
|
||||
public final ExponentialProfile.State inflectionPoint;
|
||||
|
||||
TestCase(
|
||||
ExponentialProfile.State initial,
|
||||
ExponentialProfile.State goal,
|
||||
ExponentialProfile.State inflectionPoint) {
|
||||
this.initial = initial;
|
||||
this.goal = goal;
|
||||
this.inflectionPoint = inflectionPoint;
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void testHeuristic() {
|
||||
List<TestCase> testCases =
|
||||
List.of(
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(0.75, -4),
|
||||
new ExponentialProfile.State(1.3758, 4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(1.4103, 4),
|
||||
new ExponentialProfile.State(1.3758, 4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(0.75, -4),
|
||||
new ExponentialProfile.State(1.3758, 4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(1.4103, 4),
|
||||
new ExponentialProfile.State(1.3758, 4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(0.5, -2),
|
||||
new ExponentialProfile.State(0.4367, 3.7217)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(0.546, 2),
|
||||
new ExponentialProfile.State(0.4367, 3.7217)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(0.5, -2),
|
||||
new ExponentialProfile.State(0.5560, -2.9616)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(0.546, 2),
|
||||
new ExponentialProfile.State(0.5560, -2.9616)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(-0.75, -4),
|
||||
new ExponentialProfile.State(-0.7156, -4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(-0.0897, 4),
|
||||
new ExponentialProfile.State(-0.7156, -4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(-0.75, -4),
|
||||
new ExponentialProfile.State(-0.7156, -4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(-0.0897, 4),
|
||||
new ExponentialProfile.State(-0.7156, -4.4304)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(-0.5, -4.5),
|
||||
new ExponentialProfile.State(1.095, 4.314)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -4),
|
||||
new ExponentialProfile.State(1.0795, 4.5),
|
||||
new ExponentialProfile.State(-0.5122, -4.351)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(-0.5, -4.5),
|
||||
new ExponentialProfile.State(1.095, 4.314)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.6603, 4),
|
||||
new ExponentialProfile.State(1.0795, 4.5),
|
||||
new ExponentialProfile.State(-0.5122, -4.351)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -8),
|
||||
new ExponentialProfile.State(0, 0),
|
||||
new ExponentialProfile.State(-0.1384, 3.342)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, -8),
|
||||
new ExponentialProfile.State(-1, 0),
|
||||
new ExponentialProfile.State(-0.562, -6.792)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, 8),
|
||||
new ExponentialProfile.State(1, 0),
|
||||
new ExponentialProfile.State(0.562, 6.792)),
|
||||
new TestCase(
|
||||
new ExponentialProfile.State(0.0, 8),
|
||||
new ExponentialProfile.State(-1, 0),
|
||||
new ExponentialProfile.State(-0.785, -4.346)));
|
||||
|
||||
var profile = new ExponentialProfile(constraints);
|
||||
|
||||
for (var testCase : testCases) {
|
||||
var state = profile.calculateInflectionPoint(testCase.initial, testCase.goal);
|
||||
assertNear(testCase.inflectionPoint, state, 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToCurrent() {
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
for (int i = 0; i < 400; i++) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
assertNear(profile.timeLeftUntil(state, state), 0, 2e-2);
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToGoal() {
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(2, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(state, goal);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
|
||||
if (!reachedGoal && state.equals(goal)) {
|
||||
// Expected value using for loop index is just an approximation since
|
||||
// the time left in the profile doesn't increase linearly at the
|
||||
// endpoints
|
||||
assertNear(predictedTimeLeft, i / 100.0, 0.25);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToNegativeGoal() {
|
||||
ExponentialProfile profile = new ExponentialProfile(constraints);
|
||||
|
||||
ExponentialProfile.State goal = new ExponentialProfile.State(-2, 0);
|
||||
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(state, goal);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
state = checkDynamics(profile, state, goal);
|
||||
|
||||
if (!reachedGoal && state.equals(goal)) {
|
||||
// Expected value using for loop index is just an approximation since
|
||||
// the time left in the profile doesn't increase linearly at the
|
||||
// endpoints
|
||||
assertNear(predictedTimeLeft, i / 100.0, 0.25);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,338 @@
|
||||
// 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/trajectory/ExponentialProfile.h" // NOLINT(build/include_order)
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <tuple>
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "frc/controller/SimpleMotorFeedforward.h"
|
||||
#include "units/acceleration.h"
|
||||
#include "units/frequency.h"
|
||||
#include "units/length.h"
|
||||
#include "units/math.h"
|
||||
#include "units/velocity.h"
|
||||
#include "units/voltage.h"
|
||||
|
||||
static constexpr auto kDt = 10_ms;
|
||||
static constexpr auto kV = 2.5629_V / 1_mps;
|
||||
static constexpr auto kA = 0.43277_V / 1_mps_sq;
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
|
||||
if (val1 <= val2) { \
|
||||
EXPECT_LE(val1, val2); \
|
||||
} else { \
|
||||
EXPECT_NEAR_UNITS(val1, val2, eps); \
|
||||
}
|
||||
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile,
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints
|
||||
constraints,
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward,
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State current,
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal) {
|
||||
auto next = profile.Calculate(kDt, current, goal);
|
||||
auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
|
||||
|
||||
EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
|
||||
|
||||
return next;
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, ReachesGoal) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
for (int i = 0; i < 450; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Tests that decreasing the maximum velocity in the middle when it is already
|
||||
// moving faster than the new max is handled correctly
|
||||
TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
if (i == 150) {
|
||||
constraints.maxInput = 9_V;
|
||||
profile =
|
||||
frc::ExponentialProfile<units::meter, units::volts>{constraints};
|
||||
}
|
||||
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Tests that decreasing the maximum velocity in the middle when it is already
|
||||
// moving faster than the new max is handled correctly
|
||||
TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
for (int i = 0; i < 300; ++i) {
|
||||
if (i == 150) {
|
||||
constraints.maxInput = 9_V;
|
||||
profile =
|
||||
frc::ExponentialProfile<units::meter, units::volts>{constraints};
|
||||
}
|
||||
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// There is some somewhat tricky code for dealing with going backwards
|
||||
TEST(ExponentialProfileTest, Backwards) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state;
|
||||
|
||||
for (int i = 0; i < 400; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
for (int i = 0; i < 50; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_NE(state, goal);
|
||||
|
||||
goal = {0.0_m, 0.0_mps};
|
||||
for (int i = 0; i < 100; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed on long trajectories
|
||||
TEST(ExponentialProfileTest, TopSpeed) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state;
|
||||
|
||||
units::meters_per_second_t maxSpeed = 0_mps;
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
maxSpeed = units::math::max(state.velocity, maxSpeed);
|
||||
}
|
||||
|
||||
EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed on long trajectories
|
||||
TEST(ExponentialProfileTest, TopSpeedBackward) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state;
|
||||
|
||||
units::meters_per_second_t maxSpeed = 0_mps;
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
maxSpeed = units::math::min(state.velocity, maxSpeed);
|
||||
}
|
||||
|
||||
EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed on long trajectories
|
||||
TEST(ExponentialProfileTest, HighInitialSpeed) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed on long trajectories
|
||||
TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
}
|
||||
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, TestHeuristic) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
std::vector<std::tuple<
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State, // initial
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State, // goal
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State> // inflection
|
||||
// point
|
||||
>
|
||||
testCases{
|
||||
// red > green and purple => always positive => false
|
||||
{{0_m, -4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
|
||||
{{0_m, -4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
|
||||
{{0.6603_m, 4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}},
|
||||
{{0.6603_m, 4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}},
|
||||
|
||||
// purple > red > green => positive if v0 < 0 => c && !d && a
|
||||
{{0_m, -4_mps}, {0.5_m, -2_mps}, {0.4367_m, 3.7217_mps}},
|
||||
{{0_m, -4_mps}, {0.546_m, 2_mps}, {0.4367_m, 3.7217_mps}},
|
||||
{{0.6603_m, 4_mps}, {0.5_m, -2_mps}, {0.5560_m, -2.9616_mps}},
|
||||
{{0.6603_m, 4_mps}, {0.546_m, 2_mps}, {0.5560_m, -2.9616_mps}},
|
||||
|
||||
// red < green and purple => always negative => true => !c && !d
|
||||
{{0_m, -4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
|
||||
{{0_m, -4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
|
||||
{{0.6603_m, 4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}},
|
||||
{{0.6603_m, 4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}},
|
||||
|
||||
// green > red > purple => positive if vf < 0 => !c && d && b
|
||||
{{0_m, -4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
|
||||
{{0_m, -4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
|
||||
{{0.6603_m, 4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}},
|
||||
{{0.6603_m, 4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}},
|
||||
|
||||
// tests for initial velocity > V/kV
|
||||
{{0_m, -8_mps}, {0_m, 0_mps}, {-0.1384_m, 3.342_mps}},
|
||||
{{0_m, -8_mps}, {-1_m, 0_mps}, {-0.562_m, -6.792_mps}},
|
||||
{{0_m, 8_mps}, {1_m, 0_mps}, {0.562_m, 6.792_mps}},
|
||||
{{0_m, 8_mps}, {-1_m, 0_mps}, {-0.785_m, -4.346_mps}},
|
||||
};
|
||||
|
||||
for (auto& testCase : testCases) {
|
||||
auto state = profile.CalculateInflectionPoint(std::get<0>(testCase),
|
||||
std::get<1>(testCase));
|
||||
EXPECT_NEAR_UNITS(std::get<2>(testCase).position / 1_m,
|
||||
state.position / 1_m, 1e-3);
|
||||
EXPECT_NEAR_UNITS(std::get<2>(testCase).velocity / 1_mps,
|
||||
state.velocity / 1_mps, 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, TimingToCurrent) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s);
|
||||
}
|
||||
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, TimingToGoal) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
auto prediction = profile.TimeLeftUntil(state, goal);
|
||||
auto reachedGoal = false;
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
if (!reachedGoal && state == goal) {
|
||||
EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(ExponentialProfileTest, TimingToNegativeGoal) {
|
||||
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
|
||||
12_V, -kV / kA, 1 / kA};
|
||||
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
|
||||
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
|
||||
0.43277_V / 1_mps_sq};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
|
||||
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
|
||||
|
||||
auto prediction = profile.TimeLeftUntil(state, goal);
|
||||
auto reachedGoal = false;
|
||||
|
||||
for (int i = 0; i < 900; ++i) {
|
||||
state = CheckDynamics(profile, constraints, feedforward, state, goal);
|
||||
if (!reachedGoal && state == goal) {
|
||||
EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
Reference in New Issue
Block a user