Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add Exponential motion profile (#5720)

Browse Source
This commit is contained in:
Jordan McMichael
2023-10-19 20:26:32 -04:00
committed by GitHub
parent 7c6fe56cf2
commit ecb7cfa9ef
24 changed files with 2663 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

449
wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java Normal file
View File

@@ -0,0 +1,449 @@
// 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 java.util.Objects;
/**
* A exponential curve-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 state-space model dynamics. To compute the reference
* obeying this constraint, do the following.
*
* <p>Initialization:
*
* <pre><code>
* ExponentialProfile.Constraints constraints =
* ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA);
* ExponentialProfile.State previousProfiledReference =
* new ExponentialProfile.State(initialReference, 0.0);
* ExponentialProfile profile = new ExponentialProfile(constraints);
* </code></pre>
*
* <p>Run on update:
*
* <pre><code>
* previousProfiledReference =
* profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
* </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 ExponentialProfile {
private final Constraints m_constraints;
public static class ProfileTiming {
public final double inflectionTime;
public final double totalTime;
protected ProfileTiming(double inflectionTime, double totalTime) {
this.inflectionTime = inflectionTime;
this.totalTime = totalTime;
}
/**
* Decides if the profile is finished by time t.
*
* @param t The time since the beginning of the profile.
* @return if the profile is finished at time t.
*/
public boolean isFinished(double t) {
return t > inflectionTime;
}
}
public static class Constraints {
public final double maxInput;
public final double A;
public final double B;
/**
* Construct constraints for an ExponentialProfile.
*
* @param maxInput maximum unsigned input voltage
* @param A The State-Space 1x1 system matrix.
* @param B The State-Space 1x1 input matrix.
*/
private Constraints(double maxInput, double A, double B) {
this.maxInput = maxInput;
this.A = A;
this.B = B;
}
/**
* Computes the max achievable velocity for an Exponential Profile.
*
* @return The seady-state velocity achieved by this profile.
*/
public double maxVelocity() {
return -maxInput * B / A;
}
/**
* Construct constraints for an ExponentialProfile from characteristics.
*
* @param maxInput maximum unsigned input voltage
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @return The Constraints object.
*/
public static Constraints fromCharacteristics(double maxInput, double kV, double kA) {
return new Constraints(maxInput, -kV / kA, 1.0 / kA);
}
/**
* Construct constraints for an ExponentialProfile from State-Space parameters.
*
* @param maxInput maximum unsigned input voltage
* @param A The State-Space 1x1 system matrix.
* @param B The State-Space 1x1 input matrix.
* @return The Constraints object.
*/
public static Constraints fromStateSpace(double maxInput, double A, double B) {
return new Constraints(maxInput, A, B);
}
}
public static class State {
public final double position;
public final double velocity;
/**
* Construct a state within an exponential profile.
*
* @param position The position at this state.
* @param velocity The velocity at this 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 an ExponentialProfile.
*
* @param constraints The constraints on the profile, like maximum input.
*/
public ExponentialProfile(Constraints constraints) {
m_constraints = constraints;
}
/**
* Calculate the correct position and velocity for the profile at a time t where the current state
* is at time t = 0.
*
* @param t The time since the beginning of the profile.
* @param current The current state.
* @param goal The desired state when the profile is complete.
* @return The position and velocity of the profile at time t.
*/
public State calculate(double t, State current, State goal) {
var direction = shouldFlipInput(current, goal) ? -1 : 1;
var u = direction * m_constraints.maxInput;
var inflectionPoint = calculateInflectionPoint(current, goal, u);
var timing = calculateProfileTiming(current, inflectionPoint, goal, u);
if (t < 0) {
return current;
} else if (t < timing.inflectionTime) {
return new State(
computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current));
} else if (t < timing.totalTime) {
return new State(
computeDistanceFromTime(t - timing.totalTime, -u, goal),
computeVelocityFromTime(t - timing.totalTime, -u, goal));
} else {
return goal;
}
}
/**
* Calculate the point after which the fastest way to reach the goal state is to apply input in
* the opposite direction.
*
* @param current The current state.
* @param goal The desired state when the profile is complete.
* @return The position and velocity of the profile at the inflection point.
*/
public State calculateInflectionPoint(State current, State goal) {
var direction = shouldFlipInput(current, goal) ? -1 : 1;
var u = direction * m_constraints.maxInput;
return calculateInflectionPoint(current, goal, u);
}
/**
* Calculate the point after which the fastest way to reach the goal state is to apply input in
* the opposite direction.
*
* @param current The current state.
* @param goal The desired state when the profile is complete.
* @param input The signed input applied to this profile from the current state.
* @return The position and velocity of the profile at the inflection point.
*/
private State calculateInflectionPoint(State current, State goal, double input) {
var u = input;
if (current.equals(goal)) {
return current;
}
var inflectionVelocity = solveForInflectionVelocity(u, current, goal);
var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal);
return new State(inflectionPosition, inflectionVelocity);
}
/**
* Calculate the time it will take for this profile to reach the goal state.
*
* @param current The current state.
* @param goal The desired state when the profile is complete.
* @return The total duration of this profile.
*/
public double timeLeftUntil(State current, State goal) {
var timing = calculateProfileTiming(current, goal);
return timing.totalTime;
}
/**
* Calculate the time it will take for this profile to reach the inflection point, and the time it
* will take for this profile to reach the goal state.
*
* @param current The current state.
* @param goal The desired state when the profile is complete.
* @return The timing information for this profile.
*/
public ProfileTiming calculateProfileTiming(State current, State goal) {
var direction = shouldFlipInput(current, goal) ? -1 : 1;
var u = direction * m_constraints.maxInput;
var inflectionPoint = calculateInflectionPoint(current, goal, u);
return calculateProfileTiming(current, inflectionPoint, goal, u);
}
/**
* Calculate the time it will take for this profile to reach the inflection point, and the time it
* will take for this profile to reach the goal state.
*
* @param current The current state.
* @param inflectionPoint The inflection point of this profile.
* @param goal The desired state when the profile is complete.
* @param input The signed input applied to this profile from the current state.
* @return The timing information for this profile.
*/
private ProfileTiming calculateProfileTiming(
State current, State inflectionPoint, State goal, double input) {
var u = input;
double inflectionT_forward;
// We need to handle 5 cases here:
//
// - Approaching -maxVelocity from below
// - Approaching -maxVelocity from above
// - Approaching maxVelocity from below
// - Approaching maxVelocity from above
// - At +-maxVelocity
//
// For cases 1 and 3, we want to subtract epsilon from the inflection point velocity.
// For cases 2 and 4, we want to add epsilon to the inflection point velocity.
// For case 5, we have reached inflection point velocity.
double epsilon = 1e-9;
if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity)
< epsilon) {
double solvableV = inflectionPoint.velocity;
double t_to_solvable_v;
double x_at_solvable_v;
if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) {
t_to_solvable_v = 0;
x_at_solvable_v = current.position;
} else {
if (Math.abs(current.velocity) > m_constraints.maxVelocity()) {
solvableV += Math.signum(u) * epsilon;
} else {
solvableV -= Math.signum(u) * epsilon;
}
t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity);
x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current);
}
inflectionT_forward =
t_to_solvable_v
+ Math.signum(input)
* (inflectionPoint.position - x_at_solvable_v)
/ m_constraints.maxVelocity();
} else {
inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
}
var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward);
}
/**
* Calculate the position reached after t seconds when applying an input from the initial state.
*
* @param t The time since the initial state.
* @param input The signed input applied to this profile from the initial state.
* @param initial The initial state.
* @return The distance travelled by this profile.
*/
private double computeDistanceFromTime(double t, double input, State initial) {
var A = m_constraints.A;
var B = m_constraints.B;
var u = input;
return initial.position
+ (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A;
}
/**
* Calculate the velocity reached after t seconds when applying an input from the initial state.
*
* @param t The time since the initial state.
* @param input The signed input applied to this profile from the initial state.
* @param initial The initial state.
* @return The distance travelled by this profile.
*/
private double computeVelocityFromTime(double t, double input, State initial) {
var A = m_constraints.A;
var B = m_constraints.B;
var u = input;
return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A;
}
/**
* Calculate the time required to reach a specified velocity given the initial velocity.
*
* @param velocity The goal velocity.
* @param input The signed input applied to this profile from the initial state.
* @param initial The initial velocity.
* @return The time required to reach the goal velocity.
*/
private double computeTimeFromVelocity(double velocity, double input, double initial) {
var A = m_constraints.A;
var B = m_constraints.B;
var u = input;
return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A;
}
/**
* Calculate the distance reached at the same time as the given velocity when applying the given
* input from the initial state.
*
* @param velocity The velocity reached by this profile
* @param input The signed input applied to this profile from the initial state.
* @param initial The initial state.
* @return The distance reached when the given velocity is reached.
*/
private double computeDistanceFromVelocity(double velocity, double input, State initial) {
var A = m_constraints.A;
var B = m_constraints.B;
var u = input;
return initial.position
+ (velocity - initial.velocity) / A
- B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u));
}
/**
* Calculate the velocity at which input should be reversed in order to reach the goal state from
* the current state.
*
* @param input The signed input applied to this profile from the current state.
* @param current The current state.
* @param goal The goal state.
* @return The inflection velocity.
*/
private double solveForInflectionVelocity(double input, State current, State goal) {
var A = m_constraints.A;
var B = m_constraints.B;
var u = input;
var U_dir = Math.signum(u);
var position_delta = goal.position - current.position;
var velocity_delta = goal.velocity - current.velocity;
var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
var power = -A / B / u * (A * position_delta - velocity_delta);
var a = -A * A;
var c = (B * B) * (u * u) + scalar * Math.exp(power);
if (-1e-9 < c && c < 0) {
// Numerical stability issue - the heuristic gets it right but c is around -1e-13
return 0;
}
return U_dir * Math.sqrt(-c / a);
}
/**
* Returns true if the profile should be inverted.
*
* <p>The profile is inverted if we should first apply negative input in order to reach the goal
* state.
*
* @param current The initial state (usually the current state).
* @param goal The desired state when the profile is complete.
*/
@SuppressWarnings("UnnecessaryParentheses")
private boolean shouldFlipInput(State current, State goal) {
var u = m_constraints.maxInput;
var xf = goal.position;
var v0 = current.velocity;
var vf = goal.velocity;
var x_forward = computeDistanceFromVelocity(vf, u, current);
var x_reverse = computeDistanceFromVelocity(vf, -u, current);
if (v0 >= m_constraints.maxVelocity()) {
return xf < x_reverse;
}
if (v0 <= -m_constraints.maxVelocity()) {
return xf < x_forward;
}
var a = v0 >= 0;
var b = vf >= 0;
var c = xf >= x_forward;
var d = xf >= x_reverse;
return (a && !d) || (b && !c) || (!c && !d);
}
}

194
wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h Normal file
View File

@@ -0,0 +1,194 @@
// 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.
#pragma once
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
/**
* A Exponential-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
* ExponentialProfile velocity constraints. To compute the reference obeying
* this constraint, do the following.
*
* Initialization:
* @code{.cpp}
* ExponentialProfile::Constraints constraints{kMaxV, kV, kA};
* State previousProfiledReference = {initialReference, 0_mps};
* @endcode
*
* Run on update:
* @code{.cpp}
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
* previousProfiledReference, unprofiledReference);
* @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()`.
*/
template <class Distance, class Input>
class ExponentialProfile {
public:
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Input_t = units::unit_t<Input>;
using A_t = units::unit_t<units::inverse<units::seconds>>;
using B_t =
units::unit_t<units::compound_unit<Acceleration, units::inverse<Input>>>;
using KV = units::compound_unit<Input, units::inverse<Velocity>>;
using kV_t = units::unit_t<KV>;
using KA = units::compound_unit<Input, units::inverse<Acceleration>>;
using kA_t = units::unit_t<KA>;
class Constraints {
public:
Constraints(Input_t maxInput, A_t A, B_t B)
: maxInput{maxInput}, A{A}, B{B} {}
Constraints(Input_t maxInput, kV_t kV, kA_t kA)
: maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {}
Velocity_t MaxVelocity() const { return -maxInput * B / A; }
Input_t maxInput{0};
A_t A{0};
B_t B{0};
};
class State {
public:
Distance_t position{0};
Velocity_t velocity{0};
bool operator==(const State&) const = default;
};
class ProfileTiming {
public:
units::second_t inflectionTime;
units::second_t totalTime;
bool IsFinished(const units::second_t& time) const {
return time > totalTime;
}
};
/**
* Construct a ExponentialProfile.
*
* @param constraints The constraints on the profile, like maximum input.
*/
explicit ExponentialProfile(Constraints constraints);
ExponentialProfile(const ExponentialProfile&) = default;
ExponentialProfile& operator=(const ExponentialProfile&) = default;
ExponentialProfile(ExponentialProfile&&) = default;
ExponentialProfile& operator=(ExponentialProfile&&) = default;
/**
* Calculate the correct position and velocity for the profile at a time t
* where the current state is at time t = 0.
*/
State Calculate(const units::second_t& t, const State& current,
const State& goal) const;
/**
* Calculate the point after which the fastest way to reach the goal state is
* to apply input in the opposite direction.
*/
State CalculateInflectionPoint(const State& current, const State& goal) const;
/**
* Calculate the time it will take for this profile to reach the goal state.
*/
units::second_t TimeLeftUntil(const State& current, const State& goal) const;
/**
* Calculate the time it will take for this profile to reach the inflection
* point, and the time it will take for this profile to reach the goal state.
*/
ProfileTiming CalculateProfileTiming(const State& current,
const State& goal) const;
private:
/**
* Calculate the point after which the fastest way to reach the goal state is
* to apply input in the opposite direction.
*/
State CalculateInflectionPoint(const State& current, const State& goal,
const Input_t& input) const;
/**
* Calculate the time it will take for this profile to reach the inflection
* point, and the time it will take for this profile to reach the goal state.
*/
ProfileTiming CalculateProfileTiming(const State& current,
const State& inflectionPoint,
const State& goal,
const Input_t& input) const;
/**
* Calculate the velocity reached after t seconds when applying an input from
* the initial state.
*/
Velocity_t ComputeVelocityFromTime(const units::second_t& time,
const Input_t& input,
const State& initial) const;
/**
* Calculate the position reached after t seconds when applying an input from
* the initial state.
*/
Distance_t ComputeDistanceFromTime(const units::second_t& time,
const Input_t& input,
const State& initial) const;
/**
* Calculate the distance reached at the same time as the given velocity when
* applying the given input from the initial state.
*/
Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity,
const Input_t& input,
const State& initial) const;
/**
* Calculate the time required to reach a specified velocity given the initial
* velocity.
*/
units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity,
const Input_t& input,
const Velocity_t& initial) const;
/**
* Calculate the velocity at which input should be reversed in order to reach
* the goal state from the current state.
*/
Velocity_t SolveForInflectionVelocity(const Input_t& input,
const State& current,
const State& goal) const;
/**
* Returns true if the profile should be inverted.
*
* <p>The profile is inverted if we should first apply negative input in order
* to reach the goal state.
*/
bool ShouldFlipInput(const State& current, const State& goal) const;
Constraints m_constraints;
};
} // namespace frc
#include "ExponentialProfile.inc"

253
wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc Normal file
View File

@@ -0,0 +1,253 @@
// 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.
#pragma once
#include <algorithm>
#include <fmt/core.h>
#include "frc/trajectory/ExponentialProfile.h"
#include "units/math.h"
namespace frc {
template <class Distance, class Input>
ExponentialProfile<Distance, Input>::ExponentialProfile(Constraints constraints)
: m_constraints(constraints) {}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::Calculate(const units::second_t& t,
const State& current,
const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u);
if (t < 0_s) {
return current;
} else if (t < timing.inflectionTime) {
return {ComputeDistanceFromTime(t, u, current),
ComputeVelocityFromTime(t, u, current)};
} else if (t < timing.totalTime) {
return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal),
ComputeVelocityFromTime(t - timing.totalTime, -u, goal)};
} else {
return goal;
}
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
const State& current, const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
return CalculateInflectionPoint(current, goal, u);
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::State
ExponentialProfile<Distance, Input>::CalculateInflectionPoint(
const State& current, const State& goal, const Input_t& input) const {
auto u = input;
if (current == goal) {
return current;
}
auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal);
auto inflectionPosition =
ComputeDistanceFromVelocity(inflectionVelocity, -u, goal);
return {inflectionPosition, inflectionVelocity};
}
template <class Distance, class Input>
units::second_t ExponentialProfile<Distance, Input>::TimeLeftUntil(
const State& current, const State& goal) const {
auto timing = CalculateProfileTiming(current, goal);
return timing.totalTime;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::ProfileTiming
ExponentialProfile<Distance, Input>::CalculateProfileTiming(
const State& current, const State& goal) const {
auto direction = ShouldFlipInput(current, goal) ? -1 : 1;
auto u = direction * m_constraints.maxInput;
auto inflectionPoint = CalculateInflectionPoint(current, goal, u);
return CalculateProfileTiming(current, inflectionPoint, goal, u);
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::ProfileTiming
ExponentialProfile<Distance, Input>::CalculateProfileTiming(
const State& current, const State& inflectionPoint, const State& goal,
const Input_t& input) const {
auto u = input;
auto u_dir = units::math::abs(u) / u;
units::second_t inflectionT_forward;
// We need to handle 5 cases here:
//
// - Approaching -maxVelocity from below
// - Approaching -maxVelocity from above
// - Approaching maxVelocity from below
// - Approaching maxVelocity from above
// - At +-maxVelocity
//
// For cases 1 and 3, we want to subtract epsilon from the inflection point
// velocity For cases 2 and 4, we want to add epsilon to the inflection point
// velocity. For case 5, we have reached inflection point velocity.
auto epsilon = Velocity_t(1e-9);
if (units::math::abs(u_dir * m_constraints.MaxVelocity() -
inflectionPoint.velocity) < epsilon) {
auto solvableV = inflectionPoint.velocity;
units::second_t t_to_solvable_v;
Distance_t x_at_solvable_v;
if (units::math::abs(current.velocity - inflectionPoint.velocity) <
epsilon) {
t_to_solvable_v = 0_s;
x_at_solvable_v = current.position;
} else {
if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) {
solvableV += u_dir * epsilon;
} else {
solvableV -= u_dir * epsilon;
}
t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity);
x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current);
}
inflectionT_forward =
t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) /
m_constraints.MaxVelocity();
} else {
inflectionT_forward =
ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity);
}
auto inflectionT_backward =
ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity);
return {inflectionT_forward, inflectionT_forward - inflectionT_backward};
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Distance_t
ExponentialProfile<Distance, Input>::ComputeDistanceFromTime(
const units::second_t& time, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position +
(-B * u * time +
(initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) /
A;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Velocity_t
ExponentialProfile<Distance, Input>::ComputeVelocityFromTime(
const units::second_t& time, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return (initial.velocity + B * u / A) * units::math::exp(A * time) -
B * u / A;
}
template <class Distance, class Input>
units::second_t ExponentialProfile<Distance, Input>::ComputeTimeFromVelocity(
const Velocity_t& velocity, const Input_t& input,
const Velocity_t& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A;
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Distance_t
ExponentialProfile<Distance, Input>::ComputeDistanceFromVelocity(
const Velocity_t& velocity, const Input_t& input,
const State& initial) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
return initial.position + (velocity - initial.velocity) / A -
B * u / (A * A) *
units::math::log((A * velocity + B * u) /
(A * initial.velocity + B * u));
}
template <class Distance, class Input>
typename ExponentialProfile<Distance, Input>::Velocity_t
ExponentialProfile<Distance, Input>::SolveForInflectionVelocity(
const Input_t& input, const State& current, const State& goal) const {
auto A = m_constraints.A;
auto B = m_constraints.B;
auto u = input;
auto u_dir = u / units::math::abs(u);
auto position_delta = goal.position - current.position;
auto velocity_delta = goal.velocity - current.velocity;
auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u);
auto power = -A / B / u * (A * position_delta - velocity_delta);
auto a = -A * A;
auto c = B * B * u * u + scalar * units::math::exp(power);
if (-1e-9 < c.value() && c.value() < 0) {
// numeric instability - the heuristic gets it right but c is around -1e-13
return Velocity_t(0);
}
return u_dir * units::math::sqrt(-c / a);
}
template <class Distance, class Input>
bool ExponentialProfile<Distance, Input>::ShouldFlipInput(
const State& current, const State& goal) const {
auto u = m_constraints.maxInput;
auto v0 = current.velocity;
auto xf = goal.position;
auto vf = goal.velocity;
auto x_forward = ComputeDistanceFromVelocity(vf, u, current);
auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current);
if (v0 >= m_constraints.MaxVelocity()) {
return xf < x_reverse;
}
if (v0 <= -m_constraints.MaxVelocity()) {
return xf < x_forward;
}
auto a = v0 >= Velocity_t(0);
auto b = vf >= Velocity_t(0);
auto c = xf >= x_forward;
auto d = xf >= x_reverse;
return (a && !d) || (b && !c) || (!c && !d);
}
} // namespace frc

364
wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java Normal file
View File

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

338
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp Normal file
View File

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