Support decoupling acceleration and deceleration in TrapezoidProfile
For the catapult, we want to be able to accelerate more gently than we
stop it. Rather than try to change the trajectory in the middle to stop
nicely, teach the profile how to solve for the two separately.
Change-Id: I798d937a1f6b41da872b8023a9039136fdc8e04b
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/util/trapezoid_profile.h b/aos/util/trapezoid_profile.h
index 6669777..621ff30 100644
--- a/aos/util/trapezoid_profile.h
+++ b/aos/util/trapezoid_profile.h
@@ -9,57 +9,116 @@
namespace aos::util {
// Calculates a trapezoidal motion profile (like for a control loop's goals).
-// Supports having the end speed and position changed in the middle.
+// Supports having the destination position, acceleration, and velocity caps
+// changed in the middle, and for having different accelerations and
+// decelerations.
//
// The only units assumption that this class makes is that the unit of time is
// seconds.
-class TrapezoidProfile {
+class AsymmetricTrapezoidProfile {
public:
- // delta_time is how long between each call to Update.
- TrapezoidProfile(::std::chrono::nanoseconds delta_time);
+ // Constructs a profile. delta_time is the timestep to assume when solving.
+ AsymmetricTrapezoidProfile(::std::chrono::nanoseconds delta_time);
- // Updates the state.
+ // Updates the state to provide the next position and velocity to go to to
+ // follow the profile.
const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
double goal_velocity);
- // Useful for preventing windup etc.
+
+ // Updates the internal position. Useful for handling windup when a loop
+ // saturates or gets disabled.
void MoveCurrentState(const Eigen::Matrix<double, 2, 1> ¤t) {
output_ = current;
}
- // Useful for preventing windup etc.
+ // Adjusts the internal position by the provided position delta.
void MoveGoal(double dx) { output_(0, 0) += dx; }
+ // Sets the internal position to the provided position.
void SetGoal(double x) { output_(0, 0) = x; }
void set_maximum_acceleration(double maximum_acceleration) {
maximum_acceleration_ = maximum_acceleration;
}
+ void set_maximum_deceleration(double maximum_deceleration) {
+ maximum_deceleration_ = maximum_deceleration;
+ }
+
void set_maximum_velocity(double maximum_velocity) {
maximum_velocity_ = maximum_velocity;
}
private:
- // Basic kinematics to update output_, given that we are going to accelerate
- // by acceleration over delta_time.
+ // Updates output_ to match the basic kinematics, given that we are going to
+ // accelerate by acceleration over delta_time.
void UpdateVals(double acceleration, double delta_time);
// Calculates how long to go for each segment.
- void CalculateTimes(double distance_to_target, double goal_velocity);
+ void CalculateTimes(double distance_to_target, double goal_velocity,
+ Eigen::Matrix<double, 2, 1> current);
// output_ is where it should go at time_.
Eigen::Matrix<double, 2, 1> output_;
+ // Time and acceleration to slow down if we need to reverse directions.
+ double deceleration_reversal_time_;
+ double deceleration_reversal_;
+
+ // Time and acceleration to speed up with.
double acceleration_time_;
double acceleration_;
+ // Time to go at max speed at.
double constant_time_;
+ // Time and acceleration to slow down with.
double deceleration_time_;
double deceleration_;
- double maximum_acceleration_;
- double maximum_velocity_;
+ double maximum_acceleration_ = 0;
+ double maximum_deceleration_ = 0;
+ double maximum_velocity_ = 0;
// How long between calls to Update.
::std::chrono::nanoseconds timestep_;
- DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
+ DISALLOW_COPY_AND_ASSIGN(AsymmetricTrapezoidProfile);
+};
+
+// Class to implement a AsymmetricTrapezoidProfile where both acceleration and
+// deceleration match.
+class TrapezoidProfile {
+ public:
+ TrapezoidProfile(::std::chrono::nanoseconds delta_time)
+ : asymmetric_trapezoid_profile_(delta_time) {}
+
+ // Updates the state to provide the next position and velocity to go to to
+ // follow the profile.
+ const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
+ double goal_velocity) {
+ return asymmetric_trapezoid_profile_.Update(goal_position, goal_velocity);
+ }
+
+ // Updates the internal position. Useful for handling windup when a loop
+ // saturates or gets disabled.
+ void MoveCurrentState(const Eigen::Matrix<double, 2, 1> ¤t) {
+ asymmetric_trapezoid_profile_.MoveCurrentState(current);
+ }
+
+ // Adjusts the internal position by the provided position delta.
+ void MoveGoal(double dx) { asymmetric_trapezoid_profile_.MoveGoal(dx); }
+
+ // Sets the internal position to the provided position.
+ void SetGoal(double x) { asymmetric_trapezoid_profile_.SetGoal(x); }
+
+ void set_maximum_acceleration(double maximum_acceleration) {
+ asymmetric_trapezoid_profile_.set_maximum_acceleration(
+ maximum_acceleration);
+ asymmetric_trapezoid_profile_.set_maximum_deceleration(
+ maximum_acceleration);
+ }
+ void set_maximum_velocity(double maximum_velocity) {
+ asymmetric_trapezoid_profile_.set_maximum_velocity(maximum_velocity);
+ }
+
+ private:
+ AsymmetricTrapezoidProfile asymmetric_trapezoid_profile_;
};
} // namespace aos::util