Reduce mallocs when constructing trajectories
We are mostly allocating because we are lazy. Allocate some of the data
up front and pass around pointers, and use spans to represent the rest
of the data and either point it at vectors, or at the underlying
flatbuffer.
Change-Id: I819dfaa85bc1ba9895eb92efc74a1540d93a6c38
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 9f7eae8..13c8ade 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -607,10 +607,12 @@
deps = [
":spline",
":trajectory_fbs",
+ "//aos/containers:sized_array",
"//aos/logging",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:fixed_quadrature",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
"@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index ab38980..a1f19cd 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -9,16 +9,16 @@
namespace drivetrain {
::std::vector<float> DistanceSpline::BuildDistances(size_t num_alpha) {
- num_alpha = num_alpha == 0 ? 100 * splines_.size() : num_alpha;
+ num_alpha = num_alpha == 0 ? 100 * splines().size() : num_alpha;
::std::vector<float> distances;
distances.push_back(0.0);
- if (splines_.size() > 1) {
+ if (splines().size() > 1) {
// We've got a multispline to follow!
// Confirm that the ends line up to the correct number of derivatives.
- for (size_t i = 1; i < splines_.size(); ++i) {
- const Spline &spline0 = splines_[i - 1];
- const Spline &spline1 = splines_[i];
+ for (size_t i = 1; i < splines().size(); ++i) {
+ const Spline &spline0 = splines()[i - 1];
+ const Spline &spline1 = splines()[i];
const ::Eigen::Matrix<double, 2, 1> end0 = spline0.Point(1.0);
const ::Eigen::Matrix<double, 2, 1> start1 = spline1.Point(0.0);
@@ -57,7 +57,7 @@
}
const double dalpha =
- static_cast<double>(splines_.size()) / static_cast<double>(num_alpha - 1);
+ static_cast<double>(splines().size()) / static_cast<double>(num_alpha - 1);
double last_alpha = 0.0;
for (size_t i = 1; i < num_alpha; ++i) {
const double alpha = dalpha * i;
@@ -66,8 +66,8 @@
[this](double alpha) {
const size_t spline_index = ::std::min(
static_cast<size_t>(::std::floor(alpha)),
- splines_.size() - 1);
- return this->splines_[spline_index]
+ splines().size() - 1);
+ return this->splines()[spline_index]
.DPoint(alpha - spline_index)
.norm();
},
@@ -94,6 +94,24 @@
return splines;
}
+aos::SizedArray<Spline, FinishedDistanceSpline::kMaxSplines>
+SizedFlatbufferToSplines(const MultiSpline *fb) {
+ CHECK_NOTNULL(fb);
+ const size_t spline_count = fb->spline_count();
+ CHECK_EQ(fb->spline_x()->size(), static_cast<size_t>(spline_count * 5 + 1));
+ CHECK_EQ(fb->spline_y()->size(), static_cast<size_t>(spline_count * 5 + 1));
+ aos::SizedArray<Spline, FinishedDistanceSpline::kMaxSplines> splines;
+ for (size_t ii = 0; ii < spline_count; ++ii) {
+ Eigen::Matrix<double, 2, 6> points;
+ for (int jj = 0; jj < 6; ++jj) {
+ points(0, jj) = fb->spline_x()->Get(ii * 5 + jj);
+ points(1, jj) = fb->spline_y()->Get(ii * 5 + jj);
+ }
+ splines.emplace_back(Spline(points));
+ }
+ return splines;
+}
+
DistanceSpline::DistanceSpline(::std::vector<Spline> &&splines, int num_alpha)
: splines_(::std::move(splines)), distances_(BuildDistances(num_alpha)) {}
@@ -106,19 +124,18 @@
// TODO(james): Directly use the flatbuffer vector for accessing distances,
// rather than doing this redundant copy.
-DistanceSpline::DistanceSpline(const fb::DistanceSpline &fb)
- : splines_(FlatbufferToSplines(fb.spline())),
- distances_(CHECK_NOTNULL(fb.distances())->begin(),
- fb.distances()->end()) {}
+FinishedDistanceSpline::FinishedDistanceSpline(const fb::DistanceSpline &fb)
+ : splines_(SizedFlatbufferToSplines(fb.spline())),
+ distances_(fb.distances()->data(), fb.distances()->size()) {}
-flatbuffers::Offset<fb::DistanceSpline> DistanceSpline::Serialize(
+flatbuffers::Offset<fb::DistanceSpline> DistanceSplineBase::Serialize(
flatbuffers::FlatBufferBuilder *fbb,
flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
constraints) const {
- if (splines_.empty()) {
+ if (splines().empty()) {
return {};
}
- const size_t num_points = splines_.size() * 5 + 1;
+ const size_t num_points = splines().size() * 5 + 1;
float *spline_x_vector = nullptr;
float *spline_y_vector = nullptr;
const flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
@@ -127,38 +144,38 @@
fbb->CreateUninitializedVector(num_points, &spline_y_vector);
CHECK_NOTNULL(spline_x_vector);
CHECK_NOTNULL(spline_y_vector);
- spline_x_vector[0] = splines_[0].control_points()(0, 0);
- spline_y_vector[0] = splines_[0].control_points()(1, 0);
- for (size_t spline_index = 0; spline_index < splines_.size();
+ spline_x_vector[0] = splines()[0].control_points()(0, 0);
+ spline_y_vector[0] = splines()[0].control_points()(1, 0);
+ for (size_t spline_index = 0; spline_index < splines().size();
++spline_index) {
for (size_t point = 1; point < 6u; ++point) {
spline_x_vector[spline_index * 5 + point] =
- splines_[spline_index].control_points()(0, point);
+ splines()[spline_index].control_points()(0, point);
spline_y_vector[spline_index * 5 + point] =
- splines_[spline_index].control_points()(1, point);
+ splines()[spline_index].control_points()(1, point);
}
}
MultiSpline::Builder multi_spline_builder(*fbb);
- multi_spline_builder.add_spline_count(splines_.size());
+ multi_spline_builder.add_spline_count(splines().size());
multi_spline_builder.add_spline_x(spline_x_offset);
multi_spline_builder.add_spline_y(spline_y_offset);
multi_spline_builder.add_constraints(constraints);
const flatbuffers::Offset<MultiSpline> multi_spline_offset =
multi_spline_builder.Finish();
const flatbuffers::Offset<flatbuffers::Vector<float>> distances_offset =
- fbb->CreateVector(distances_);
+ fbb->CreateVector(distances().data(), distances().size());
fb::DistanceSpline::Builder spline_builder(*fbb);
spline_builder.add_spline(multi_spline_offset);
spline_builder.add_distances(distances_offset);
return spline_builder.Finish();
}
-::Eigen::Matrix<double, 2, 1> DistanceSpline::DDXY(double distance) const {
+::Eigen::Matrix<double, 2, 1> DistanceSplineBase::DDXY(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
const ::Eigen::Matrix<double, 2, 1> dspline_point =
- splines_[a.index].DPoint(a.alpha);
+ splines()[a.index].DPoint(a.alpha);
const ::Eigen::Matrix<double, 2, 1> ddspline_point =
- splines_[a.index].DDPoint(a.alpha);
+ splines()[a.index].DDPoint(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -169,17 +186,17 @@
::std::pow(squared_norm, 2);
}
-double DistanceSpline::DDTheta(double distance) const {
+double DistanceSplineBase::DDTheta(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
// TODO(austin): We are re-computing DPoint here even worse
const ::Eigen::Matrix<double, 2, 1> dspline_point =
- splines_[a.index].DPoint(a.alpha);
+ splines()[a.index].DPoint(a.alpha);
const ::Eigen::Matrix<double, 2, 1> ddspline_point =
- splines_[a.index].DDPoint(a.alpha);
+ splines()[a.index].DDPoint(a.alpha);
- const double dtheta = splines_[a.index].DTheta(a.alpha);
- const double ddtheta = splines_[a.index].DDTheta(a.alpha);
+ const double dtheta = splines()[a.index].DTheta(a.alpha);
+ const double ddtheta = splines()[a.index].DDTheta(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -189,25 +206,25 @@
::std::pow(squared_norm, 2);
}
-DistanceSpline::AlphaAndIndex DistanceSpline::DistanceToAlpha(
+DistanceSplineBase::AlphaAndIndex DistanceSplineBase::DistanceToAlpha(
double distance) const {
if (distance <= 0.0) {
return {0, 0.0};
}
if (distance >= length()) {
- return {splines_.size() - 1, 1.0};
+ return {splines().size() - 1, 1.0};
}
// Find the distance right below our number using a binary search.
size_t after = ::std::distance(
- distances_.begin(),
- ::std::lower_bound(distances_.begin(), distances_.end(), distance));
+ distances().begin(),
+ ::std::lower_bound(distances().begin(), distances().end(), distance));
size_t before = after - 1;
const double distance_step_size =
- (splines_.size() / static_cast<double>(distances_.size() - 1));
+ (splines().size() / static_cast<double>(distances().size() - 1));
- const double alpha = (distance - distances_[before]) /
- (distances_[after] - distances_[before]) *
+ const double alpha = (distance - distances()[before]) /
+ (distances()[after] - distances()[before]) *
distance_step_size +
static_cast<double>(before) * distance_step_size;
const size_t index = static_cast<size_t>(::std::floor(alpha));
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 0feb902..91b9898 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -4,6 +4,8 @@
#include <vector>
#include "Eigen/Dense"
+#include "absl/types/span.h"
+#include "aos/containers/sized_array.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/fixed_quadrature.h"
@@ -15,16 +17,11 @@
std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb);
// Class to hold a spline as a function of distance.
-class DistanceSpline {
+class DistanceSplineBase {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- DistanceSpline(const Spline &spline, int num_alpha = 0);
- DistanceSpline(::std::vector<Spline> &&splines, int num_alpha = 0);
- DistanceSpline(const MultiSpline *fb, int num_alpha = 0);
- // Copies the distances for the spline directly out of the provided buffer,
- // rather than constructing the distances from the original splines.
- DistanceSpline(const fb::DistanceSpline &fb);
+ virtual ~DistanceSplineBase() {}
flatbuffers::Offset<fb::DistanceSpline> Serialize(
flatbuffers::FlatBufferBuilder *fbb,
@@ -34,13 +31,13 @@
// Returns a point on the spline as a function of distance.
::Eigen::Matrix<double, 2, 1> XY(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
- return splines_[a.index].Point(a.alpha);
+ return splines()[a.index].Point(a.alpha);
}
// Returns the velocity as a function of distance.
::Eigen::Matrix<double, 2, 1> DXY(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
- return splines_[a.index].DPoint(a.alpha).normalized();
+ return splines()[a.index].DPoint(a.alpha).normalized();
}
// Returns the acceleration as a function of distance.
@@ -49,7 +46,7 @@
// Returns the heading as a function of distance.
double Theta(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
- return splines_[a.index].Theta(a.alpha);
+ return splines()[a.index].Theta(a.alpha);
}
// Returns the angular velocity as a function of distance.
@@ -57,7 +54,7 @@
double DTheta(double distance) const {
// TODO(austin): We are re-computing DPoint here!
const AlphaAndIndex a = DistanceToAlpha(distance);
- const Spline &spline = splines_[a.index];
+ const Spline &spline = splines()[a.index];
return spline.DTheta(a.alpha) / spline.DPoint(a.alpha).norm();
}
@@ -71,10 +68,10 @@
double DDTheta(double distance) const;
// Returns the length of the path in meters.
- double length() const { return distances_.back(); }
+ double length() const { return distances().back(); }
- const std::vector<float> &distances() const { return distances_; }
- const std::vector<Spline> &splines() const { return splines_; }
+ virtual const absl::Span<const float> distances() const = 0;
+ virtual const absl::Span<const Spline> splines() const = 0;
private:
struct AlphaAndIndex {
@@ -84,7 +81,21 @@
// Computes alpha for a distance
AlphaAndIndex DistanceToAlpha(double distance) const;
+};
+// Class which computes a DistanceSpline and stores it.
+class DistanceSpline final : public DistanceSplineBase {
+ public:
+ DistanceSpline(const Spline &spline, int num_alpha = 0);
+ DistanceSpline(::std::vector<Spline> &&splines, int num_alpha = 0);
+ DistanceSpline(const MultiSpline *fb, int num_alpha = 0);
+
+ const absl::Span<const float> distances() const override {
+ return distances_;
+ }
+ const absl::Span<const Spline> splines() const override { return splines_; }
+
+ private:
::std::vector<float> BuildDistances(size_t num_alpha);
// The spline we are converting to a distance.
@@ -93,6 +104,28 @@
const ::std::vector<float> distances_;
};
+// Class exposing a finished DistanceSpline flatbuffer as a DistanceSpline.
+//
+// The lifetime of the provided fb::DistanceSpline needs to out-live this class.
+class FinishedDistanceSpline final : public DistanceSplineBase {
+ public:
+ static constexpr size_t kMaxSplines = 6;
+ FinishedDistanceSpline(const fb::DistanceSpline &fb);
+
+ const absl::Span<const float> distances() const override {
+ return distances_;
+ }
+ const absl::Span<const Spline> splines() const override { return splines_; }
+
+ private:
+ ::std::vector<float> BuildDistances(size_t num_alpha);
+
+ // 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_;
+};
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 5b244a6..0dd58ba 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -157,7 +157,7 @@
flatbuffers::FlatBufferBuilder fbb;
fbb.Finish(distance_spline_.Serialize(&fbb, {}));
const aos::FlatbufferDetachedBuffer<fb::DistanceSpline> spline(fbb.Release());
- DistanceSpline reread_spline(spline.message());
+ FinishedDistanceSpline reread_spline(spline.message());
ASSERT_EQ(reread_spline.distances().size(),
distance_spline_.distances().size());
for (size_t ii = 0; ii < distance_spline_.distances().size(); ++ii) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 629150c..5a4e5da 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,9 +1,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#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"
@@ -18,6 +16,11 @@
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
: dt_config_(dt_config),
+ velocity_drivetrain_(
+ std::make_shared<StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ dt_config_.make_hybrid_drivetrain_velocity_loop())),
current_xva_(Eigen::Vector3d::Zero()),
next_xva_(Eigen::Vector3d::Zero()),
next_U_(Eigen::Vector2d::Zero()) {}
@@ -73,8 +76,8 @@
}
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
- trajectories_.emplace_back(
- std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
+ trajectories_.emplace_back(std::make_unique<FinishedTrajectory>(
+ dt_config_, trajectory, velocity_drivetrain_));
UpdateSplineHandles();
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 303701f..390236b 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -88,6 +88,11 @@
const DrivetrainConfig<double> dt_config_;
+ std::shared_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
+
bool executing_spline_ = false;
// TODO(james): Sort out construction to avoid so much dynamic memory
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index c92a81b..27b31a0 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -33,11 +33,15 @@
}
} // namespace
-FinishedTrajectory::FinishedTrajectory(const DrivetrainConfig<double> &config,
- const fb::Trajectory *buffer)
+FinishedTrajectory::FinishedTrajectory(
+ 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)
: BaseTrajectory(CHECK_NOTNULL(CHECK_NOTNULL(buffer->spline())->spline())
->constraints(),
- config),
+ config, std::move(velocity_drivetrain)),
buffer_(buffer),
spline_(*buffer_->spline()) {}
@@ -77,15 +81,12 @@
BaseTrajectory::BaseTrajectory(
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
- const DrivetrainConfig<double> &config)
- : velocity_drivetrain_(
- std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>(
- new StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>(
- config.make_hybrid_drivetrain_velocity_loop()))),
+ 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),
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 60695de..6bccc74 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -44,7 +44,20 @@
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())) {}
+
+ BaseTrajectory(
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ const DrivetrainConfig<double> &config,
+ std::shared_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain);
virtual ~BaseTrajectory() = default;
@@ -94,7 +107,7 @@
Eigen::Matrix<double, 2, 1> *K4,
Eigen::Matrix<double, 2, 1> *K5) const;
- virtual const DistanceSpline &spline() const = 0;
+ virtual const DistanceSplineBase &spline() const = 0;
// Returns the length of the path in meters.
double length() const { return spline().length(); }
@@ -175,7 +188,7 @@
const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
ConstraintType type);
- std::unique_ptr<
+ std::shared_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
@@ -196,8 +209,21 @@
public:
// 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,
+ std::shared_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain);
+
explicit FinishedTrajectory(const DrivetrainConfig<double> &config,
- const fb::Trajectory *buffer);
+ 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())) {}
virtual ~FinishedTrajectory() = default;
@@ -224,9 +250,9 @@
const fb::Trajectory &trajectory() const { return *buffer_; }
private:
- const DistanceSpline &spline() const override { return spline_; }
+ const DistanceSplineBase &spline() const override { return spline_; }
const fb::Trajectory *buffer_;
- const DistanceSpline spline_;
+ const FinishedDistanceSpline spline_;
};
// Class to handle plannign a trajectory and producing a flatbuffer containing