Refactor trajectory generation to separate process
This pulls all the trajectory planning into a TrajectoryGenerator class,
which produces a Trajectory spline that the drivetrain code can consume
and use to track the spline.
Broadly speaking, this change:
- Separates the Trajectory class into a generation class and a
FinishedTrajectory class, where the generator produces a flatbuffer
and the FinishedTrajectory reads all the required information from
the flatbuffer.
- Add an option for serialization/deserialization of a DistanceSpline.
- Removes some dead code from Trajectory class (mostly having to do with
the old feedback algorithm).
- Uses floats in more places, to keep the size of the Trajectory
flatbuffer under control
- Update the tests & autonomous code to use the new spline code.
Further work that may make sense:
- Experiment with alternatives to current structure of the Trajectory
flatbuffer to see whether (a) the size is a problem; and (b) if so,
what we should do about it.
- Add shims to allow replaying logfiles with old-style spline goals.
Change-Id: Ic80ce4e384ec4d1bd22940580e3652ecd305b352
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 423465a..cdedce0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,6 +2,8 @@
#include "Eigen/Dense"
+#include "aos/json_to_flatbuffer.h"
+
#include "aos/realtime.h"
#include "aos/util/math.h"
#include "frc971/control_loops/control_loops_generated.h"
@@ -15,9 +17,10 @@
namespace drivetrain {
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config), new_goal_(&mutex_) {
- worker_thread_ = std::thread(&SplineDrivetrain::ComputeTrajectory, this);
-}
+ : dt_config_(dt_config),
+ current_xva_(Eigen::Vector3d::Zero()),
+ next_xva_(Eigen::Vector3d::Zero()),
+ next_U_(Eigen::Vector2d::Zero()) {}
void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
output_was_capped_ =
@@ -28,151 +31,72 @@
}
}
-void SplineDrivetrain::ComputeTrajectory() {
- ::aos::SetCurrentThreadRealtimePriority(1);
-
- ::aos::MutexLocker locker(&mutex_);
- while (run_) {
- while (goal_.spline_idx == future_spline_idx_) {
- AOS_CHECK(!new_goal_.Wait());
- if (!run_) {
- return;
- }
- }
- past_distance_spline_.reset();
- past_trajectory_.reset();
-
- plan_state_ = PlanningState::BUILDING_TRAJECTORY;
- future_spline_idx_ = goal_.spline_idx;
- planning_spline_idx_ = future_spline_idx_;
- const MultiSpline &multispline = goal_.spline;
- auto x = multispline.spline_x;
- auto y = multispline.spline_y;
- ::std::vector<Spline> splines = ::std::vector<Spline>();
- for (int i = 0; i < multispline.spline_count; ++i) {
- ::Eigen::Matrix<double, 2, 6> points =
- ::Eigen::Matrix<double, 2, 6>::Zero();
- for (int j = 0; j < 6; ++j) {
- points(0, j) = x[i * 5 + j];
- points(1, j) = y[i * 5 + j];
- }
- splines.emplace_back(Spline(points));
- }
-
- future_drive_spline_backwards_ = goal_.drive_spline_backwards;
-
- future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
- new DistanceSpline(::std::move(splines)));
-
- future_trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(future_distance_spline_.get(), dt_config_));
-
- for (size_t i = 0; i < multispline.constraints.size(); ++i) {
- const ::frc971::ConstraintT &constraint = multispline.constraints[i];
- switch (constraint.constraint_type) {
- case frc971::ConstraintType::CONSTRAINT_TYPE_UNDEFINED:
- break;
- case frc971::ConstraintType::LONGITUDINAL_ACCELERATION:
- future_trajectory_->set_longitudinal_acceleration(constraint.value);
- break;
- case frc971::ConstraintType::LATERAL_ACCELERATION:
- future_trajectory_->set_lateral_acceleration(constraint.value);
- break;
- case frc971::ConstraintType::VOLTAGE:
- future_trajectory_->set_voltage_limit(constraint.value);
- break;
- case frc971::ConstraintType::VELOCITY:
- future_trajectory_->LimitVelocity(constraint.start_distance,
- constraint.end_distance,
- constraint.value);
- break;
- }
- }
- plan_state_ = PlanningState::PLANNING_TRAJECTORY;
-
- future_trajectory_->Plan();
- plan_state_ = PlanningState::PLANNED;
- }
-}
-
void SplineDrivetrain::SetGoal(
const ::frc971::control_loops::drivetrain::Goal *goal) {
- current_spline_handle_ = goal->spline_handle();
-
- // If told to stop, set the executing spline to an invalid index and clear out
- // its plan:
- if (current_spline_handle_ == 0 &&
- (goal->spline() == nullptr ||
- current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
- current_spline_idx_ = -1;
- }
-
- ::aos::Mutex::State mutex_state = mutex_.TryLock();
- if (mutex_state == ::aos::Mutex::State::kLocked) {
- if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
- future_spline_idx_ != goal->spline()->spline_idx()) {
- CHECK_NOTNULL(goal->spline());
- goal_.spline_idx = goal->spline()->spline_idx();
- goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
-
- const frc971::MultiSpline *multispline = goal->spline()->spline();
- CHECK_NOTNULL(multispline);
-
- goal_.spline.spline_count = multispline->spline_count();
-
- CHECK_EQ(multispline->spline_x()->size(),
- static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
- CHECK_EQ(multispline->spline_y()->size(),
- static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
-
- std::copy(multispline->spline_x()->begin(),
- multispline->spline_x()->end(), goal_.spline.spline_x.begin());
- std::copy(multispline->spline_y()->begin(),
- multispline->spline_y()->end(), goal_.spline.spline_y.begin());
-
- for (size_t i = 0; i < 6; ++i) {
- if (multispline->constraints() != nullptr &&
- i < multispline->constraints()->size()) {
- multispline->constraints()->Get(i)->UnPackTo(
- &goal_.spline.constraints[i]);
- } else {
- goal_.spline.constraints[i].constraint_type =
- ConstraintType::CONSTRAINT_TYPE_UNDEFINED;
- }
- }
-
- new_goal_.Broadcast();
- if (current_spline_handle_ != current_spline_idx_) {
- // If we aren't going to actively execute the current spline, evict it's
- // plan.
- past_trajectory_ = std::move(current_trajectory_);
- past_distance_spline_ = std::move(current_distance_spline_);
- }
- }
- // If you never started executing the previous spline, you're screwed.
- if (future_trajectory_ &&
- (!current_trajectory_ ||
- current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
- current_spline_idx_ == -1)) {
- // Move current data to other variables to be cleared by worker.
- past_trajectory_ = std::move(current_trajectory_);
- past_distance_spline_ = std::move(current_distance_spline_);
-
- // Move the computed data to be executed.
- current_trajectory_ = std::move(future_trajectory_);
- current_distance_spline_ = std::move(future_distance_spline_);
- current_drive_spline_backwards_ = future_drive_spline_backwards_;
- current_spline_idx_ = future_spline_idx_;
-
- // Reset internal state to a trajectory start position.
- current_xva_ = current_trajectory_->FFAcceleration(0);
- current_xva_(1) = 0.0;
- has_started_execution_ = false;
- }
- mutex_.Unlock();
+ if (goal->has_spline_handle()) {
+ commanded_spline_ = goal->spline_handle();
} else {
- VLOG(1) << "Failed to acquire trajectory lock.";
+ commanded_spline_.reset();
}
+ UpdateSplineHandles();
+}
+
+bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
+ if (trajectory == nullptr) {
+ return false;
+ }
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ return true;
+ }
+ }
+ return false;
+}
+
+void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
+ trajectories_.emplace_back(
+ std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
+ UpdateSplineHandles();
+}
+
+void SplineDrivetrain::DeleteCurrentSpline() {
+ CHECK(current_trajectory_index_);
+ CHECK_LT(*current_trajectory_index_, trajectories_.size());
+ trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+ executing_spline_ = false;
+ current_trajectory_index_.reset();
+ current_xva_.setZero();
+}
+
+void SplineDrivetrain::UpdateSplineHandles() {
+ // If we are currently executing a spline and have received a change
+ if (executing_spline_) {
+ if (!commanded_spline_) {
+ // We've been told to stop executing a spline; remove it from our queue,
+ // and clean up.
+ DeleteCurrentSpline();
+ return;
+ } else {
+ if (executing_spline_ &&
+ current_trajectory().spline_handle() != *commanded_spline_) {
+ // If we are executing a spline, and the handle has changed, garbage
+ // collect the old spline.
+ DeleteCurrentSpline();
+ }
+ }
+ }
+ // We've now cleaned up the previous state; handle any new commands.
+ if (!commanded_spline_) {
+ return;
+ }
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
+ executing_spline_ = true;
+ current_trajectory_index_ = ii;
+ }
+ }
+ // If we didn't find the commanded spline in the list of available splines,
+ // that's fine; it just means, it hasn't been fully planned yet.
}
// TODO(alex): Hold position when done following the spline.
@@ -181,20 +105,19 @@
const ::Eigen::Matrix<double, 2, 1> &voltage_error) {
next_U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
enable_ = enable;
- if (enable && current_trajectory_) {
+ if (enable && executing_spline_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!IsAtEnd() && current_spline_handle_ == current_spline_idx_) {
- has_started_execution_ = true;
+ if (!IsAtEnd() && executing_spline_) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
- U_ff = current_trajectory_->FFVoltage(current_xva_(0));
+ U_ff = current_trajectory().FFVoltage(current_xva_(0));
}
const double current_distance = current_xva_(0);
::Eigen::Matrix<double, 2, 5> K =
- current_trajectory_->GainForDistance(current_distance);
+ current_trajectory().GainForDistance(current_distance);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- if (current_drive_spline_backwards_) {
+ if (current_trajectory().drive_spline_backwards()) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
goal_state(2, 0) += M_PI;
@@ -204,16 +127,16 @@
goal_state(4, 0) = -left_goal;
}
const Eigen::Matrix<double, 5, 1> relative_goal =
- current_trajectory_->StateToPathRelativeState(current_distance,
+ current_trajectory().StateToPathRelativeState(current_distance,
goal_state);
const Eigen::Matrix<double, 5, 1> relative_state =
- current_trajectory_->StateToPathRelativeState(current_distance, state);
+ current_trajectory().StateToPathRelativeState(current_distance, state);
Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
- next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &xv_state);
+ next_xva_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
@@ -225,7 +148,7 @@
if (!output) {
return;
}
- if (current_spline_handle_ == current_spline_idx_) {
+ if (executing_spline_) {
if (!IsAtEnd()) {
output->left_voltage = next_U_(0);
output->right_voltage = next_U_(1);
@@ -248,33 +171,39 @@
flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const {
+ int *spline_handles;
+ const flatbuffers::Offset<flatbuffers::Vector<int>> handles_vector =
+ builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
+
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ spline_handles[ii] = trajectories_[ii]->spline_handle();
+ }
+
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
- if (current_distance_spline_) {
+ if (executing_spline_) {
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
trajectory_logging_builder.add_x(goal_state(0));
trajectory_logging_builder.add_y(goal_state(1));
trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
- goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+ goal_state(2) +
+ (current_trajectory().drive_spline_backwards() ? M_PI : 0.0)));
trajectory_logging_builder.add_left_velocity(goal_state(3));
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
- trajectory_logging_builder.add_planning_state(plan_state_.load());
trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
- has_started_execution_);
- trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
- IsAtEnd());
- trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
- trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
- trajectory_logging_builder.add_distance_remaining(
- current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
- : 0.0);
-
- int32_t planning_spline_idx = planning_spline_idx_;
- if (current_spline_idx_ == planning_spline_idx) {
- trajectory_logging_builder.add_planning_spline_idx(0);
- } else {
- trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+ executing_spline_);
+ trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
+ if (commanded_spline_) {
+ trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
+ if (executing_spline_) {
+ trajectory_logging_builder.add_current_spline_idx(*commanded_spline_);
+ }
}
+ trajectory_logging_builder.add_distance_remaining(
+ executing_spline_ ? current_trajectory().length() - current_xva_.x()
+ : 0.0);
+ trajectory_logging_builder.add_available_splines(handles_vector);
+
return trajectory_logging_builder.Finish();
}