Merge changes Ie0fc08c6,I798d937a
* changes:
Make profile type a template parameter
Support decoupling acceleration and deceleration in TrapezoidProfile
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index bba7696..05e79ca 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -1,54 +1,70 @@
#include "aos/util/trapezoid_profile.h"
-#include "aos/logging/logging.h"
-
-using ::Eigen::Matrix;
+#include "glog/logging.h"
namespace aos::util {
-TrapezoidProfile::TrapezoidProfile(::std::chrono::nanoseconds delta_time)
- : maximum_acceleration_(0), maximum_velocity_(0), timestep_(delta_time) {
+AsymmetricTrapezoidProfile::AsymmetricTrapezoidProfile(
+ ::std::chrono::nanoseconds delta_time)
+ : timestep_(delta_time) {
output_.setZero();
}
-void TrapezoidProfile::UpdateVals(double acceleration, double delta_time) {
+void AsymmetricTrapezoidProfile::UpdateVals(double acceleration,
+ double delta_time) {
output_(0) +=
output_(1) * delta_time + 0.5 * acceleration * delta_time * delta_time;
output_(1) += acceleration * delta_time;
}
-const Matrix<double, 2, 1> &TrapezoidProfile::Update(double goal_position,
- double goal_velocity) {
- CalculateTimes(goal_position - output_(0), goal_velocity);
+const Eigen::Matrix<double, 2, 1> &AsymmetricTrapezoidProfile::Update(
+ double goal_position, double goal_velocity) {
+ CalculateTimes(goal_position - output_(0), goal_velocity, output_);
double next_timestep = ::aos::time::DurationInSeconds(timestep_);
+ if (deceleration_reversal_time_ > next_timestep) {
+ UpdateVals(deceleration_reversal_, next_timestep);
+ return output_;
+ }
+
+ UpdateVals(deceleration_reversal_, deceleration_reversal_time_);
+ next_timestep -= deceleration_reversal_time_;
+
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
- } else {
- UpdateVals(acceleration_, acceleration_time_);
+ return output_;
+ }
- next_timestep -= acceleration_time_;
- if (constant_time_ > next_timestep) {
- UpdateVals(0, next_timestep);
- } else {
- UpdateVals(0, constant_time_);
- next_timestep -= constant_time_;
- if (deceleration_time_ > next_timestep) {
- UpdateVals(deceleration_, next_timestep);
- } else {
- UpdateVals(deceleration_, deceleration_time_);
- next_timestep -= deceleration_time_;
- UpdateVals(0, next_timestep);
- }
+ UpdateVals(acceleration_, acceleration_time_);
+ next_timestep -= acceleration_time_;
+
+ if (constant_time_ > next_timestep) {
+ UpdateVals(0, next_timestep);
+ return output_;
+ }
+
+ UpdateVals(0, constant_time_);
+ next_timestep -= constant_time_;
+ if (deceleration_time_ > next_timestep) {
+ UpdateVals(deceleration_, next_timestep);
+ } else {
+ UpdateVals(deceleration_, deceleration_time_);
+ next_timestep -= deceleration_time_;
+ UpdateVals(0, next_timestep);
+
+ if (next_timestep >= 0 && goal_velocity == 0) {
+ output_(0) = goal_position;
+ output_(1) = goal_velocity;
}
}
return output_;
}
-void TrapezoidProfile::CalculateTimes(double distance_to_target,
- double goal_velocity) {
+void AsymmetricTrapezoidProfile::CalculateTimes(
+ double distance_to_target, double goal_velocity,
+ Eigen::Matrix<double, 2, 1> current) {
if (distance_to_target == 0) {
// We're there. Stop everything.
// TODO(aschuh): Deal with velocity not right.
@@ -57,39 +73,69 @@
constant_time_ = 0;
deceleration_time_ = 0;
deceleration_ = 0;
+ deceleration_reversal_time_ = 0;
+ deceleration_reversal_ = 0;
return;
} else if (distance_to_target < 0) {
// Recurse with everything inverted.
- output_(1) *= -1;
- CalculateTimes(-distance_to_target, -goal_velocity);
- output_(1) *= -1;
+ current(1) *= -1;
+ CalculateTimes(-distance_to_target, -goal_velocity, current);
acceleration_ *= -1;
deceleration_ *= -1;
+ deceleration_reversal_ *= -1;
return;
}
constant_time_ = 0;
acceleration_ = maximum_acceleration_;
+
+ // Calculate the fastest speed we could get going to by the distance to
+ // target. We will have normalized everything out to be a positive distance
+ // by now so we never have to deal with going "backwards".
double maximum_acceleration_velocity =
distance_to_target * 2 * std::abs(acceleration_) +
- output_(1) * output_(1);
- if (maximum_acceleration_velocity > 0) {
- maximum_acceleration_velocity = sqrt(maximum_acceleration_velocity);
- } else {
- maximum_acceleration_velocity = -sqrt(-maximum_acceleration_velocity);
- }
+ current(1) * current(1);
+ CHECK_GE(maximum_acceleration_velocity, 0);
+ maximum_acceleration_velocity = sqrt(maximum_acceleration_velocity);
- // Since we know what we'd have to do if we kept after it to decelerate, we
- // know the sign of the acceleration.
+ // If we could get going faster than the target, we will need to decelerate
+ // after accelerating.
if (maximum_acceleration_velocity > goal_velocity) {
- deceleration_ = -maximum_acceleration_;
+ deceleration_ = -maximum_deceleration_;
} else {
+ // We couldn't get up to speed by the destination. Set our decel to
+ // accelerate to keep accelerating to get up to speed.
+ //
+ // Note: goal_velocity != 0 isn't well tested, use at your own risk.
+ LOG(FATAL) << "Untested";
deceleration_ = maximum_acceleration_;
}
+ // If we are going away from the goal, we will need to change directions.
+ if (current(1) < 0) {
+ deceleration_reversal_time_ = current(1) / deceleration_;
+ deceleration_reversal_ = -deceleration_;
+ } else if ((goal_velocity - current(1)) * (goal_velocity + current(1)) <
+ 2.0 * deceleration_ * distance_to_target) {
+ // Then, can we stop in time if we get after it? If so, we don't need to
+ // decel first before running the profile.
+ deceleration_reversal_time_ = -current(1) / deceleration_;
+ deceleration_reversal_ = deceleration_;
+ } else {
+ // Otherwise, we are all good and don't need to handle reversing.
+ deceleration_reversal_time_ = 0.0;
+ deceleration_reversal_ = 0.0;
+ }
+
+ current(0) += current(1) * deceleration_reversal_time_ +
+ 0.5 * deceleration_reversal_ * deceleration_reversal_time_ *
+ deceleration_reversal_time_;
+ current(1) += deceleration_reversal_ * deceleration_reversal_time_;
+ // OK, now we've compensated for slowing down.
+
// We now know the top velocity we can get to.
- double top_velocity = sqrt(
- (distance_to_target + (output_(1) * output_(1)) / (2.0 * acceleration_) +
+ const double top_velocity = sqrt(
+ (distance_to_target + (current(1) * current(1)) / (2.0 * acceleration_) +
(goal_velocity * goal_velocity) / (2.0 * deceleration_)) /
(-1.0 / (2.0 * deceleration_) + 1.0 / (2.0 * acceleration_)));
@@ -97,22 +143,22 @@
// how long to go at constant velocity.
if (top_velocity > maximum_velocity_) {
acceleration_time_ =
- (maximum_velocity_ - output_(1)) / maximum_acceleration_;
+ (maximum_velocity_ - current(1)) / maximum_acceleration_;
constant_time_ = ((-0.5 * maximum_acceleration_ * acceleration_time_ *
acceleration_time_ -
- output_(1) * acceleration_time_) +
+ current(1) * acceleration_time_) +
distance_to_target +
(goal_velocity * goal_velocity -
maximum_velocity_ * maximum_velocity_) /
- (2.0 * maximum_acceleration_)) /
+ (2.0 * maximum_deceleration_)) /
maximum_velocity_;
} else {
- acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
+ acceleration_time_ = (top_velocity - current(1)) / acceleration_;
}
- AOS_CHECK_GT(top_velocity, -maximum_velocity_);
+ CHECK_GT(top_velocity, -maximum_velocity_);
- if (output_(1) > maximum_velocity_) {
+ if (current(1) > maximum_velocity_) {
constant_time_ = 0;
acceleration_time_ = 0;
}
@@ -120,9 +166,9 @@
deceleration_time_ =
(goal_velocity - ::std::min(top_velocity, maximum_velocity_)) /
deceleration_;
+ if (acceleration_time_ <= 0) acceleration_time_ = 0;
if (constant_time_ <= 0) constant_time_ = 0;
if (deceleration_time_ <= 0) deceleration_time_ = 0;
- if (acceleration_time_ <= 0) acceleration_time_ = 0;
}
} // namespace aos::util
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
diff --git a/aos/util/trapezoid_profile_test.cc b/aos/util/trapezoid_profile_test.cc
index 34dc35c..54ffeb5 100644
--- a/aos/util/trapezoid_profile_test.cc
+++ b/aos/util/trapezoid_profile_test.cc
@@ -1,6 +1,7 @@
#include "aos/util/trapezoid_profile.h"
#include "Eigen/Dense"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace aos::util::testing {
@@ -13,6 +14,7 @@
TrapezoidProfileTest() : profile_(kDeltaTime) {
position_.setZero();
profile_.set_maximum_acceleration(0.75);
+ profile_.set_maximum_deceleration(0.75);
profile_.set_maximum_velocity(1.75);
}
@@ -33,7 +35,7 @@
const Eigen::Matrix<double, 2, 1> &position() { return position_; }
- TrapezoidProfile profile_;
+ AsymmetricTrapezoidProfile profile_;
::testing::AssertionResult At(double position, double velocity) {
static const double kDoubleNear = 0.00001;
@@ -59,9 +61,7 @@
constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::kDeltaTime;
TEST_F(TrapezoidProfileTest, ReachesGoal) {
- for (int i = 0; i < 450; ++i) {
- RunIteration(3, 0);
- }
+ RunFor(3, 0, std::chrono::milliseconds(4500));
EXPECT_TRUE(At(3, 0));
}
@@ -69,14 +69,14 @@
// moving faster than the new max is handled correctly.
TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
profile_.set_maximum_velocity(1.75);
- RunIteration(12.0, 0);
+ RunFor(12.0, 0, std::chrono::milliseconds(10));
double last_pos = position()(0);
double last_vel = 1.75;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
profile_.set_maximum_velocity(0.75);
}
- RunIteration(12.0, 0);
+ RunFor(12.0, 0, std::chrono::milliseconds(10));
if (i >= 400) {
EXPECT_TRUE(::std::abs(last_pos - position()(0)) <= 1.75 * 0.01);
EXPECT_NEAR(last_vel, ::std::abs(last_pos - position()(0)), 0.0001);
@@ -89,48 +89,38 @@
// There is some somewhat tricky code for dealing with going backwards.
TEST_F(TrapezoidProfileTest, Backwards) {
- for (int i = 0; i < 400; ++i) {
- RunIteration(-2, 0);
- }
+ RunFor(-2, 0, std::chrono::milliseconds(4000));
EXPECT_TRUE(At(-2, 0));
}
TEST_F(TrapezoidProfileTest, SwitchGoalInMiddle) {
- for (int i = 0; i < 200; ++i) {
- RunIteration(-2, 0);
- }
+ RunFor(-2, 0, std::chrono::milliseconds(2000));
EXPECT_FALSE(At(-2, 0));
- for (int i = 0; i < 550; ++i) {
- RunIteration(0, 0);
- }
+ RunFor(0, 0, std::chrono::milliseconds(5500));
EXPECT_TRUE(At(0, 0));
}
// Checks to make sure that it hits top speed.
TEST_F(TrapezoidProfileTest, TopSpeed) {
- for (int i = 0; i < 200; ++i) {
- RunIteration(4, 0);
- }
+ RunFor(4, 0, std::chrono::milliseconds(2000));
EXPECT_NEAR(1.5, position()(1), 10e-5);
- for (int i = 0; i < 2000; ++i) {
- RunIteration(4, 0);
- }
+ RunFor(4, 0, std::chrono::milliseconds(20000));
EXPECT_TRUE(At(4, 0));
}
// Tests that the position and velocity exactly match at the end. Some code we
// have assumes this to be true as a simplification.
TEST_F(TrapezoidProfileTest, ExactlyReachesGoal) {
- for (int i = 0; i < 450; ++i) {
- RunIteration(1, 0);
- }
+ RunFor(1, 0, std::chrono::milliseconds(4500));
EXPECT_EQ(position()(1), 0.0);
EXPECT_EQ(position()(0), 1.0);
}
-// Tests that we can move a goal without the trajectory teleporting.
+// Tests that we can move a goal without the trajectory teleporting. The goal
+// needs to move to something we haven't already passed, but will blow by.
TEST_F(TrapezoidProfileTest, MoveGoal) {
profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
profile_.set_maximum_velocity(2.0);
RunFor(5.0, 0, std::chrono::seconds(1));
@@ -143,4 +133,168 @@
EXPECT_TRUE(At(3.5, 0.0));
}
+// Tests that we can move a goal back before where we currently are without
+// teleporting.
+TEST_F(TrapezoidProfileTest, MoveGoalFar) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.0, 2.0));
+ RunFor(5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(3.0, 2.0));
+ RunFor(2.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(4.0, 0.0));
+ RunFor(2.5, 0, std::chrono::seconds(2));
+ EXPECT_TRUE(At(2.5, 0.0));
+}
+
+// Tests that we can move a goal without the trajectory teleporting. The goal
+// needs to move to something we haven't already passed, but will blow by. Do
+// this one in the negative direction.
+TEST_F(TrapezoidProfileTest, MoveGoalNegative) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.0, -2.0));
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.0, -2.0));
+ RunFor(-3.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-4.0, 0.0));
+ RunFor(-3.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.5, 0.0));
+}
+
+// Tests that we can move a goal back before where we currently are without
+// teleporting. Do this one in the negative direction.
+TEST_F(TrapezoidProfileTest, MoveGoalNegativeFar) {
+ profile_.set_maximum_acceleration(2.0);
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.0, -2.0));
+ RunFor(-5.0, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-3.0, -2.0));
+ RunFor(-2.5, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-4.0, 0.0));
+ RunFor(-2.5, 0, std::chrono::seconds(2));
+ EXPECT_TRUE(At(-2.5, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecel) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude, and hitting saturation.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelUnconstrained) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(0.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(0.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(0.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude, and hitting saturation.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelUnconstrainedNegative) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(2.0);
+
+ RunFor(-0.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-0.5, -1.0));
+
+ RunFor(-0.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(-0.75, 0.0));
+}
+
+// Tests that we can execute a profile with acceleration and deceleration not
+// matching in magnitude when going in the negative direction.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelNegative) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(-1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(-0.5, -1.0));
+
+ RunFor(-1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(-1.5, -1.0));
+ RunFor(-1.75, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(-1.75, 0.0));
+}
+
+// Tests that we can move the goal when an asymmetric profile is executing.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelMoveGoal) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.6, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+ RunFor(1.6, 0, std::chrono::milliseconds(520));
+ RunFor(1.6, 0, std::chrono::milliseconds(2500));
+ EXPECT_TRUE(At(1.6, 0.0));
+}
+
+// Tests that we can move the goal when an asymmetric profile is executing in
+// the negative direction.
+TEST_F(TrapezoidProfileTest, AsymmetricAccelDecelMoveGoalFar) {
+ // Accelerates up until t=1. Will be at x=0.5
+ profile_.set_maximum_acceleration(1.0);
+ // Decelerates in t=0.5 Will take x=0.25
+ profile_.set_maximum_deceleration(2.0);
+ profile_.set_maximum_velocity(1.0);
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+
+ EXPECT_TRUE(At(0.5, 1.0));
+
+ RunFor(1.75, 0, std::chrono::seconds(1));
+ EXPECT_TRUE(At(1.5, 1.0));
+ RunFor(1.0, 0, std::chrono::milliseconds(500));
+ EXPECT_TRUE(At(1.75, 0.0));
+ RunFor(1.0, 0, std::chrono::milliseconds(2500));
+ EXPECT_TRUE(At(1.0, 0.0));
+}
+
} // namespace aos::util::testing
diff --git a/frc971/control_loops/catapult/catapult.cc b/frc971/control_loops/catapult/catapult.cc
index 2e1867c..a827dc9 100644
--- a/frc971/control_loops/catapult/catapult.cc
+++ b/frc971/control_loops/catapult/catapult.cc
@@ -102,9 +102,11 @@
case CatapultState::RESETTING:
if (catapult_.controller().R(1, 0) > 7.0) {
- catapult_.AdjustProfile(7.0, 2000.0);
+ catapult_.mutable_profile()->set_maximum_velocity(7.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(2000.0);
} else if (catapult_.controller().R(1, 0) > 0.0) {
- catapult_.AdjustProfile(7.0, 1000.0);
+ catapult_.mutable_profile()->set_maximum_velocity(7.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(1000.0);
} else {
catapult_state_ = CatapultState::PROFILE;
}
@@ -135,4 +137,4 @@
return catapult_.MakeStatus(fbb);
}
-} // namespace frc971::control_loops::catapult
\ No newline at end of file
+} // namespace frc971::control_loops::catapult
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 7a0b7fa..599c4da 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -148,7 +148,8 @@
};
template <typename ZeroingEstimator =
- ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator,
+ class Profile = aos::util::TrapezoidProfile>
class SingleDOFProfiledSubsystem
: public ::frc971::control_loops::ProfiledSubsystem<3, 1,
ZeroingEstimator> {
@@ -209,6 +210,9 @@
double default_velocity() const { return default_velocity_; }
double default_acceleration() const { return default_acceleration_; }
+ // Returns a pointer to the profile in use.
+ Profile *mutable_profile() { return &profile_; }
+
protected:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -218,7 +222,7 @@
private:
void UpdateOffset(double offset);
- aos::util::TrapezoidProfile profile_;
+ Profile profile_;
bool enable_profile_ = true;
// Current measurement.
@@ -240,12 +244,13 @@
} // namespace internal
-template <class ZeroingEstimator>
-SingleDOFProfiledSubsystem<ZeroingEstimator>::SingleDOFProfiledSubsystem(
- ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
- const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
- const ::frc971::constants::Range &range, double default_velocity,
- double default_acceleration)
+template <class ZeroingEstimator, class Profile>
+SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::
+ SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_velocity,
+ double default_acceleration)
: ProfiledSubsystem<3, 1, ZeroingEstimator>(
::std::move(loop), {{ZeroingEstimator(zeroing_constants)}}),
profile_(this->loop_->plant().coefficients().dt),
@@ -254,11 +259,11 @@
default_acceleration_(default_acceleration) {
Y_.setZero();
offset_.setZero();
- AdjustProfile(0.0, 0.0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::UpdateOffset(
+ double offset) {
const double doffset = offset - offset_(0, 0);
AOS_LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
@@ -273,9 +278,10 @@
CapGoal("R", &this->loop_->mutable_R());
}
-template <class ZeroingEstimator>
+template <class ZeroingEstimator, class Profile>
template <class StatusTypeBuilder>
-StatusTypeBuilder SingleDOFProfiledSubsystem<ZeroingEstimator>::BuildStatus(
+StatusTypeBuilder
+SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::BuildStatus(
flatbuffers::FlatBufferBuilder *fbb) {
flatbuffers::Offset<typename ZeroingEstimator::State> estimator_state =
this->EstimatorState(fbb, 0);
@@ -306,8 +312,8 @@
return builder;
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::Correct(
const typename ZeroingEstimator::Position &new_position) {
this->estimators_[0].UpdateEstimate(new_position);
@@ -336,8 +342,8 @@
this->X_hat_ = this->loop_->X_hat();
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::CapGoal(
const char *name, Eigen::Matrix<double, 3, 1> *goal, bool print) {
// Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
@@ -356,8 +362,8 @@
}
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::ForceGoal(
double goal, double goal_velocity) {
set_unprofiled_goal(goal, goal_velocity, false);
this->loop_->mutable_R() = this->unprofiled_goal_;
@@ -367,8 +373,8 @@
this->profile_.MoveCurrentState(R.block<2, 1>(0, 0));
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::set_unprofiled_goal(
double unprofiled_goal, double unprofiled_goal_velocity, bool print) {
this->unprofiled_goal_(0, 0) = unprofiled_goal;
this->unprofiled_goal_(1, 0) = unprofiled_goal_velocity;
@@ -376,8 +382,8 @@
CapGoal("unprofiled R", &this->unprofiled_goal_, print);
}
-template <class ZeroingEstimator>
-double SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateController(
+template <class ZeroingEstimator, class Profile>
+double SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::UpdateController(
bool disable) {
// TODO(austin): What do we want to do with the profile on reset? Also, we
// should probably reset R, the offset, the profile, etc.
@@ -418,22 +424,23 @@
return this->loop_->U(0, 0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateObserver(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::UpdateObserver(
double voltage) {
this->loop_->mutable_U(0, 0) = voltage;
this->loop_->UpdateObserver(this->loop_->U(), this->loop_->plant().dt());
}
-template <class ZeroingEstimator>
-double SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+template <class ZeroingEstimator, class Profile>
+double SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::Update(
+ bool disable) {
const double voltage = UpdateController(disable);
UpdateObserver(voltage);
return voltage;
}
-template <class ZeroingEstimator>
-bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
+template <class ZeroingEstimator, class Profile>
+bool SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
if (position() > range_.upper_hard || position() < range_.lower_hard) {
@@ -447,8 +454,8 @@
return false;
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::AdjustProfile(
const ::frc971::ProfileParameters *profile_parameters) {
AdjustProfile(
profile_parameters != nullptr ? profile_parameters->max_velocity() : 0.0,
@@ -456,8 +463,8 @@
: 0.0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::AdjustProfile(
double max_angular_velocity, double max_angular_acceleration) {
profile_.set_maximum_velocity(
internal::UseUnlessZero(max_angular_velocity, default_velocity_));
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 069914e..507a914 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -96,7 +96,8 @@
// Class for controlling and motion profiling a single degree of freedom
// subsystem with a zeroing strategy of not moving.
template <typename TZeroingEstimator, typename TProfiledJointStatus,
- typename TSubsystemParams = TZeroingEstimator>
+ typename TSubsystemParams = TZeroingEstimator,
+ typename TProfile = aos::util::TrapezoidProfile>
class StaticZeroingSingleDOFProfiledSubsystem {
public:
// Constructs the subsystem from flatbuffer types (appropriate when using the
@@ -147,8 +148,6 @@
// Sets the unprofiled goal which UpdateController will go to.
void set_unprofiled_goal(double position, double velocity);
- // Changes the profile parameters for UpdateController to track.
- void AdjustProfile(double velocity, double acceleration);
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
@@ -178,6 +177,12 @@
flatbuffers::Offset<ProfiledJointStatus> MakeStatus(
flatbuffers::FlatBufferBuilder *status_fbb);
+ // Sets whether to use the trapezoidal profiler or whether to just bypass it
+ // and pass the unprofiled goal through directly.
+ void set_enable_profile(bool enable) {
+ profiled_subsystem_.set_enable_profile(enable);
+ }
+
// Iterates the controller with the provided goal.
flatbuffers::Offset<ProfiledJointStatus> Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
@@ -220,6 +225,9 @@
return profiled_subsystem_.controller();
}
+ // Returns a pointer to the profile in use.
+ TProfile *mutable_profile() { return profiled_subsystem_.mutable_profile(); }
+
private:
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
@@ -227,14 +235,15 @@
const StaticZeroingSingleDOFProfiledSubsystemParams<TSubsystemParams> params_;
- ::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator>
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator,
+ TProfile>
profiled_subsystem_;
};
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
+ SubsystemParams, Profile>::
StaticZeroingSingleDOFProfiledSubsystem(
const StaticZeroingSingleDOFProfiledSubsystemParams<SubsystemParams>
¶ms)
@@ -251,9 +260,9 @@
};
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::Reset() {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams, Profile>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
@@ -261,12 +270,12 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
bool StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::
- Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position,
- bool disabled) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled) {
CHECK_NOTNULL(position);
profiled_subsystem_.Correct(*position);
@@ -296,8 +305,9 @@
// jump.
profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
// Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(
- params_.zeroing_profile_params.max_velocity,
+ mutable_profile()->set_maximum_velocity(
+ params_.zeroing_profile_params.max_velocity);
+ mutable_profile()->set_maximum_acceleration(
params_.zeroing_profile_params.max_acceleration);
// We are not ready to start doing anything yet.
@@ -321,11 +331,20 @@
if (goal) {
if (goal->profile_params()) {
- AdjustProfile(goal->profile_params()->max_velocity(),
- goal->profile_params()->max_acceleration());
+ mutable_profile()->set_maximum_velocity(
+ internal::UseUnlessZero(goal->profile_params()->max_velocity(),
+ profiled_subsystem_.default_velocity()));
+ mutable_profile()->set_maximum_acceleration(
+ std::min(static_cast<double>(max_acceleration_),
+ internal::UseUnlessZero(
+ goal->profile_params()->max_acceleration(),
+ profiled_subsystem_.default_acceleration())));
} else {
- AdjustProfile(profiled_subsystem_.default_velocity(),
- profiled_subsystem_.default_acceleration());
+ mutable_profile()->set_maximum_velocity(
+ profiled_subsystem_.default_velocity());
+ mutable_profile()->set_maximum_acceleration(
+ std::min(static_cast<double>(max_acceleration_),
+ profiled_subsystem_.default_acceleration()));
}
if (goal->has_ignore_profile()) {
@@ -352,10 +371,10 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::set_unprofiled_goal(double goal, double goal_velocity) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::set_unprofiled_goal(double goal, double goal_velocity) {
if (goal < min_position_) {
VLOG(1) << "Limiting to " << min_position_ << " from " << goal;
goal = min_position_;
@@ -368,22 +387,14 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
-void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::AdjustProfile(double velocity, double acceleration) {
- profiled_subsystem_.AdjustProfile(
- velocity, std::min(acceleration, static_cast<double>(max_acceleration_)));
-}
-
-template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
flatbuffers::Offset<ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
- Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- flatbuffers::FlatBufferBuilder *status_fbb) {
+StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ double *output,
+ flatbuffers::FlatBufferBuilder *status_fbb) {
const bool disabled = Correct(goal, position, output == nullptr);
// Calculate the loops for a cycle.
@@ -400,35 +411,35 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
double StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::UpdateController(bool disabled) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::UpdateController(bool disabled) {
return profiled_subsystem_.UpdateController(disabled);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::UpdateObserver(double voltage) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::UpdateObserver(double voltage) {
profiled_subsystem_.UpdateObserver(voltage);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::ForceGoal(double goal, double goal_velocity) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::ForceGoal(double goal, double goal_velocity) {
profiled_subsystem_.ForceGoal(goal, goal_velocity);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
flatbuffers::Offset<ProfiledJointStatus>
StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
CHECK_NOTNULL(status_fbb);
typename ProfiledJointStatus::Builder status_builder =
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index ed2f210..eeea9b5 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -208,26 +208,26 @@
if (superstructure_status_fetcher_->state() == Superstructure::RUNNING ||
superstructure_status_fetcher_->state() ==
Superstructure::LANDING_RUNNING) {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(
- ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
- Superstructure::kOperatingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
+ Superstructure::kOperatingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kOperatingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
+ Superstructure::kOperatingVoltage);
} else {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(
- ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
- Superstructure::kZeroingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
+ Superstructure::kZeroingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kZeroingVoltage);
+ CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
+ Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
- Superstructure::kShoulderTransitionToLanded + 1e-4) {
- AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
- Superstructure::kLandingShoulderDownVoltage - 0.00001);
+ Superstructure::kShoulderTransitionToLanded - 1e-4) {
+ CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
+ Superstructure::kLandingShoulderDownVoltage - 0.00001)
+ << ": Arm at " << arm_plant_->X(0, 0) << " and transition at "
+ << Superstructure::kShoulderTransitionToLanded;
}
// Use the plant to generate the next physical state given the voltages to