mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Add TrapezoidProfile class (#1673)
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
804926fb5b
commit
9b798d228f
@@ -34,6 +34,7 @@ includeOtherLibs {
|
||||
^ntcore
|
||||
^opencv2/
|
||||
^support/
|
||||
^units/
|
||||
^vision/
|
||||
^wpi/
|
||||
}
|
||||
|
||||
158
wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
Normal file
158
wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
Normal file
@@ -0,0 +1,158 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/trajectory/TrapezoidProfile.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
|
||||
State initial)
|
||||
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
|
||||
m_constraints(constraints),
|
||||
m_initial(Direct(initial)),
|
||||
m_goal(Direct(goal)) {
|
||||
if (m_initial.velocity > m_constraints.maxVelocity) {
|
||||
m_initial.velocity = m_constraints.maxVelocity;
|
||||
}
|
||||
|
||||
// Deal with a possibly truncated motion profile (with nonzero initial or
|
||||
// final velocity) by calculating the parameters as if the profile began and
|
||||
// ended at zero velocity
|
||||
units::second_t cutoffBegin =
|
||||
m_initial.velocity / m_constraints.maxAcceleration;
|
||||
units::meter_t cutoffDistBegin =
|
||||
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
|
||||
units::meter_t cutoffDistEnd =
|
||||
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
// Now we can calculate the parameters as if it was a full trapezoid instead
|
||||
// of a truncated one
|
||||
|
||||
units::meter_t fullTrapezoidDist =
|
||||
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
|
||||
units::second_t accelerationTime =
|
||||
m_constraints.maxVelocity / m_constraints.maxAcceleration;
|
||||
|
||||
units::meter_t fullSpeedDist =
|
||||
fullTrapezoidDist -
|
||||
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
|
||||
|
||||
// Handle the case where the profile never reaches full speed
|
||||
if (fullSpeedDist < 0_m) {
|
||||
accelerationTime =
|
||||
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
|
||||
fullSpeedDist = 0_m;
|
||||
}
|
||||
|
||||
m_endAccel = accelerationTime - cutoffBegin;
|
||||
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
|
||||
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
|
||||
}
|
||||
|
||||
TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
|
||||
State result = m_initial;
|
||||
|
||||
if (t < m_endAccel) {
|
||||
result.velocity += t * m_constraints.maxAcceleration;
|
||||
result.position +=
|
||||
(m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
|
||||
} else if (t < m_endFullSpeed) {
|
||||
result.velocity = m_constraints.maxVelocity;
|
||||
result.position += (m_initial.velocity +
|
||||
m_endAccel * m_constraints.maxAcceleration / 2.0) *
|
||||
m_endAccel +
|
||||
m_constraints.maxVelocity * (t - m_endAccel);
|
||||
} else if (t <= m_endDeccel) {
|
||||
result.velocity =
|
||||
m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
|
||||
units::second_t timeLeft = m_endDeccel - t;
|
||||
result.position =
|
||||
m_goal.position -
|
||||
(m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
|
||||
timeLeft;
|
||||
} else {
|
||||
result = m_goal;
|
||||
}
|
||||
|
||||
return Direct(result);
|
||||
}
|
||||
|
||||
units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
|
||||
units::meter_t position = m_initial.position * m_direction;
|
||||
units::meters_per_second_t velocity = m_initial.velocity * m_direction;
|
||||
|
||||
units::second_t endAccel = m_endAccel * m_direction;
|
||||
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
|
||||
|
||||
if (target < position) {
|
||||
endAccel *= -1.0;
|
||||
endFullSpeed *= -1.0;
|
||||
velocity *= -1.0;
|
||||
}
|
||||
|
||||
endAccel = units::math::max(endAccel, 0_s);
|
||||
endFullSpeed = units::math::max(endFullSpeed, 0_s);
|
||||
units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
|
||||
endDeccel = units::math::max(endDeccel, 0_s);
|
||||
|
||||
const units::meters_per_second_squared_t acceleration =
|
||||
m_constraints.maxAcceleration;
|
||||
const units::meters_per_second_squared_t decceleration =
|
||||
-m_constraints.maxAcceleration;
|
||||
|
||||
units::meter_t distToTarget = units::math::abs(target - position);
|
||||
|
||||
if (distToTarget < 1e-6_m) {
|
||||
return 0_s;
|
||||
}
|
||||
|
||||
units::meter_t accelDist =
|
||||
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
|
||||
|
||||
units::meters_per_second_t deccelVelocity;
|
||||
if (endAccel > 0_s) {
|
||||
deccelVelocity = units::math::sqrt(
|
||||
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
|
||||
} else {
|
||||
deccelVelocity = velocity;
|
||||
}
|
||||
|
||||
units::meter_t deccelDist =
|
||||
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
|
||||
|
||||
deccelDist = units::math::max(deccelDist, 0_m);
|
||||
|
||||
units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
|
||||
|
||||
if (accelDist > distToTarget) {
|
||||
accelDist = distToTarget;
|
||||
fullSpeedDist = 0_m;
|
||||
deccelDist = 0_m;
|
||||
} else if (accelDist + fullSpeedDist > distToTarget) {
|
||||
fullSpeedDist = distToTarget - accelDist;
|
||||
deccelDist = 0_m;
|
||||
} else {
|
||||
deccelDist = distToTarget - fullSpeedDist - accelDist;
|
||||
}
|
||||
|
||||
units::second_t accelTime =
|
||||
(-velocity + units::math::sqrt(units::math::abs(
|
||||
velocity * velocity + 2 * acceleration * accelDist))) /
|
||||
acceleration;
|
||||
|
||||
units::second_t deccelTime =
|
||||
(-deccelVelocity +
|
||||
units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
|
||||
2 * decceleration * deccelDist))) /
|
||||
decceleration;
|
||||
|
||||
units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
|
||||
|
||||
return accelTime + fullSpeedTime + deccelTime;
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* A trapezoid-shaped velocity profile.
|
||||
*
|
||||
* While this class can be used for a profiled movement from start to finish,
|
||||
* the intended usage is to filter a reference's dynamics based on trapezoidal
|
||||
* velocity constraints. To compute the reference obeying this constraint, do
|
||||
* the following.
|
||||
*
|
||||
* Initialization:
|
||||
* @code{.cpp}
|
||||
* TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
|
||||
* double previousProfiledReference = initialReference;
|
||||
* @endcode
|
||||
*
|
||||
* Run on update:
|
||||
* @code{.cpp}
|
||||
* TrapezoidalMotionProfile profile{constraints, unprofiledReference,
|
||||
* previousProfiledReference};
|
||||
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
|
||||
* @endcode
|
||||
*
|
||||
* where `unprofiledReference` is free to change between calls. Note that when
|
||||
* the unprofiled reference is within the constraints, `Calculate()` returns the
|
||||
* unprofiled reference unchanged.
|
||||
*
|
||||
* Otherwise, a timer can be started to provide monotonic values for
|
||||
* `Calculate()` and to determine when the profile has completed via
|
||||
* `IsFinished()`.
|
||||
*/
|
||||
class TrapezoidProfile {
|
||||
public:
|
||||
class Constraints {
|
||||
public:
|
||||
units::meters_per_second_t maxVelocity = 0_mps;
|
||||
units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
|
||||
};
|
||||
|
||||
class State {
|
||||
public:
|
||||
units::meter_t position = 0_m;
|
||||
units::meters_per_second_t velocity = 0_mps;
|
||||
bool operator==(const State& rhs) const {
|
||||
return position == rhs.position && velocity == rhs.velocity;
|
||||
}
|
||||
bool operator!=(const State& rhs) const { return !(*this == rhs); }
|
||||
};
|
||||
|
||||
/**
|
||||
* Construct a TrapezoidProfile.
|
||||
*
|
||||
* @param constraints The constraints on the profile, like maximum velocity.
|
||||
* @param goal The desired state when the profile is complete.
|
||||
* @param initial The initial state (usually the current state).
|
||||
*/
|
||||
TrapezoidProfile(Constraints constraints, State goal,
|
||||
State initial = State{0_m, 0_mps});
|
||||
|
||||
/**
|
||||
* Calculate the correct position and velocity for the profile at a time t
|
||||
* where the beginning of the profile was at time t = 0.
|
||||
*
|
||||
* @param t The time since the beginning of the profile.
|
||||
*/
|
||||
State Calculate(units::second_t t) const;
|
||||
|
||||
/**
|
||||
* Returns the time left until a target distance in the profile is reached.
|
||||
*
|
||||
* @param target The target distance.
|
||||
*/
|
||||
units::second_t TimeLeftUntil(units::meter_t target) const;
|
||||
|
||||
/**
|
||||
* Returns the total time the profile takes to reach the goal.
|
||||
*/
|
||||
units::second_t TotalTime() const { return m_endDeccel; }
|
||||
|
||||
/**
|
||||
* Returns true if the profile has reached the goal.
|
||||
*
|
||||
* The profile has reached the goal if the time since the profile started
|
||||
* has exceeded the profile's total time.
|
||||
*
|
||||
* @param t The time since the beginning of the profile.
|
||||
*/
|
||||
bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
|
||||
|
||||
private:
|
||||
/**
|
||||
* Returns true if the profile inverted.
|
||||
*
|
||||
* The profile is inverted if goal position is less than the initial position.
|
||||
*
|
||||
* @param initial The initial state (usually the current state).
|
||||
* @param goal The desired state when the profile is complete.
|
||||
*/
|
||||
static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
|
||||
return initial.position > goal.position;
|
||||
}
|
||||
|
||||
// Flip the sign of the velocity and position if the profile is inverted
|
||||
State Direct(const State& in) const {
|
||||
State result = in;
|
||||
result.position *= m_direction;
|
||||
result.velocity *= m_direction;
|
||||
return result;
|
||||
}
|
||||
|
||||
// The direction of the profile, either 1 for forwards or -1 for inverted
|
||||
int m_direction;
|
||||
|
||||
Constraints m_constraints;
|
||||
State m_initial;
|
||||
State m_goal;
|
||||
|
||||
units::second_t m_endAccel;
|
||||
units::second_t m_endFullSpeed;
|
||||
units::second_t m_endDeccel;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
223
wpilibc/src/test/native/cpp/TrapezoidProfileTest.cpp
Normal file
223
wpilibc/src/test/native/cpp/TrapezoidProfileTest.cpp
Normal file
@@ -0,0 +1,223 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/trajectory/TrapezoidProfile.h" // NOLINT(build/include_order)
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
static constexpr auto kDt = 10_ms;
|
||||
|
||||
#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); \
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, ReachesGoal) {
|
||||
frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{3_m, 0_mps};
|
||||
frc::TrapezoidProfile::State state;
|
||||
|
||||
for (int i = 0; i < 450; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
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(TrapezoidProfileTest, PosContinousUnderVelChange) {
|
||||
frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{12_m, 0_mps};
|
||||
|
||||
frc::TrapezoidProfile profile{constraints, goal};
|
||||
auto state = profile.Calculate(kDt);
|
||||
|
||||
auto lastPos = state.position;
|
||||
for (int i = 0; i < 1600; ++i) {
|
||||
if (i == 400) {
|
||||
constraints.maxVelocity = 0.75_mps;
|
||||
}
|
||||
|
||||
profile = frc::TrapezoidProfile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
auto estimatedVel = (state.position - lastPos) / kDt;
|
||||
|
||||
if (i >= 400) {
|
||||
// Since estimatedVel can have floating point rounding errors, we check
|
||||
// whether value is less than or within an error delta of the new
|
||||
// constraint.
|
||||
EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
|
||||
|
||||
EXPECT_LE(state.velocity, constraints.maxVelocity);
|
||||
}
|
||||
|
||||
lastPos = state.position;
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// There is some somewhat tricky code for dealing with going backwards
|
||||
TEST(TrapezoidProfileTest, Backwards) {
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
|
||||
frc::TrapezoidProfile::State state;
|
||||
|
||||
for (int i = 0; i < 400; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
|
||||
frc::TrapezoidProfile::State state;
|
||||
|
||||
for (int i = 0; i < 200; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
EXPECT_NE(state, goal);
|
||||
|
||||
goal = {0.0_m, 0.0_mps};
|
||||
for (int i = 0; i < 550; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed
|
||||
TEST(TrapezoidProfileTest, TopSpeed) {
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{4_m, 0_mps};
|
||||
frc::TrapezoidProfile::State state;
|
||||
|
||||
for (int i = 0; i < 200; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
|
||||
|
||||
for (int i = 0; i < 2000; ++i) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
}
|
||||
EXPECT_EQ(state, goal);
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, TimingToCurrent) {
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{2_m, 0_mps};
|
||||
frc::TrapezoidProfile::State state;
|
||||
|
||||
for (int i = 0; i < 400; i++) {
|
||||
frc::TrapezoidProfile profile{constraints, goal, state};
|
||||
state = profile.Calculate(kDt);
|
||||
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, TimingToGoal) {
|
||||
using units::unit_cast;
|
||||
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{2_m, 0_mps};
|
||||
|
||||
frc::TrapezoidProfile profile{constraints, goal};
|
||||
auto state = profile.Calculate(kDt);
|
||||
|
||||
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
|
||||
bool reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = frc::TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.Calculate(kDt);
|
||||
if (!reachedGoal && state == 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
|
||||
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
|
||||
using units::unit_cast;
|
||||
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{2_m, 0_mps};
|
||||
|
||||
frc::TrapezoidProfile profile{constraints, goal};
|
||||
auto state = profile.Calculate(kDt);
|
||||
|
||||
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
|
||||
bool reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = frc::TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.Calculate(kDt);
|
||||
if (!reachedGoal &&
|
||||
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
|
||||
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
|
||||
using units::unit_cast;
|
||||
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
|
||||
|
||||
frc::TrapezoidProfile profile{constraints, goal};
|
||||
auto state = profile.Calculate(kDt);
|
||||
|
||||
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
|
||||
bool reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = frc::TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.Calculate(kDt);
|
||||
if (!reachedGoal && state == 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
|
||||
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
|
||||
using units::unit_cast;
|
||||
|
||||
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
|
||||
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
|
||||
|
||||
frc::TrapezoidProfile profile{constraints, goal};
|
||||
auto state = profile.Calculate(kDt);
|
||||
|
||||
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
|
||||
bool reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = frc::TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.Calculate(kDt);
|
||||
if (!reachedGoal &&
|
||||
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
|
||||
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,294 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.trajectory;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* A trapezoid-shaped velocity profile.
|
||||
*
|
||||
* <p>While this class can be used for a profiled movement from start to finish,
|
||||
* the intended usage is to filter a reference's dynamics based on trapezoidal
|
||||
* velocity constraints. To compute the reference obeying this constraint, do
|
||||
* the following.
|
||||
*
|
||||
* <p>Initialization:
|
||||
* <pre><code>
|
||||
* TrapezoidProfile.Constraints constraints =
|
||||
* new TrapezoidProfile.Constraints(kMaxV, kMaxA);
|
||||
* TrapezoidProfile.State previousProfiledReference =
|
||||
* new TrapezoidProfile.State(initialReference, 0.0);
|
||||
* </code></pre>
|
||||
*
|
||||
* <p>Run on update:
|
||||
* <pre><code>
|
||||
* TrapezoidProfile profile =
|
||||
* new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
|
||||
* previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
|
||||
* </code></pre>
|
||||
*
|
||||
* <p>where `unprofiledReference` is free to change between calls. Note that when
|
||||
* the unprofiled reference is within the constraints, `calculate()` returns the
|
||||
* unprofiled reference unchanged.
|
||||
*
|
||||
* <p>Otherwise, a timer can be started to provide monotonic values for
|
||||
* `calculate()` and to determine when the profile has completed via
|
||||
* `isFinished()`.
|
||||
*/
|
||||
public class TrapezoidProfile {
|
||||
// The direction of the profile, either 1 for forwards or -1 for inverted
|
||||
private int m_direction;
|
||||
|
||||
private Constraints m_constraints;
|
||||
private State m_initial;
|
||||
private State m_goal;
|
||||
|
||||
private double m_endAccel;
|
||||
private double m_endFullSpeed;
|
||||
private double m_endDeccel;
|
||||
|
||||
public static class Constraints {
|
||||
@SuppressWarnings("MemberName")
|
||||
public double maxVelocity;
|
||||
@SuppressWarnings("MemberName")
|
||||
public double maxAcceleration;
|
||||
|
||||
public Constraints() {
|
||||
}
|
||||
|
||||
public Constraints(double maxVelocity, double maxAcceleration) {
|
||||
this.maxVelocity = maxVelocity;
|
||||
this.maxAcceleration = maxAcceleration;
|
||||
}
|
||||
}
|
||||
|
||||
public static class State {
|
||||
@SuppressWarnings("MemberName")
|
||||
public double position;
|
||||
@SuppressWarnings("MemberName")
|
||||
public double velocity;
|
||||
|
||||
public State() {
|
||||
}
|
||||
|
||||
public State(double position, double velocity) {
|
||||
this.position = position;
|
||||
this.velocity = velocity;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object other) {
|
||||
if (other instanceof State) {
|
||||
State rhs = (State) other;
|
||||
return this.position == rhs.position && this.velocity == rhs.velocity;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(position, velocity);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a TrapezoidProfile.
|
||||
*
|
||||
* @param constraints The constraints on the profile, like maximum velocity.
|
||||
* @param goal The desired state when the profile is complete.
|
||||
* @param initial The initial state (usually the current state).
|
||||
*/
|
||||
public TrapezoidProfile(Constraints constraints, State goal, State initial) {
|
||||
m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
|
||||
m_constraints = constraints;
|
||||
m_initial = direct(initial);
|
||||
m_goal = direct(goal);
|
||||
|
||||
if (m_initial.velocity > m_constraints.maxVelocity) {
|
||||
m_initial.velocity = m_constraints.maxVelocity;
|
||||
}
|
||||
|
||||
// Deal with a possibly truncated motion profile (with nonzero initial or
|
||||
// final velocity) by calculating the parameters as if the profile began and
|
||||
// ended at zero velocity
|
||||
double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
|
||||
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
|
||||
double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
// Now we can calculate the parameters as if it was a full trapezoid instead
|
||||
// of a truncated one
|
||||
|
||||
double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
|
||||
+ cutoffDistEnd;
|
||||
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
|
||||
|
||||
double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
|
||||
* m_constraints.maxAcceleration;
|
||||
|
||||
// Handle the case where the profile never reaches full speed
|
||||
if (fullSpeedDist < 0) {
|
||||
accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
|
||||
fullSpeedDist = 0;
|
||||
}
|
||||
|
||||
m_endAccel = accelerationTime - cutoffBegin;
|
||||
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
|
||||
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct a TrapezoidProfile.
|
||||
*
|
||||
* @param constraints The constraints on the profile, like maximum velocity.
|
||||
* @param goal The desired state when the profile is complete.
|
||||
*/
|
||||
public TrapezoidProfile(Constraints constraints, State goal) {
|
||||
this(constraints, goal, new State(0, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the correct position and velocity for the profile at a time t
|
||||
* where the beginning of the profile was at time t = 0.
|
||||
*
|
||||
* @param t The time since the beginning of the profile.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public State calculate(double t) {
|
||||
State result = m_initial;
|
||||
|
||||
if (t < m_endAccel) {
|
||||
result.velocity += t * m_constraints.maxAcceleration;
|
||||
result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
|
||||
} else if (t < m_endFullSpeed) {
|
||||
result.velocity = m_constraints.maxVelocity;
|
||||
result.position += (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration
|
||||
/ 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel);
|
||||
} else if (t <= m_endDeccel) {
|
||||
result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
|
||||
double timeLeft = m_endDeccel - t;
|
||||
result.position = m_goal.position - (m_goal.velocity + timeLeft
|
||||
* m_constraints.maxAcceleration / 2.0) * timeLeft;
|
||||
} else {
|
||||
result = m_goal;
|
||||
}
|
||||
|
||||
return direct(result);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the time left until a target distance in the profile is reached.
|
||||
*
|
||||
* @param target The target distance.
|
||||
*/
|
||||
public double timeLeftUntil(double target) {
|
||||
double position = m_initial.position * m_direction;
|
||||
double velocity = m_initial.velocity * m_direction;
|
||||
|
||||
double endAccel = m_endAccel * m_direction;
|
||||
double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
|
||||
|
||||
if (target < position) {
|
||||
endAccel = -endAccel;
|
||||
endFullSpeed = -endFullSpeed;
|
||||
velocity = -velocity;
|
||||
}
|
||||
|
||||
endAccel = Math.max(endAccel, 0);
|
||||
endFullSpeed = Math.max(endFullSpeed, 0);
|
||||
double endDeccel = m_endDeccel - endAccel - endFullSpeed;
|
||||
endDeccel = Math.max(endDeccel, 0);
|
||||
|
||||
final double acceleration = m_constraints.maxAcceleration;
|
||||
final double decceleration = -m_constraints.maxAcceleration;
|
||||
|
||||
double distToTarget = Math.abs(target - position);
|
||||
if (distToTarget < 1e-6) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
|
||||
|
||||
double deccelVelocity;
|
||||
if (endAccel > 0) {
|
||||
deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
|
||||
} else {
|
||||
deccelVelocity = velocity;
|
||||
}
|
||||
|
||||
double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
|
||||
|
||||
deccelDist = Math.max(deccelDist, 0);
|
||||
|
||||
double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
|
||||
|
||||
if (accelDist > distToTarget) {
|
||||
accelDist = distToTarget;
|
||||
fullSpeedDist = 0;
|
||||
deccelDist = 0;
|
||||
} else if (accelDist + fullSpeedDist > distToTarget) {
|
||||
fullSpeedDist = distToTarget - accelDist;
|
||||
deccelDist = 0;
|
||||
} else {
|
||||
deccelDist = distToTarget - fullSpeedDist - accelDist;
|
||||
}
|
||||
|
||||
double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
|
||||
* accelDist))) / acceleration;
|
||||
|
||||
double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
|
||||
+ 2 * decceleration * deccelDist))) / decceleration;
|
||||
|
||||
double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
|
||||
|
||||
return accelTime + fullSpeedTime + deccelTime;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the total time the profile takes to reach the goal.
|
||||
*/
|
||||
public double totalTime() {
|
||||
return m_endDeccel;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the profile has reached the goal.
|
||||
*
|
||||
* <p>The profile has reached the goal if the time since the profile started
|
||||
* has exceeded the profile's total time.
|
||||
*
|
||||
* @param t The time since the beginning of the profile.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public boolean isFinished(double t) {
|
||||
return t >= totalTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the profile inverted.
|
||||
*
|
||||
* <p>The profile is inverted if goal position is less than the initial position.
|
||||
*
|
||||
* @param initial The initial state (usually the current state).
|
||||
* @param goal The desired state when the profile is complete.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
private static boolean shouldFlipAcceleration(State initial, State goal) {
|
||||
return initial.position > goal.position;
|
||||
}
|
||||
|
||||
// Flip the sign of the velocity and position if the profile is inverted
|
||||
private State direct(State in) {
|
||||
State result = new State(in.position, in.velocity);
|
||||
result.position = result.position * m_direction;
|
||||
result.velocity = result.velocity * m_direction;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,259 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
|
||||
class TrapezoidProfileTest {
|
||||
private static final double kDt = 0.01;
|
||||
|
||||
/**
|
||||
* Asserts "val1" is less than or equal to "val2".
|
||||
*
|
||||
* @param val1 First operand in comparison.
|
||||
* @param val2 Second operand in comparison.
|
||||
*/
|
||||
private static void assertLessThanOrEquals(double val1, double val2) {
|
||||
assertTrue(val1 <= val2, Double.toString(val1) + " is greater than " + val2);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Asserts "val1" is less than or 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 assertLessThanOrNear(double val1, double val2, double eps) {
|
||||
if (val1 <= val2) {
|
||||
assertLessThanOrEquals(val1, val2);
|
||||
} else {
|
||||
assertNear(val1, val2, eps);
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void reachesGoal() {
|
||||
TrapezoidProfile.Constraints constraints =
|
||||
new TrapezoidProfile.Constraints(1.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
|
||||
TrapezoidProfile.State state = new TrapezoidProfile.State();
|
||||
|
||||
for (int i = 0; i < 450; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
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 posContinousUnderVelChange() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
|
||||
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
|
||||
TrapezoidProfile.State state = profile.calculate(kDt);
|
||||
|
||||
double lastPos = state.position;
|
||||
for (int i = 0; i < 1600; ++i) {
|
||||
if (i == 400) {
|
||||
constraints.maxVelocity = 0.75;
|
||||
}
|
||||
|
||||
profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
double estimatedVel = (state.position - lastPos) / kDt;
|
||||
|
||||
if (i >= 400) {
|
||||
// Since estimatedVel can have floating point rounding errors, we check
|
||||
// whether value is less than or within an error delta of the new
|
||||
// constraint.
|
||||
assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
|
||||
|
||||
assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
|
||||
}
|
||||
|
||||
lastPos = state.position;
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// There is some somewhat tricky code for dealing with going backwards
|
||||
@Test
|
||||
void backwards() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
|
||||
TrapezoidProfile.State state = new TrapezoidProfile.State();
|
||||
|
||||
for (int i = 0; i < 400; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void switchGoalInMiddle() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
|
||||
TrapezoidProfile.State state = new TrapezoidProfile.State();
|
||||
|
||||
for (int i = 0; i < 200; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
assertNotEquals(state, goal);
|
||||
|
||||
goal = new TrapezoidProfile.State(0.0, 0.0);
|
||||
for (int i = 0; i < 550; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
// Checks to make sure that it hits top speed
|
||||
@Test
|
||||
void topSpeed() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
|
||||
TrapezoidProfile.State state = new TrapezoidProfile.State();
|
||||
|
||||
for (int i = 0; i < 200; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
|
||||
|
||||
for (int i = 0; i < 2000; ++i) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
}
|
||||
assertEquals(state, goal);
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToCurrent() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
|
||||
TrapezoidProfile.State state = new TrapezoidProfile.State();
|
||||
|
||||
for (int i = 0; i < 400; i++) {
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToGoal() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
|
||||
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
|
||||
TrapezoidProfile.State state = profile.calculate(kDt);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
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 timingBeforeGoal() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
|
||||
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
|
||||
TrapezoidProfile.State state = profile.calculate(kDt);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(1);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
|
||||
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void timingToNegativeGoal() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
|
||||
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
|
||||
TrapezoidProfile.State state = profile.calculate(kDt);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
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 timingBeforeNegativeGoal() {
|
||||
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
|
||||
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
|
||||
|
||||
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
|
||||
TrapezoidProfile.State state = profile.calculate(kDt);
|
||||
|
||||
double predictedTimeLeft = profile.timeLeftUntil(-1);
|
||||
boolean reachedGoal = false;
|
||||
for (int i = 0; i < 400; i++) {
|
||||
profile = new TrapezoidProfile(constraints, goal, state);
|
||||
state = profile.calculate(kDt);
|
||||
if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
|
||||
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
|
||||
reachedGoal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user