Remove mallocs in realtime trajectory code
Modifications to GetDrivetrainConfig for y2024 (which uses the constants
flatbuffer as a source of the drivetrain config) make it
non-realtime-safe.
Change-Id: Iad160acf3cf09d0feacb4498d03884f1065acc94
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 57abe69..b1af886 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -120,8 +120,9 @@
int num_distance) {
return new Trajectory(
DistanceSpline(*spline),
- ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), nullptr, -1,
- vmax, num_distance);
+ std::make_unique<DrivetrainConfig<double>>(
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig()),
+ nullptr, -1, vmax, num_distance);
}
void deleteTrajectory(Trajectory *t) { delete t; }
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 508967c..f90e7e0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -74,7 +74,7 @@
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
CHECK_LT(trajectories_.size(), trajectories_.capacity());
- trajectories_.emplace_back(dt_config_, trajectory, velocity_drivetrain_);
+ trajectories_.emplace_back(&dt_config_, trajectory, velocity_drivetrain_);
UpdateSplineHandles(commanded_spline_);
}
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 36d0947..a876ae3 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -33,7 +33,7 @@
} // namespace
FinishedTrajectory::FinishedTrajectory(
- const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+ const DrivetrainConfig<double> *config, const fb::Trajectory *buffer,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
@@ -80,15 +80,15 @@
BaseTrajectory::BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config,
+ const DrivetrainConfig<double> *config,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain)
: velocity_drivetrain_(std::move(velocity_drivetrain)),
config_(config),
- robot_radius_l_(config.robot_radius),
- robot_radius_r_(config.robot_radius),
+ robot_radius_l_(config->robot_radius),
+ robot_radius_r_(config->robot_radius),
lateral_acceleration_(
ConstraintValue(constraints, ConstraintType::LATERAL_ACCELERATION)),
longitudinal_acceleration_(ConstraintValue(
@@ -96,7 +96,7 @@
voltage_limit_(ConstraintValue(constraints, ConstraintType::VOLTAGE)) {}
Trajectory::Trajectory(const SplineGoal &spline_goal,
- const DrivetrainConfig<double> &config)
+ const DrivetrainConfig<double> *config)
: Trajectory(DistanceSpline{spline_goal.spline()}, config,
spline_goal.spline()->constraints(),
spline_goal.spline_idx()) {
@@ -104,7 +104,7 @@
}
Trajectory::Trajectory(
- DistanceSpline &&input_spline, const DrivetrainConfig<double> &config,
+ DistanceSpline &&input_spline, const DrivetrainConfig<double> *config,
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
int spline_idx, double vmax, int num_distance)
: BaseTrajectory(constraints, config),
@@ -127,6 +127,15 @@
}
}
+Trajectory::Trajectory(
+ DistanceSpline &&spline, std::unique_ptr<DrivetrainConfig<double>> config,
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ int spline_idx, double vmax, int num_distance)
+ : Trajectory(std::move(spline), config.get(), constraints, spline_idx, vmax,
+ num_distance) {
+ owned_config_ = std::move(config);
+}
+
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
const double distance = Distance(i);
@@ -751,7 +760,8 @@
// finite-horizon much longer (albeit with the extension just using the
// linearization for the infal point).
void Trajectory::CalculatePathGains() {
- const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan = PlanXVA(config_.dt);
+ const std::vector<Eigen::Matrix<double, 3, 1>> xva_plan =
+ PlanXVA(config_->dt);
if (xva_plan.empty()) {
LOG(ERROR) << "Plan is empty--unable to plan trajectory.";
return;
@@ -783,7 +793,7 @@
PathRelativeContinuousSystem(distance, &A_continuous, &B_continuous);
Eigen::Matrix<double, 5, 5> A_discrete;
Eigen::Matrix<double, 5, 2> B_discrete;
- controls::C2D(A_continuous, B_continuous, config_.dt, &A_discrete,
+ controls::C2D(A_continuous, B_continuous, config_->dt, &A_discrete,
&B_discrete);
if (i == max_index) {
@@ -898,9 +908,9 @@
result(2, 0) = spline().Theta(distance);
result.block<2, 1>(3, 0) =
- config_.Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
- spline().DThetaDt(distance, velocity))
- .finished();
+ config_->Tla_to_lr() * (Eigen::Matrix<double, 2, 1>() << velocity,
+ spline().DThetaDt(distance, velocity))
+ .finished();
return result;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a9f14f6..5964bf5 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -14,6 +14,11 @@
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/control_loops/state_feedback_loop.h"
+// Note for all of these classes:
+// Whenever a pointer to a DrivetrainConfig is taken in the constructor, it must
+// live for the entire lifetime of the object. The classes here do not take
+// ownership of the pointer.
+
namespace frc971::control_loops::drivetrain {
template <typename F>
@@ -43,16 +48,16 @@
public:
BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config)
+ const DrivetrainConfig<double> *config)
: BaseTrajectory(constraints, config,
std::make_shared<StateFeedbackLoop<
2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>(
- config.make_hybrid_drivetrain_velocity_loop())) {}
+ config->make_hybrid_drivetrain_velocity_loop())) {}
BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config,
+ const DrivetrainConfig<double> *config,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
@@ -192,7 +197,7 @@
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
- DrivetrainConfig<double> config_;
+ const DrivetrainConfig<double> *config_;
// Robot radiuses.
double robot_radius_l_;
@@ -209,20 +214,20 @@
// Note: The lifetime of the supplied buffer is assumed to be greater than
// that of this object.
explicit FinishedTrajectory(
- const DrivetrainConfig<double> &config, const fb::Trajectory *buffer,
+ const DrivetrainConfig<double> *config, const fb::Trajectory *buffer,
std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain);
- explicit FinishedTrajectory(const DrivetrainConfig<double> &config,
+ explicit FinishedTrajectory(const DrivetrainConfig<double> *config,
const fb::Trajectory *buffer)
: FinishedTrajectory(
config, buffer,
std::make_shared<StateFeedbackLoop<
2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>(
- config.make_hybrid_drivetrain_velocity_loop())) {}
+ config->make_hybrid_drivetrain_velocity_loop())) {}
FinishedTrajectory(const FinishedTrajectory &) = delete;
FinishedTrajectory &operator=(const FinishedTrajectory &) = delete;
@@ -264,16 +269,23 @@
class Trajectory : public BaseTrajectory {
public:
Trajectory(const SplineGoal &spline_goal,
- const DrivetrainConfig<double> &config);
+ const DrivetrainConfig<double> *config);
Trajectory(
- DistanceSpline &&spline, const DrivetrainConfig<double> &config,
+ DistanceSpline &&spline, const DrivetrainConfig<double> *config,
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
int spline_idx = 0, double vmax = 10.0, int num_distance = 0);
+ // Version that owns its own DrivetrainConfig.
+ Trajectory(
+ DistanceSpline &&spline, std::unique_ptr<DrivetrainConfig<double>> config,
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ int spline_idx, double vmax, int num_distance);
+
virtual ~Trajectory() = default;
std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(std::chrono::nanoseconds dt);
+ const DrivetrainConfig<double> *drivetrain_config() { return config_; }
enum class VoltageLimit {
kConservative,
kAggressive,
@@ -390,7 +402,8 @@
// The spline we are planning for.
const DistanceSpline spline_;
- const DrivetrainConfig<double> config_;
+ std::unique_ptr<DrivetrainConfig<double>> owned_config_;
+ const DrivetrainConfig<double> *config_;
// Velocities in the plan (distance for each index is defined by Distance())
std::vector<double> plan_;
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index 8e3e0f8..e531167 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -14,7 +14,7 @@
}
void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
- Trajectory trajectory(goal, dt_config_);
+ Trajectory trajectory(goal, &dt_config_);
trajectory.Plan();
aos::Sender<fb::Trajectory>::Builder builder =
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 8c3ef47..9eb95b5 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -50,7 +50,7 @@
(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2 * FLAGS_forward,
-0.2 * FLAGS_forward, FLAGS_forward, 0.0, 0.0, 1.0, 1.0)
.finished()))),
- config, nullptr);
+ &config, nullptr);
trajectory.set_lateral_acceleration(2.0);
trajectory.set_longitudinal_acceleration(1.0);
@@ -131,7 +131,7 @@
aos::FlatbufferDetachedBuffer<fb::Trajectory> trajectory_buffer(
fbb.Release());
- FinishedTrajectory finished_trajectory(config, &trajectory_buffer.message());
+ FinishedTrajectory finished_trajectory(&config, &trajectory_buffer.message());
::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
state(0, 0) = FLAGS_dx;
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index fbf8970..6e9089a 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -74,7 +74,7 @@
const int spline_index = 12345;
// Run lots of steps to make the feedforwards terms more accurate.
trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
- DistanceSpline(GetParam().control_points), dt_config_,
+ DistanceSpline(GetParam().control_points), &dt_config_,
/*constraints=*/nullptr, spline_index, GetParam().velocity_limit));
distance_spline_ = &trajectory_->spline();
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
@@ -111,7 +111,7 @@
EXPECT_EQ(spline_index, trajectory_buffer_->message().handle());
finished_trajectory_ = std::make_unique<FinishedTrajectory>(
- dt_config_, &trajectory_buffer_->message());
+ &dt_config_, &trajectory_buffer_->message());
}
void TearDown() {
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 7f0b13d..7c33579 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -173,7 +173,7 @@
spline_goal_buffer(fbb.Release());
frc971::control_loops::drivetrain::Trajectory trajectory(
- spline_goal_buffer.message(), dt_config_);
+ spline_goal_buffer.message(), &dt_config_);
trajectory.Plan();
flatbuffers::FlatBufferBuilder traj_fbb;