mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpimath] Add Exponential motion profile (#5720)
This commit is contained in:
Jordan McMichael
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user