Add tests and an implemention of velocity limiting
Single point and region velocity limits are now supported.
Change-Id: I3007ac15f839c24844f84fba96b89fbf39225a5d
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 271b29b..64b4612 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -391,6 +391,36 @@
return result;
}
+void Trajectory::LimitVelocity(double starting_distance, double ending_distance,
+ const double max_velocity) {
+ const double segment_length = ending_distance - starting_distance;
+
+ const double min_length = length() / static_cast<double>(plan_.size() - 1);
+ if (starting_distance > ending_distance) {
+ LOG(FATAL, "End before start: %f > %f\n", starting_distance,
+ ending_distance);
+ }
+ starting_distance = ::std::min(length(), ::std::max(0.0, starting_distance));
+ ending_distance = ::std::min(length(), ::std::max(0.0, ending_distance));
+ if (segment_length < min_length) {
+ const size_t plan_index = static_cast<size_t>(
+ ::std::round((starting_distance + ending_distance) / 2.0 / min_length));
+ if (max_velocity < plan_[plan_index]) {
+ plan_[plan_index] = max_velocity;
+ }
+ } else {
+ for (size_t i = DistanceToSegment(starting_distance) + 1;
+ i < DistanceToSegment(ending_distance) + 1; ++i) {
+ if (max_velocity < plan_[i]) {
+ plan_[i] = max_velocity;
+ if (i < DistanceToSegment(ending_distance)) {
+ plan_segment_type_[i] = SegmentType::VELOCITY_LIMITED;
+ }
+ }
+ }
+ }
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 1fb9062..d3468b3 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -61,6 +61,10 @@
return ::std::sqrt(lateral_acceleration_ / spline_->DDXY(distance).norm());
}
+ // Limits the velocity in the specified segment to the max velocity.
+ void LimitVelocity(double starting_distance, double ending_distance,
+ double max_velocity);
+
// Runs the lateral acceleration (curvature) pass on the plan.
void LateralAccelPass();
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index e61c7ff..58f0a22 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -78,8 +78,11 @@
double longitudal_acceleration;
double velocity_limit;
double voltage_limit;
+ ::std::function<void(Trajectory *)> trajectory_modification_fn;
};
+void NullTrajectoryModificationFunction(Trajectory *) {}
+
class ParameterizedSplineTest
: public ::testing::TestWithParam<SplineTestParams> {
public:
@@ -105,6 +108,8 @@
trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
trajectory_->set_voltage_limit(GetParam().voltage_limit);
+ GetParam().trajectory_modification_fn(trajectory_.get());
+
initial_plan_ = trajectory_->plan();
trajectory_->LateralAccelPass();
curvature_plan_ = trajectory_->plan();
@@ -117,6 +122,8 @@
}
void TearDown() {
+ printf(" Spline takes %f seconds to follow\n",
+ length_plan_xva_.size() * ::y2016::control_loops::drivetrain::kDt);
#if defined(SUPPORT_PLOT)
if (FLAGS_plot) {
::std::vector<double> distances = trajectory_->Distances();
@@ -128,7 +135,20 @@
length_plan_a_.push_back(length_plan_xva_[i](2));
}
+ ::std::vector<double> plan_segment_center_distance;
+ ::std::vector<double> plan_type;
+ for (Trajectory::SegmentType segment_type :
+ trajectory_->plan_segment_type()) {
+ plan_type.push_back(static_cast<int>(segment_type));
+ }
+ for (size_t i = 0; i < distances.size() - 1; ++i) {
+ plan_segment_center_distance.push_back(
+ (distances[i] + distances[i + 1]) / 2.0);
+ }
+
matplotlibcpp::figure();
+ matplotlibcpp::plot(plan_segment_center_distance, plan_type,
+ {{"label", "plan_type"}});
matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
@@ -248,6 +268,14 @@
return params;
}
+void LimitMiddleOfPathTrajectoryModificationFunction(Trajectory *trajectory) {
+ trajectory->LimitVelocity(1.0, 2.0, 0.5);
+}
+
+void ShortLimitMiddleOfPathTrajectoryModificationFunction(Trajectory *trajectory) {
+ trajectory->LimitVelocity(1.5, 1.5, 0.5);
+}
+
INSTANTIATE_TEST_CASE_P(
SplineTest, ParameterizedSplineTest,
::testing::Values(
@@ -257,28 +285,48 @@
.finished(),
2.0 /*lateral acceleration*/,
1.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */}),
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Be velocity limited.
MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
.finished(),
2.0 /*lateral acceleration*/,
1.0 /*longitudinal acceleration*/,
- 0.5 /* velocity limit */, 12.0 /* volts */}),
+ 0.5 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Hit the voltage limit.
MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
-1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
.finished(),
2.0 /*lateral acceleration*/,
3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 5.0 /* volts */}),
- // Hit the curvature limit
+ 10.0 /* velocity limit */, 5.0 /* volts */,
+ NullTrajectoryModificationFunction}),
+ // Hit the curvature limit.
MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
-0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
.finished(),
1.0 /*lateral acceleration*/,
3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */})));
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
+ // Add an artifical velocity limit in the middle.
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
+ -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 2.0 /*lateral acceleration*/,
+ 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ LimitMiddleOfPathTrajectoryModificationFunction}),
+ // Add a really short artifical velocity limit in the middle.
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
+ -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 2.0 /*lateral acceleration*/,
+ 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ ShortLimitMiddleOfPathTrajectoryModificationFunction})));
// TODO(austin): Handle saturation. 254 does this by just not going that
// fast... We want to maybe replan when we get behind, or something. Maybe