Remove runtime malloc's from SplineDrivetrain
This lets drivetrain_lib_test pass with die_on_malloc enabled.
Change-Id: If85b2ee5635fd82ba4aaf2ef18b4736cb6099d27
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 91b9898..f445fa4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -123,7 +123,7 @@
// The spline we are converting to a distance.
aos::SizedArray<Spline, kMaxSplines> splines_;
// An interpolation table of distances evenly distributed in alpha.
- const absl::Span<const float> distances_;
+ absl::Span<const float> distances_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a06b965..0d100ed 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -146,9 +146,10 @@
class DrivetrainLoop
: public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- // Note that we only actually store N - 1 splines, since we need to keep one
- // fetcher free to check whether there are any new splines.
- static constexpr size_t kNumSplineFetchers = 5;
+ // Note that we only actually store N - 1 splines consistently, since we need
+ // to keep one fetcher free to check whether there are any new splines.
+ static constexpr size_t kNumSplineFetchers =
+ SplineDrivetrain::kMaxTrajectories;
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 5a4e5da..0f9418c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -36,18 +36,16 @@
void SplineDrivetrain::SetGoal(
const ::frc971::control_loops::drivetrain::Goal *goal) {
- if (goal->has_spline_handle()) {
- commanded_spline_ = goal->spline_handle();
- } else {
- commanded_spline_.reset();
- }
- UpdateSplineHandles();
+ UpdateSplineHandles(goal->has_spline_handle()
+ ? std::make_optional<int>(goal->spline_handle())
+ : std::nullopt);
}
bool SplineDrivetrain::IsCurrentTrajectory(
const fb::Trajectory *trajectory) const {
- return (current_trajectory_ != nullptr &&
- current_trajectory().spline_handle() == trajectory->handle());
+ const FinishedTrajectory *current = current_trajectory();
+ return (current != nullptr &&
+ current->spline_handle() == trajectory->handle());
}
bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
@@ -55,7 +53,7 @@
return false;
}
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ if (trajectories_[ii].spline_handle() == trajectory->handle()) {
return true;
}
}
@@ -66,7 +64,7 @@
CHECK(trajectory != nullptr);
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ if (trajectories_[ii].spline_handle() == trajectory->handle()) {
trajectories_.erase(trajectories_.begin() + ii);
return;
}
@@ -76,49 +74,69 @@
}
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
- trajectories_.emplace_back(std::make_unique<FinishedTrajectory>(
- dt_config_, trajectory, velocity_drivetrain_));
- UpdateSplineHandles();
+ CHECK_LT(trajectories_.size(), trajectories_.capacity());
+ trajectories_.emplace_back(dt_config_, trajectory, velocity_drivetrain_);
+ UpdateSplineHandles(commanded_spline_);
}
void SplineDrivetrain::DeleteCurrentSpline() {
- DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
+ DeleteTrajectory(&CHECK_NOTNULL(current_trajectory())->trajectory());
executing_spline_ = false;
- current_trajectory_ = nullptr;
+ commanded_spline_.reset();
current_xva_.setZero();
}
-void SplineDrivetrain::UpdateSplineHandles() {
+void SplineDrivetrain::UpdateSplineHandles(
+ std::optional<int> commanded_spline) {
// If we are currently executing a spline and have received a change
if (executing_spline_) {
- if (!commanded_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_) {
+ CHECK_NOTNULL(current_trajectory())->spline_handle() !=
+ *commanded_spline) {
// If we are executing a spline, and the handle has changed, garbage
// collect the old spline.
DeleteCurrentSpline();
}
}
}
+ commanded_spline_ = commanded_spline;
// 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_) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
executing_spline_ = true;
- current_trajectory_ = trajectories_[ii].get();
}
}
// 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.
}
+FinishedTrajectory *SplineDrivetrain::current_trajectory() {
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+ return &trajectories_[ii];
+ }
+ }
+ return nullptr;
+}
+
+const FinishedTrajectory *SplineDrivetrain::current_trajectory() const {
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+ return &trajectories_[ii];
+ }
+ }
+ return nullptr;
+}
+
// TODO(alex): Hold position when done following the spline.
void SplineDrivetrain::Update(
bool enable, const ::Eigen::Matrix<double, 5, 1> &state,
@@ -127,17 +145,19 @@
enable_ = enable;
if (enable && executing_spline_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+ const FinishedTrajectory *const trajectory =
+ CHECK_NOTNULL(current_trajectory());
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 = trajectory->FFVoltage(current_xva_(0));
}
const double current_distance = current_xva_(0);
::Eigen::Matrix<double, 2, 5> K =
- current_trajectory().GainForDistance(current_distance);
+ trajectory->GainForDistance(current_distance);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- const bool backwards = current_trajectory().drive_spline_backwards();
+ const bool backwards = trajectory->drive_spline_backwards();
if (backwards) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
@@ -148,11 +168,11 @@
goal_state(4, 0) = -left_goal;
}
const Eigen::Matrix<double, 5, 1> relative_goal =
- current_trajectory().StateToPathRelativeState(current_distance,
- goal_state, backwards);
+ trajectory->StateToPathRelativeState(current_distance, goal_state,
+ backwards);
const Eigen::Matrix<double, 5, 1> relative_state =
- current_trajectory().StateToPathRelativeState(current_distance, state,
- backwards);
+ trajectory->StateToPathRelativeState(current_distance, state,
+ backwards);
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;
@@ -163,7 +183,7 @@
}
::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_ = trajectory->GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
@@ -203,7 +223,7 @@
builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- spline_handles[ii] = trajectories_[ii]->spline_handle();
+ spline_handles[ii] = trajectories_[ii].spline_handle();
}
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
@@ -211,7 +231,7 @@
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
trajectory_logging_builder.add_x(goal_state(0));
trajectory_logging_builder.add_y(goal_state(1));
- if (current_trajectory().drive_spline_backwards()) {
+ if (CHECK_NOTNULL(current_trajectory())->drive_spline_backwards()) {
trajectory_logging_builder.add_left_velocity(-goal_state(4));
trajectory_logging_builder.add_right_velocity(-goal_state(3));
trajectory_logging_builder.add_theta(
@@ -223,8 +243,7 @@
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
}
- trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
- executing_spline_);
+ trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
if (commanded_spline_) {
trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
@@ -233,8 +252,9 @@
}
}
trajectory_logging_builder.add_distance_remaining(
- executing_spline_ ? current_trajectory().length() - current_xva_.x()
- : 0.0);
+ executing_spline_
+ ? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
+ : 0.0);
trajectory_logging_builder.add_available_splines(handles_vector);
return trajectory_logging_builder.Finish();
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 390236b..cd712bc 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -5,7 +5,6 @@
#include <thread>
#include "Eigen/Dense"
-
#include "aos/condition.h"
#include "aos/mutex/mutex.h"
#include "frc971/control_loops/control_loops_generated.h"
@@ -23,6 +22,7 @@
class SplineDrivetrain {
public:
+ static constexpr size_t kMaxTrajectories = 5;
SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
@@ -49,15 +49,15 @@
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return executing_spline_ ? current_trajectory().GoalState(current_xva_(0),
- current_xva_(1))
+ return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+ ->GoalState(current_xva_(0), current_xva_(1))
: ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
- return executing_spline_
- ? current_trajectory().is_at_end(current_xva_.block<2, 1>(0, 0))
- : true;
+ return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+ ->is_at_end(current_xva_.block<2, 1>(0, 0))
+ : true;
}
size_t trajectory_count() const { return trajectories_.size(); }
@@ -70,21 +70,20 @@
// 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
+ // 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_
+ // 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();
+ void UpdateSplineHandles(std::optional<int> commanded_spline);
// Deletes the currently executing trajectory.
void DeleteCurrentSpline();
- const FinishedTrajectory ¤t_trajectory() const {
- return *CHECK_NOTNULL(current_trajectory_);
- }
+ FinishedTrajectory *current_trajectory();
+ const FinishedTrajectory *current_trajectory() const;
const DrivetrainConfig<double> dt_config_;
@@ -97,8 +96,7 @@
// TODO(james): Sort out construction to avoid so much dynamic memory
// allocation...
- std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
- const FinishedTrajectory *current_trajectory_ = nullptr;
+ aos::SizedArray<FinishedTrajectory, kMaxTrajectories> trajectories_;
std::optional<int> commanded_spline_;
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 6bccc74..ab1c55f 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -3,12 +3,12 @@
#include <chrono>
-#include "aos/flatbuffers.h"
#include "Eigen/Dense"
+#include "aos/flatbuffers.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/drivetrain/spline_goal_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/hybrid_state_feedback_loop.h"
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -193,11 +193,11 @@
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
- const DrivetrainConfig<double> config_;
+ DrivetrainConfig<double> config_;
// Robot radiuses.
- const double robot_radius_l_;
- const double robot_radius_r_;
+ double robot_radius_l_;
+ double robot_radius_r_;
float lateral_acceleration_ = 3.0;
float longitudinal_acceleration_ = 2.0;
float voltage_limit_ = 12.0;
@@ -205,7 +205,7 @@
// A wrapper around the Trajectory flatbuffer to allow for controlling to a
// spline using a pre-generated trajectory.
-class FinishedTrajectory : public BaseTrajectory {
+class FinishedTrajectory : public BaseTrajectory {
public:
// Note: The lifetime of the supplied buffer is assumed to be greater than
// that of this object.
@@ -225,6 +225,11 @@
HybridKalman<2, 2, 2>>>(
config.make_hybrid_drivetrain_velocity_loop())) {}
+ FinishedTrajectory(const FinishedTrajectory &) = delete;
+ FinishedTrajectory &operator=(const FinishedTrajectory &) = delete;
+ FinishedTrajectory(FinishedTrajectory &&) = default;
+ FinishedTrajectory &operator=(FinishedTrajectory &&) = default;
+
virtual ~FinishedTrajectory() = default;
// Takes the 5-element state that is [x, y, theta, v_left, v_right] and
@@ -252,7 +257,7 @@
private:
const DistanceSplineBase &spline() const override { return spline_; }
const fb::Trajectory *buffer_;
- const FinishedDistanceSpline spline_;
+ FinishedDistanceSpline spline_;
};
// Class to handle plannign a trajectory and producing a flatbuffer containing
@@ -268,8 +273,7 @@
virtual ~Trajectory() = default;
- std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(
- std::chrono::nanoseconds dt);
+ std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(std::chrono::nanoseconds dt);
enum class VoltageLimit {
kConservative,
@@ -375,10 +379,7 @@
const DistanceSpline &spline() const override { return spline_; }
private:
-
- float plan_velocity(size_t index) const override {
- return plan_[index];
- }
+ float plan_velocity(size_t index) const override { return plan_[index]; }
size_t distance_plan_size() const override { return plan_.size(); }
fb::SegmentConstraint plan_constraint(size_t index) const override {
@@ -410,8 +411,7 @@
inline Eigen::Matrix<double, 5, 1> ContinuousDynamics(
const StateFeedbackHybridPlant<2, 2, 2> &velocity_drivetrain,
const Eigen::Matrix<double, 2, 2> &Tlr_to_la,
- const Eigen::Matrix<double, 5, 1> X,
- const Eigen::Matrix<double, 2, 1> U) {
+ const Eigen::Matrix<double, 5, 1> X, const Eigen::Matrix<double, 2, 1> U) {
const auto &velocity = X.block<2, 1>(3, 0);
const double theta = X(2);
Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;