Store the strategy used for each spline segment
We were running into an issue where the ff voltage would chatter. The
core problem was that we planned a segment using one acceleration
function, and executed it with another. That's not consistent with the
velocity plan, whether or not it's right. We need the acceleration that
we use to execute the plan to be consistent with the velocity, which
only works if we use the same acceleration function used to integrate up
the velocity during the segment. It's more important to be internally
consistent than to track constraints perfectly.
Change-Id: Id28c7036c2d3e585a99577cb4e03967f2bba3e26
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 511543d..271b29b 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -36,7 +36,11 @@
1.0 / (robot_radius_l_ + robot_radius_r_))
.finished()),
Tla_to_lr_(Tlr_to_la_.inverse()),
- plan_(num_distance, vmax) {}
+ plan_(num_distance == 0
+ ? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
+ : num_distance,
+ vmax),
+ plan_segment_type_(plan_.size() - 1, SegmentType::VELOCITY_LIMITED) {}
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
@@ -100,11 +104,14 @@
const double distance = Distance(i);
// Integrate our acceleration forward one step.
- plan_[i + 1] = ::std::min(
- plan_[i + 1],
- IntegrateAccelForDistance(
- [this](double x, double v) { return ForwardAcceleration(x, v); },
- plan_[i], distance, delta_distance));
+ const double new_plan_velocity = IntegrateAccelForDistance(
+ [this](double x, double v) { return ForwardAcceleration(x, v); },
+ plan_[i], distance, delta_distance);
+
+ if (new_plan_velocity < plan_[i + 1]) {
+ plan_[i + 1] = new_plan_velocity;
+ plan_segment_type_[i] = SegmentType::ACCELERATION_LIMITED;
+ }
}
}
@@ -159,69 +166,70 @@
const double distance = Distance(i);
// Integrate our deceleration back one step.
- plan_[i - 1] = ::std::min(
- plan_[i - 1],
- IntegrateAccelForDistance(
- [this](double x, double v) { return BackwardAcceleration(x, v); },
- plan_[i], distance, delta_distance));
+ const double new_plan_velocity = IntegrateAccelForDistance(
+ [this](double x, double v) { return BackwardAcceleration(x, v); },
+ plan_[i], distance, delta_distance);
+
+ if (new_plan_velocity < plan_[i - 1]) {
+ plan_[i - 1] = new_plan_velocity;
+ plan_segment_type_[i - 1] = SegmentType::DECELERATION_LIMITED;
+ }
}
}
::Eigen::Matrix<double, 3, 1> Trajectory::FFAcceleration(double distance) {
- size_t before_index;
- size_t after_index;
- if (distance < Distance(1)) {
- // Within the first step.
- after_index = 1;
+ if (distance < 0.0) {
// Make sure we don't end up off the beginning of the curve.
- if (distance < 0.0) {
- distance = 0.0;
- }
- } else if (distance > Distance(plan_.size() - 2)) {
- // Within the last step.
- after_index = plan_.size() - 1;
+ distance = 0.0;
+ } else if (distance > length()) {
// Make sure we don't end up off the end of the curve.
- if (distance > length()) {
- distance = length();
- }
- } else {
- // Otherwise do the calculation normally.
- after_index = static_cast<size_t>(
- ::std::ceil(distance / length() * (plan_.size() - 1)));
+ distance = length();
}
- before_index = after_index - 1;
+ const size_t before_index = DistanceToSegment(distance);
+ const size_t after_index = before_index + 1;
+
const double before_distance = Distance(before_index);
const double after_distance = Distance(after_index);
- // Now, compute the velocity that we could have if we accelerated from the
- // previous step and decelerated from the next step. The min will tell us
- // which is in effect.
- const double velocity_forwards = IntegrateAccelForDistance(
- [this](double x, double v) { return ForwardAcceleration(x, v); },
- plan_[before_index], before_distance, distance - before_distance);
- const double velocity_backward = IntegrateAccelForDistance(
- [this](double x, double v) { return BackwardAcceleration(x, v); },
- plan_[after_index], after_distance, distance - after_distance);
-
// And then also make sure we aren't curvature limited.
const double vcurvature = LateralVelocityCurvature(distance);
double acceleration;
double velocity;
- if (vcurvature < velocity_forwards && vcurvature < velocity_backward) {
- // If we are curvature limited, we can't accelerate.
+ switch (plan_segment_type_[DistanceToSegment(distance)]) {
+ case SegmentType::VELOCITY_LIMITED:
+ acceleration = 0.0;
+ velocity = (plan_[before_index] + plan_[after_index]) / 2.0;
+ // TODO(austin): Accelerate or decelerate until we hit the limit in the
+ // time slice. Otherwise our acceleration will be lying for this slice.
+ // Do note, we've got small slices so the effect will be small.
+ break;
+ case SegmentType::CURVATURE_LIMITED:
+ velocity = vcurvature;
+ acceleration = 0.0;
+ break;
+ case SegmentType::ACCELERATION_LIMITED:
+ velocity = IntegrateAccelForDistance(
+ [this](double x, double v) { return ForwardAcceleration(x, v); },
+ plan_[before_index], before_distance, distance - before_distance);
+ acceleration = ForwardAcceleration(distance, velocity);
+ break;
+ case SegmentType::DECELERATION_LIMITED:
+ velocity = IntegrateAccelForDistance(
+ [this](double x, double v) { return BackwardAcceleration(x, v); },
+ plan_[after_index], after_distance, distance - after_distance);
+ acceleration = BackwardAcceleration(distance, velocity);
+ break;
+ default:
+ LOG(FATAL, "Unknown segment type %d\n",
+ static_cast<int>(plan_segment_type_[DistanceToSegment(distance)]));
+ break;
+ }
+
+ if (vcurvature < velocity) {
velocity = vcurvature;
acceleration = 0.0;
- } else if (velocity_forwards < velocity_backward) {
- // Otherwise, pick the acceleration and velocity from the forward pass if it
- // was the predominate factor in this step.
- velocity = velocity_forwards;
- acceleration = ForwardAcceleration(distance, velocity);
- } else {
- // Otherwise, pick the acceleration and velocity from the backward pass if
- // it was the predominate factor in this step.
- velocity = velocity_backward;
- acceleration = BackwardAcceleration(distance, velocity);
+ LOG(ERROR, "Curvature won\n");
}
return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
.finished();
@@ -231,21 +239,11 @@
const Eigen::Matrix<double, 3, 1> xva = FFAcceleration(distance);
const double velocity = xva(1);
const double acceleration = xva(2);
- const double current_ddtheta = spline_->DDTheta(distance);
- const double current_dtheta = spline_->DTheta(distance);
- // We've now got the equation:
- // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- const ::Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
- const ::Eigen::Matrix<double, 2, 2> B_inverse =
- velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
-
- // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- const ::Eigen::Matrix<double, 2, 1> K5 = B_inverse * my_K2;
- const ::Eigen::Matrix<double, 2, 1> K3 = B_inverse * K1(current_ddtheta);
- const ::Eigen::Matrix<double, 2, 1> K4 =
- -B_inverse * velocity_drivetrain_->plant().coefficients().A_continuous *
- my_K2;
+ ::Eigen::Matrix<double, 2, 1> K3;
+ ::Eigen::Matrix<double, 2, 1> K4;
+ ::Eigen::Matrix<double, 2, 1> K5;
+ K345(distance, &K3, &K4, &K5);
return K5 * acceleration + K3 * velocity * velocity + K4 * velocity;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a42476a..049dd33 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -150,9 +150,26 @@
::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
::std::chrono::nanoseconds dt);
+ enum SegmentType : uint8_t {
+ VELOCITY_LIMITED,
+ CURVATURE_LIMITED,
+ ACCELERATION_LIMITED,
+ DECELERATION_LIMITED
+ };
+
+ const ::std::vector<SegmentType> &plan_segment_type() const {
+ return plan_segment_type_;
+ }
+
private:
// Computes alpha for a distance.
- double DistanceToAlpha(double distance) const;
+ size_t DistanceToSegment(double distance) const {
+ return ::std::max(
+ static_cast<size_t>(0),
+ ::std::min(plan_segment_type_.size() - 1,
+ static_cast<size_t>(::std::floor(distance / length() *
+ (plan_.size() - 1)))));
+ }
// Returns K1 and K2.
// K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
@@ -211,6 +228,7 @@
const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
// Velocities in the plan (distance for each index is defined by distance())
::std::vector<double> plan_;
+ ::std::vector<SegmentType> plan_segment_type_;
// Plan voltage limit.
double voltage_limit_ = 12.0;
};
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index d9c12f4..4e082ee 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -83,6 +83,17 @@
trajectory.BackwardPass();
::aos::monotonic_clock::time_point end_time = ::aos::monotonic_clock::now();
+
+ ::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);
+ }
+
::std::vector<double> backward_plan = trajectory.plan();
LOG(INFO, "Took %fms to plan\n",
@@ -195,6 +206,8 @@
if (FLAGS_plot) {
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"}});
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 0d3a6db..520ee1a9 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -167,8 +167,6 @@
1.1e-2);
}
-// TODO(austin): Try a velocity limited plan at some point.
-//
// 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
// stop moving the setpoint like our 2018 arm?