Merge "Reduce mallocs when constructing trajectories"
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