Make profile handle goal moving such that the profile overshoots
When the goal is moved such that a profile can't get there without
decelerating past the target and then re-accelerating back up to the
point, handle it correctly.
This is originally from Lee, I just added tests.
Change-Id: I4a9bd6354a3c7d4e97df9fc9141b24530739f9bc
Signed-off-by: Lee Mracek <lee@valkyrierobotics.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/util/trapezoid_profile.cc b/aos/util/trapezoid_profile.cc
index 938ca75..bba7696 100644
--- a/aos/util/trapezoid_profile.cc
+++ b/aos/util/trapezoid_profile.cc
@@ -98,11 +98,14 @@
if (top_velocity > maximum_velocity_) {
acceleration_time_ =
(maximum_velocity_ - output_(1)) / maximum_acceleration_;
- constant_time_ =
- (distance_to_target + (goal_velocity * goal_velocity -
- maximum_velocity_ * maximum_velocity_) /
- (2.0 * maximum_acceleration_)) /
- maximum_velocity_;
+ constant_time_ = ((-0.5 * maximum_acceleration_ * acceleration_time_ *
+ acceleration_time_ -
+ output_(1) * acceleration_time_) +
+ distance_to_target +
+ (goal_velocity * goal_velocity -
+ maximum_velocity_ * maximum_velocity_) /
+ (2.0 * maximum_acceleration_)) /
+ maximum_velocity_;
} else {
acceleration_time_ = (top_velocity - output_(1)) / acceleration_;
}
@@ -114,7 +117,12 @@
acceleration_time_ = 0;
}
- deceleration_time_ = (goal_velocity - top_velocity) / deceleration_;
+ deceleration_time_ =
+ (goal_velocity - ::std::min(top_velocity, maximum_velocity_)) /
+ deceleration_;
+ 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_test.cc b/aos/util/trapezoid_profile_test.cc
index 92b9996..34dc35c 100644
--- a/aos/util/trapezoid_profile_test.cc
+++ b/aos/util/trapezoid_profile_test.cc
@@ -10,7 +10,7 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
protected:
- TrapezoidProfileTest() : profile_(delta_time) {
+ TrapezoidProfileTest() : profile_(kDeltaTime) {
position_.setZero();
profile_.set_maximum_acceleration(0.75);
profile_.set_maximum_velocity(1.75);
@@ -21,6 +21,16 @@
position_ = profile_.Update(goal_position, goal_velocity);
}
+ void RunFor(double goal_position, double goal_velocity,
+ std::chrono::nanoseconds duration) {
+ while (duration > std::chrono::nanoseconds(0)) {
+ position_ = profile_.Update(goal_position, goal_velocity);
+ duration -= kDeltaTime;
+ }
+
+ ASSERT_EQ(duration.count(), 0);
+ }
+
const Eigen::Matrix<double, 2, 1> &position() { return position_; }
TrapezoidProfile profile_;
@@ -40,13 +50,13 @@
}
private:
- static constexpr ::std::chrono::nanoseconds delta_time =
+ static constexpr ::std::chrono::nanoseconds kDeltaTime =
::std::chrono::milliseconds(10);
Eigen::Matrix<double, 2, 1> position_;
};
-constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::delta_time;
+constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::kDeltaTime;
TEST_F(TrapezoidProfileTest, ReachesGoal) {
for (int i = 0; i < 450; ++i) {
@@ -55,7 +65,7 @@
EXPECT_TRUE(At(3, 0));
}
-// Tests that decresing the maximum velocity in the middle when it is already
+// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly.
TEST_F(TrapezoidProfileTest, ContinousUnderVelChange) {
profile_.set_maximum_velocity(1.75);
@@ -118,4 +128,19 @@
EXPECT_EQ(position()(0), 1.0);
}
+// Tests that we can move a goal without the trajectory teleporting.
+TEST_F(TrapezoidProfileTest, MoveGoal) {
+ profile_.set_maximum_acceleration(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));
+}
+
} // namespace aos::util::testing