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.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 2ceaac6..0170151 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -25,16 +25,13 @@
public:
SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
- ~SplineDrivetrain() {
- {
- ::aos::MutexLocker locker(&mutex_);
- run_ = false;
- new_goal_.Signal();
- }
- worker_thread_.join();
- }
-
void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
+ // Note that the user maintains ownership of the trajectory flatbuffer for the
+ // entire time; once AddTrajectory() is called, the trajectory pointer must
+ // stay valid until the spline has finished executing and HasTrajectory()
+ // returns false.
+ bool HasTrajectory(const fb::Trajectory *trajectory) const;
+ void AddTrajectory(const fb::Trajectory *trajectory);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
const ::Eigen::Matrix<double, 2, 1> &voltage_error);
@@ -50,40 +47,54 @@
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return current_trajectory_ ? current_trajectory_->GoalState(current_xva_(0),
- current_xva_(1))
- : ::Eigen::Matrix<double, 5, 1>::Zero();
+ return executing_spline_ ? current_trajectory().GoalState(current_xva_(0),
+ current_xva_(1))
+ : ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
- return current_trajectory_
- ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0))
+ return executing_spline_
+ ? current_trajectory().is_at_end(current_xva_.block<2, 1>(0, 0))
: true;
}
// Returns true if the splinedrivetrain is enabled.
bool enable() const { return enable_; }
- enum class PlanState : int8_t {
- kNoPlan = 0,
- kBuildingTrajectory = 1,
- kPlanningTrajectory = 2,
- kPlannedTrajectory = 3,
- };
-
private:
- void ComputeTrajectory();
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
+ // This is called to update the internal state for managing all the splines.
+ // Calling it redundantly does not cause any issues. It checks the value of
+ // commanded_spline_ to determine whether we are being commanded to run a
+ // spline, and if there is any trajectory in the list of trajectories matching
+ // the command, we begin/continue executing that spline. If commanded_spline_
+ // is empty or has changed, we stop executing the previous trajectory and
+ // remove it from trajectories_. Then, when the drivetrain code checks
+ // HasTrajectory() for the old trajectory, it will return false and the
+ // drivetrain can free up the fetcher to get the next trajectory.
+ void UpdateSplineHandles();
+
+ // Deletes the currently executing trajectory.
+ void DeleteCurrentSpline();
+
+ const FinishedTrajectory ¤t_trajectory() const {
+ CHECK(current_trajectory_index_);
+ CHECK_LE(0u, *current_trajectory_index_);
+ CHECK_LT(*current_trajectory_index_, trajectories_.size());
+ return *trajectories_[*current_trajectory_index_];
+ }
+
const DrivetrainConfig<double> dt_config_;
- int32_t current_spline_handle_ = 0; // Current spline told to excecute.
- int32_t current_spline_idx_ = 0; // Current executing spline.
- bool has_started_execution_ = false;
+ bool executing_spline_ = false;
- ::std::unique_ptr<DistanceSpline> current_distance_spline_;
- ::std::unique_ptr<Trajectory> current_trajectory_;
- bool current_drive_spline_backwards_ = false;
+ // TODO(james): Sort out construction to avoid so much dynamic memory
+ // allocation...
+ std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
+ std::optional<size_t> current_trajectory_index_;
+
+ std::optional<int> commanded_spline_;
// State required to compute the next iteration's output.
::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
@@ -93,44 +104,6 @@
::Eigen::Matrix<double, 2, 1> uncapped_U_;
bool enable_ = false;
bool output_was_capped_ = false;
-
- std::atomic<PlanningState> plan_state_ = {PlanningState::NO_PLAN};
-
- ::std::thread worker_thread_;
- // mutex_ is held by the worker thread while it is doing work or by the main
- // thread when it is sending work to the worker thread.
- ::aos::Mutex mutex_;
- // new_goal_ is used to signal to the worker thread that ther is work to do.
- ::aos::Condition new_goal_;
- // The following variables are guarded by mutex_.
- bool run_ = true;
-
- // These two structures mirror the flatbuffer Multispline.
- // TODO(austin): copy the goal flatbuffer directly instead of recreating it
- // like this...
- struct MultiSpline {
- int32_t spline_count;
- std::array<float, 36> spline_x;
- std::array<float, 36> spline_y;
- std::array<ConstraintT, 6> constraints;
- };
-
- struct SplineGoal {
- int32_t spline_idx = 0;
-
- bool drive_spline_backwards;
-
- MultiSpline spline;
- };
-
- SplineGoal goal_;
- ::std::unique_ptr<DistanceSpline> past_distance_spline_;
- ::std::unique_ptr<DistanceSpline> future_distance_spline_;
- ::std::unique_ptr<Trajectory> past_trajectory_;
- ::std::unique_ptr<Trajectory> future_trajectory_;
- bool future_drive_spline_backwards_ = false;
- int32_t future_spline_idx_ = 0; // Current spline being computed.
- ::std::atomic<int32_t> planning_spline_idx_{-1};
};
} // namespace drivetrain