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/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1da9db3..b08e96f 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -6,10 +6,20 @@
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
+ name = "spline_goal_fbs",
+ srcs = ["spline_goal.fbs"],
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
name = "drivetrain_goal_fbs",
srcs = ["drivetrain_goal.fbs"],
gen_reflections = 1,
- includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+ includes = [
+ ":spline_goal_fbs_includes",
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
)
flatbuffer_cc_library(
@@ -31,6 +41,13 @@
includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
+flatbuffer_cc_library(
+ name = "trajectory_fbs",
+ srcs = ["trajectory.fbs"],
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
flatbuffer_ts_library(
name = "drivetrain_status_ts_fbs",
srcs = ["drivetrain_status.fbs"],
@@ -70,7 +87,10 @@
name = "drivetrain_goal_float_fbs",
srcs = ["drivetrain_goal_float.fbs"],
gen_reflections = 1,
- includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+ includes = [
+ ":spline_goal_fbs_includes",
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
)
flatbuffer_cc_library(
@@ -118,6 +138,8 @@
src = "drivetrain_config.json",
flatbuffers = [
":drivetrain_goal_fbs",
+ ":trajectory_fbs",
+ ":spline_goal_fbs",
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":drivetrain_position_fbs",
@@ -220,6 +242,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":spline",
+ ":spline_goal_fbs",
":trajectory",
"//aos:condition",
"//aos:init",
@@ -243,6 +266,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":localizer",
+ ":spline_goal_fbs",
"//aos:math",
"//aos/util:math",
"//frc971/control_loops:c2d",
@@ -289,6 +313,7 @@
":drivetrain_status_fbs",
":gear",
":localizer",
+ ":spline_goal_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
@@ -322,6 +347,7 @@
"//aos/controls:polytope",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:control_loops_fbs",
+ ":spline_goal_fbs",
"//frc971/control_loops:state_feedback_loop",
] + select({
"@platforms//os:linux": [
@@ -396,6 +422,7 @@
":localizer",
":localizer_fbs",
":polydrivetrain",
+ ":spline_goal_fbs",
":splinedrivetrain",
":ssdrivetrain",
"//aos/controls:control_loop",
@@ -462,6 +489,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":drivetrain_config",
+ ":trajectory_generator",
":drivetrain_lib",
":localizer_fbs",
":drivetrain_goal_fbs",
@@ -568,8 +596,11 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":spline",
+ ":trajectory_fbs",
"//aos/logging",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:fixed_quadrature",
+ "@com_github_google_glog//:glog",
"@org_tuxfamily_eigen//:eigen",
],
)
@@ -591,6 +622,7 @@
deps = [
":distance_spline",
"//aos/testing:googletest",
+ "//aos:flatbuffers",
"//aos/testing:test_shm",
"@com_github_gflags_gflags//:gflags",
] + cpu_select({
@@ -609,6 +641,8 @@
deps = [
":distance_spline",
":drivetrain_config",
+ ":spline_goal_fbs",
+ ":trajectory_fbs",
"//frc971/control_loops:c2d",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:hybrid_state_feedback_loop",
@@ -618,6 +652,20 @@
],
)
+cc_library(
+ name = "trajectory_generator",
+ srcs = ["trajectory_generator.cc"],
+ hdrs = ["trajectory_generator.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":distance_spline",
+ ":drivetrain_config",
+ ":spline_goal_fbs",
+ ":trajectory",
+ ":trajectory_fbs",
+ ],
+)
+
cc_binary(
name = "trajectory_plot",
srcs = [
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index 113eb27..5c05fe4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -2,14 +2,15 @@
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/spline.h"
+#include "glog/logging.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-::std::vector<double> DistanceSpline::BuildDistances(size_t num_alpha) {
+::std::vector<float> DistanceSpline::BuildDistances(size_t num_alpha) {
num_alpha = num_alpha == 0 ? 100 * splines_.size() : num_alpha;
- ::std::vector<double> distances;
+ ::std::vector<float> distances;
distances.push_back(0.0);
if (splines_.size() > 1) {
@@ -76,12 +77,82 @@
return distances;
}
+std::vector<Spline> FlatbufferToSplines(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));
+ std::vector<Spline> 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)) {}
DistanceSpline::DistanceSpline(const Spline &spline, int num_alpha)
: splines_({spline}), distances_(BuildDistances(num_alpha)) {}
+DistanceSpline::DistanceSpline(const MultiSpline *fb, int num_alpha)
+ : splines_(FlatbufferToSplines(fb)),
+ distances_(BuildDistances(num_alpha)) {}
+
+// 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()) {}
+
+flatbuffers::Offset<fb::DistanceSpline> DistanceSpline::Serialize(
+ flatbuffers::FlatBufferBuilder *fbb,
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
+ constraints) const {
+ if (splines_.empty()) {
+ return {};
+ }
+ 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 =
+ fbb->CreateUninitializedVector(num_points, &spline_x_vector);
+ const flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ 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_index) {
+ for (size_t point = 1; point < 6u; ++point) {
+ spline_x_vector[spline_index * 5 + point] =
+ splines_[spline_index].control_points()(0, point);
+ spline_y_vector[spline_index * 5 + 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_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_);
+ 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 {
const AlphaAndIndex a = DistanceToAlpha(distance);
const ::Eigen::Matrix<double, 2, 1> dspline_point =
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index ab88560..0feb902 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -5,12 +5,15 @@
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/spline.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/fixed_quadrature.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
+std::vector<Spline> FlatbufferToSplines(const MultiSpline *fb);
+
// Class to hold a spline as a function of distance.
class DistanceSpline {
public:
@@ -18,6 +21,15 @@
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);
+
+ flatbuffers::Offset<fb::DistanceSpline> Serialize(
+ flatbuffers::FlatBufferBuilder *fbb,
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
+ constraints) const;
// Returns a point on the spline as a function of distance.
::Eigen::Matrix<double, 2, 1> XY(double distance) const {
@@ -61,6 +73,9 @@
// Returns the length of the path in meters.
double length() const { return distances_.back(); }
+ const std::vector<float> &distances() const { return distances_; }
+ const std::vector<Spline> &splines() const { return splines_; }
+
private:
struct AlphaAndIndex {
size_t index;
@@ -70,12 +85,12 @@
// Computes alpha for a distance
AlphaAndIndex DistanceToAlpha(double distance) const;
- ::std::vector<double> BuildDistances(size_t num_alpha);
+ ::std::vector<float> BuildDistances(size_t num_alpha);
// The spline we are converting to a distance.
const ::std::vector<Spline> splines_;
// An interpolation table of distances evenly distributed in alpha.
- const ::std::vector<double> distances_;
+ const ::std::vector<float> distances_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 8189fe9..1ed756a 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -3,6 +3,7 @@
#include <vector>
#include "aos/testing/test_shm.h"
+#include "aos/flatbuffers.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#if defined(SUPPORT_PLOT)
@@ -152,6 +153,24 @@
#endif
}
+TEST_P(ParameterizedDistanceSplineTest, Serialization) {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(distance_spline_.Serialize(&fbb, {}));
+ const aos::FlatbufferDetachedBuffer<fb::DistanceSpline> spline(fbb.Release());
+ DistanceSpline 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) {
+ const float orig_distance = distance_spline_.distances()[ii];
+ const float new_distance = reread_spline.distances()[ii];
+ EXPECT_EQ(orig_distance, new_distance);
+ EXPECT_FLOAT_EQ(distance_spline_.XY(orig_distance).x(),
+ reread_spline.XY(new_distance).x());
+ EXPECT_FLOAT_EQ(distance_spline_.XY(orig_distance).y(),
+ reread_spline.XY(new_distance).y());
+ }
+}
+
INSTANTIATE_TEST_CASE_P(
DistanceSplineTest, ParameterizedDistanceSplineTest,
::testing::Values(
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6336386..1395e5d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -309,6 +309,41 @@
dt_spline_(dt_config_),
dt_line_follow_(dt_config_, localizer->target_selector()) {
event_loop->SetRuntimeRealtimePriority(30);
+ for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
+ trajectory_fetchers_[ii].fetcher =
+ event_loop->MakeFetcher<fb::Trajectory>("/drivetrain");
+ }
+}
+
+void DrivetrainLoop::UpdateTrajectoryFetchers() {
+ for (auto &fetcher : trajectory_fetchers_) {
+ const fb::Trajectory *trajectory = fetcher.fetcher.get();
+ // If the current fetcher is already being used by the SplineDrivetrain,
+ // don't touch it.
+ // We have to check both in_use and HasTrajectory because if
+ // in_use is true and HasTrajectory() is false, that implies that the
+ // SplineDrivetrain has finished executing the trajectory and disposed of
+ // it; if in_use is false and HasTrajectory() is true, that implies that
+ // this fetcher is at the same point in the queue as another fetcher, and
+ // that the other fetcher is the one that we are using to keep the message
+ // pinned.
+ // TODO(james): Consider garbage-collecting splines once we run out of
+ // fetchers.
+ if (fetcher.in_use && dt_spline_.HasTrajectory(trajectory)) {
+ continue;
+ }
+ fetcher.in_use = false;
+ // Go through and find the next Trajectory that isn't already held by the
+ // SplineDrivetrain, and add it.
+ while (fetcher.fetcher.FetchNext()) {
+ trajectory = fetcher.fetcher.get();
+ if (!dt_spline_.HasTrajectory(trajectory)) {
+ fetcher.in_use = true;
+ dt_spline_.AddTrajectory(trajectory);
+ break;
+ }
+ }
+ }
}
void DrivetrainLoop::RunIteration(
@@ -326,6 +361,8 @@
filters_.Reset(monotonic_now, position);
}
+ UpdateTrajectoryFetchers();
+
filters_.Correct(monotonic_now, position);
// Set the gear-logging parts of the status
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9a23edd..070cd03 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -144,6 +144,8 @@
class DrivetrainLoop
: public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ static constexpr size_t kNumSplineFetchers = 4;
+
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
@@ -154,6 +156,11 @@
virtual ~DrivetrainLoop() {}
protected:
+ struct TrajectoryFetcherState {
+ aos::Fetcher<fb::Trajectory> fetcher;
+ bool in_use = false;
+ };
+
// Executes one cycle of the control loop.
void RunIteration(
const ::frc971::control_loops::drivetrain::Goal *goal,
@@ -165,8 +172,11 @@
flatbuffers::Offset<drivetrain::Output> Zero(
aos::Sender<drivetrain::Output>::Builder *builder) override;
+ void UpdateTrajectoryFetchers();
+
const DrivetrainConfig<double> dt_config_;
DrivetrainFilters filters_;
+ std::array<TrajectoryFetcherState, kNumSplineFetchers> trajectory_fetchers_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index e505c82..86318ca 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -7,6 +7,20 @@
"max_size": 2000,
"frequency": 200
},
+ /* TODO(james): Experiment with size limitations. */
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+ "max_size": 200000,
+ "frequency": 10,
+ "read_method": "PIN",
+ "num_readers": 6
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.SplineGoal",
+ "frequency": 10
+ },
{
"name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
diff --git a/frc971/control_loops/drivetrain/drivetrain_goal.fbs b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
index 3df5210..25fe93a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_goal.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
@@ -1,4 +1,4 @@
-include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/drivetrain/spline_goal.fbs";
namespace frc971.control_loops.drivetrain;
@@ -9,17 +9,6 @@
LINE_FOLLOWER,
}
-table SplineGoal {
- // index of the spline. Zero indicates the spline should not be computed.
- spline_idx:int (id: 0);
-
- // Acutal spline.
- spline:frc971.MultiSpline (id: 1);
-
- // Whether to follow the spline driving forwards or backwards.
- drive_spline_backwards:bool (id: 2);
-}
-
table Goal {
// Position of the steering wheel (positive = turning left when going
// forwards).
@@ -56,9 +45,11 @@
// Parameters for a spline to follow. This just contains info on a spline to
// compute. Each time this is sent, spline drivetrain will compute a new
// spline.
- spline:SplineGoal (id: 14);
+ // TODO(james): Is this actually how we want to manage deprecation?
+ spline:SplineGoal (id: 14, deprecated);
- // Which spline to follow.
+ // Which spline to follow; if this is not populated, any running spline will
+ // be cancelled.
spline_handle:int (id: 15);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b3097ec..7dd5a52 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -10,6 +10,7 @@
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
+#include "gmock/gmock.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
@@ -19,6 +20,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/queues/gyro_generated.h"
@@ -43,6 +45,8 @@
test_event_loop_(MakeEventLoop("test")),
drivetrain_goal_sender_(
test_event_loop_->MakeSender<Goal>("/drivetrain")),
+ trajectory_goal_sender_(
+ test_event_loop_->MakeSender<SplineGoal>("/drivetrain")),
drivetrain_goal_fetcher_(
test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
drivetrain_status_fetcher_(
@@ -52,9 +56,12 @@
localizer_control_sender_(
test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop("drivetrain")),
+ trajectory_generator_event_loop_(MakeEventLoop("trajectory_generator")),
dt_config_(GetTestDrivetrainConfig()),
localizer_(drivetrain_event_loop_.get(), dt_config_),
drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ trajectory_generator_(trajectory_generator_event_loop_.get(),
+ dt_config_),
drivetrain_plant_event_loop_(MakeEventLoop("drivetrain_plant")),
drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_) {
// Too many tests care...
@@ -106,16 +113,6 @@
EXPECT_NEAR(actual(1), estimated_y, spline_estimate_tolerance_);
}
- void WaitForTrajectoryPlan() {
- do {
- // Run for fewer iterations while the worker thread computes.
- ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- RunFor(dt());
- EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
- ->planning_state() != PlanningState::PLANNED);
- }
-
void WaitForTrajectoryExecution() {
do {
RunFor(dt());
@@ -150,6 +147,7 @@
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
+ ::aos::Sender<SplineGoal> trajectory_goal_sender_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
@@ -159,10 +157,12 @@
::aos::Sender<LocalizerControl> localizer_control_sender_;
::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> trajectory_generator_event_loop_;
const DrivetrainConfig<double> dt_config_;
DeadReckonEkf localizer_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_;
+ TrajectoryGenerator trajectory_generator_;
::std::unique_ptr<::aos::EventLoop> drivetrain_plant_event_loop_;
DrivetrainSimulation drivetrain_plant_;
@@ -543,7 +543,7 @@
TEST_F(DrivetrainTest, SplineSimple) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -564,13 +564,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -582,17 +576,17 @@
goal_builder.add_spline_handle(1);
EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
+
// Tests that we can drive a spline backwards.
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>(
@@ -615,13 +609,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(true);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -632,7 +621,6 @@
goal_builder.add_spline_handle(1);
EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
// Check that we are right on the spline at the start (otherwise the feedback
// will tend to correct for going the wrong direction).
@@ -659,7 +647,7 @@
TEST_F(DrivetrainTest, SplineSingleGoal) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -680,16 +668,16 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
+ EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
goal_builder.add_spline_handle(1);
EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
@@ -699,7 +687,7 @@
TEST_F(DrivetrainTest, SplineStop) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -720,23 +708,18 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
- const double goal_y =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Now stop.
{
@@ -744,25 +727,28 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- RunFor(chrono::milliseconds(2000));
+ for (int i = 0; i < 100; ++i) {
+ RunFor(dt());
+ ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
+ EXPECT_EQ(0.0, drivetrain_output_fetcher_->left_voltage());
+ EXPECT_EQ(0.0, drivetrain_output_fetcher_->right_voltage());
+ // The goal should be null after stopping.
+ ASSERT_TRUE(drivetrain_status_fetcher_.Fetch());
+ EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->has_x());
+ EXPECT_FALSE(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->has_y());
+ }
- // The goal shouldn't change after being stopped.
- drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
- goal_x, 1e-9);
- EXPECT_NEAR(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
- goal_y, 1e-9);
}
// Tests that a spline can't be restarted.
TEST_F(DrivetrainTest, SplineRestart) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -783,23 +769,19 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
- const double goal_y =
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Send a stop goal.
{
@@ -807,7 +789,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
@@ -817,25 +799,23 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
- // The goal shouldn't change after being stopped and restarted.
+ // The goal should be empty.
drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
- goal_x, 1e-9);
- EXPECT_NEAR(
- CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
- goal_y, 1e-9);
+ EXPECT_FALSE(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_x());
+ EXPECT_FALSE(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_y());
}
// Tests that simple spline converges when it doesn't start where it thinks.
TEST_F(DrivetrainTest, SplineOffset) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.2, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -856,13 +836,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -871,9 +845,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(5000));
VerifyNearSplineGoal();
@@ -884,7 +857,7 @@
TEST_F(DrivetrainTest, SplineSideOffset) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -905,13 +878,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -920,9 +887,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(5000));
@@ -940,7 +906,7 @@
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(0.5);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -961,13 +927,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -976,9 +936,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(5000));
// Since the voltage error compensation is disabled, expect that we will have
@@ -1004,7 +963,7 @@
TEST_F(DrivetrainTest, MultiSpline) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>(
@@ -1027,13 +986,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -1042,9 +995,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(4000));
VerifyNearSplineGoal();
@@ -1054,7 +1006,7 @@
TEST_F(DrivetrainTest, SequentialSplines) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1075,13 +1027,7 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -1090,9 +1036,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
@@ -1100,7 +1045,7 @@
// Second spline.
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1121,13 +1066,7 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
RunFor(dt());
@@ -1137,18 +1076,18 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
VerifyNearSplineGoal();
}
+
// Tests that a second spline will run if the first is stopped.
TEST_F(DrivetrainTest, SplineStopFirst) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1169,18 +1108,24 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
- RunFor(chrono::milliseconds(1000));
+ RunFor(chrono::milliseconds(2000));
+ {
+ drivetrain_status_fetcher_.Fetch();
+ EXPECT_TRUE(
+ drivetrain_status_fetcher_->trajectory_logging()->is_executing());
+ EXPECT_FALSE(
+ drivetrain_status_fetcher_->trajectory_logging()->is_executed());
+ }
// Stop goal
{
@@ -1188,13 +1133,13 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
// Second spline goal.
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1215,16 +1160,15 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
goal_builder.add_spline_handle(2);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
WaitForTrajectoryExecution();
spline_control_tolerance_ = 0.1;
@@ -1237,7 +1181,7 @@
TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1258,22 +1202,14 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- goal_builder.add_spline_handle(0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(1000));
// Plan another spline, but don't start it yet:
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.25, 1.0});
@@ -1294,16 +1230,8 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- goal_builder.add_spline_handle(0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
// Now execute it.
{
@@ -1311,7 +1239,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
@@ -1323,7 +1251,7 @@
TEST_F(DrivetrainTest, ParallelSplines) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1344,19 +1272,12 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
// Second spline goal
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
@@ -1377,14 +1298,14 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ }
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
@@ -1394,7 +1315,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(4000));
@@ -1405,7 +1326,7 @@
TEST_F(DrivetrainTest, OnlyPlanSpline) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1426,23 +1347,12 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
for (int i = 0; i < 100; ++i) {
RunFor(dt());
drivetrain_status_fetcher_.Fetch();
- EXPECT_EQ(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
- ->planning_state(),
- PlanningState::PLANNED);
- ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
}
VerifyNearSplineGoal();
}
@@ -1451,7 +1361,7 @@
TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
SetEnabled(true);
{
- auto builder = drivetrain_goal_sender_.MakeBuilder();
+ auto builder = trajectory_goal_sender_.MakeBuilder();
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
@@ -1472,15 +1382,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- flatbuffers::Offset<SplineGoal> spline_goal_offset =
- spline_goal_builder.Finish();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
- goal_builder.add_spline(spline_goal_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
- WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
// Start goal
@@ -1489,13 +1392,107 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- RunFor(chrono::milliseconds(2000));
+ WaitForTrajectoryExecution();
VerifyNearPosition(1.0, 1.0);
}
+// Tests that when we send a bunch of splines we can fill up the internal
+// buffers and that we handle that case sanely.
+TEST_F(DrivetrainTest, FillSplineBuffer) {
+ SetEnabled(true);
+ std::vector<int> sent_spline_indices;
+ constexpr size_t kExtraSplines = 4;
+ for (size_t spline_index = 1;
+ spline_index < DrivetrainLoop::kNumSplineFetchers + 1 + kExtraSplines;
+ ++spline_index) {
+ auto builder = trajectory_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(spline_index);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ RunFor(dt());
+
+ sent_spline_indices.push_back(spline_index);
+ }
+
+ drivetrain_status_fetcher_.Fetch();
+
+ ASSERT_EQ(DrivetrainLoop::kNumSplineFetchers,
+ CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines())
+ ->size());
+ for (size_t ii = 0; ii < DrivetrainLoop::kNumSplineFetchers; ++ii) {
+ EXPECT_EQ(sent_spline_indices[ii],
+ CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines())
+ ->Get(ii));
+ }
+
+ // Next, start going through and executing all the splines; we should
+ // gradually work through the splines.
+ for (size_t ii = 0; ii < sent_spline_indices.size(); ++ii) {
+ const int current_spline = sent_spline_indices[ii];
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(current_spline);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ // Run for two iterations to give the drivetrain time to resolve all of its
+ // internal state for handling splines (coordinating the fetchers and goal
+ // message to get this two all happen in 1 message is entirely possible, but
+ // has no practical need).
+ RunFor(2 * dt());
+ drivetrain_status_fetcher_.Fetch();
+
+ ASSERT_EQ(current_spline, drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->current_spline_idx())
+ << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
+
+ const int num_available_splines = std::min(
+ sent_spline_indices.size() - ii, DrivetrainLoop::kNumSplineFetchers);
+
+ ASSERT_EQ(num_available_splines,
+ CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines())
+ ->size())
+ << aos::FlatbufferToJson(drivetrain_status_fetcher_.get());
+ for (int jj = 0; jj < num_available_splines; ++jj) {
+ EXPECT_EQ(sent_spline_indices[ii + jj],
+ CHECK_NOTNULL(drivetrain_status_fetcher_.get()
+ ->trajectory_logging()
+ ->available_splines())
+ ->Get(jj));
+ }
+ }
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
@@ -1507,7 +1504,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1544,7 +1541,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1589,7 +1586,7 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 5ce14e6..fe691ed 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -54,21 +54,21 @@
// For logging information about the state of the trajectory planning.
table TrajectoryLogging {
// state of planning the trajectory.
- planning_state:PlanningState (id: 0);
+ planning_state:PlanningState (id: 0, deprecated);
// State of the spline execution.
is_executing:bool (id: 1);
// Whether we have finished the spline specified by current_spline_idx.
is_executed:bool (id: 2);
- // The handle of the goal spline. 0 means stop requested.
+ // The handle of the goal spline. Empty means no goal/stop requested.
goal_spline_handle:int (id: 3);
- // Handle of the executing spline. -1 means none requested. If there was no
- // spline executing when a spline finished optimizing, it will become the
- // current spline even if we aren't ready to start yet.
+ // Handle of the executing spline. Will generally be identical to
+ // goal_spline_handle if the spline is available; however, if the commanded
+ // spline has not yet been planned, this will be empty.
current_spline_idx:int (id: 4);
// Handle of the spline that is being optimized and staged.
- planning_spline_idx:int (id: 5);
+ planning_spline_idx:int (id: 5, deprecated);
// Expected position and velocity on the spline
x:float (id: 6);
@@ -77,6 +77,9 @@
left_velocity:float (id: 9);
right_velocity:float (id: 10);
distance_remaining:float (id: 11);
+
+ // Splines that we have full plans for.
+ available_splines:[int] (id: 12);
}
// For logging state of the line follower.
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 6ea23c6..f275c17 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -121,8 +121,9 @@
Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
int num_distance) {
return new Trajectory(
- spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
- num_distance);
+ DistanceSpline(*spline),
+ ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), nullptr, -1,
+ vmax, num_distance);
}
void deleteTrajectory(Trajectory *t) { delete t; }
diff --git a/frc971/control_loops/drivetrain/spline_goal.fbs b/frc971/control_loops/drivetrain/spline_goal.fbs
new file mode 100644
index 0000000..3561a18
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_goal.fbs
@@ -0,0 +1,16 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+table SplineGoal {
+ // index of the spline.
+ spline_idx:int (id: 0);
+
+ // Acutal spline.
+ spline:frc971.MultiSpline (id: 1);
+
+ // Whether to follow the spline driving forwards or backwards.
+ drive_spline_backwards:bool (id: 2);
+}
+
+root_type SplineGoal;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 423465a..cdedce0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,6 +2,8 @@
#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"
@@ -15,9 +17,10 @@
namespace drivetrain {
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config), new_goal_(&mutex_) {
- worker_thread_ = std::thread(&SplineDrivetrain::ComputeTrajectory, this);
-}
+ : dt_config_(dt_config),
+ current_xva_(Eigen::Vector3d::Zero()),
+ next_xva_(Eigen::Vector3d::Zero()),
+ next_U_(Eigen::Vector2d::Zero()) {}
void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
output_was_capped_ =
@@ -28,151 +31,72 @@
}
}
-void SplineDrivetrain::ComputeTrajectory() {
- ::aos::SetCurrentThreadRealtimePriority(1);
-
- ::aos::MutexLocker locker(&mutex_);
- while (run_) {
- while (goal_.spline_idx == future_spline_idx_) {
- AOS_CHECK(!new_goal_.Wait());
- if (!run_) {
- return;
- }
- }
- past_distance_spline_.reset();
- past_trajectory_.reset();
-
- plan_state_ = PlanningState::BUILDING_TRAJECTORY;
- future_spline_idx_ = goal_.spline_idx;
- planning_spline_idx_ = future_spline_idx_;
- const MultiSpline &multispline = goal_.spline;
- auto x = multispline.spline_x;
- auto y = multispline.spline_y;
- ::std::vector<Spline> splines = ::std::vector<Spline>();
- for (int i = 0; i < multispline.spline_count; ++i) {
- ::Eigen::Matrix<double, 2, 6> points =
- ::Eigen::Matrix<double, 2, 6>::Zero();
- for (int j = 0; j < 6; ++j) {
- points(0, j) = x[i * 5 + j];
- points(1, j) = y[i * 5 + j];
- }
- splines.emplace_back(Spline(points));
- }
-
- future_drive_spline_backwards_ = goal_.drive_spline_backwards;
-
- future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
- new DistanceSpline(::std::move(splines)));
-
- future_trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(future_distance_spline_.get(), dt_config_));
-
- for (size_t i = 0; i < multispline.constraints.size(); ++i) {
- const ::frc971::ConstraintT &constraint = multispline.constraints[i];
- switch (constraint.constraint_type) {
- case frc971::ConstraintType::CONSTRAINT_TYPE_UNDEFINED:
- break;
- case frc971::ConstraintType::LONGITUDINAL_ACCELERATION:
- future_trajectory_->set_longitudinal_acceleration(constraint.value);
- break;
- case frc971::ConstraintType::LATERAL_ACCELERATION:
- future_trajectory_->set_lateral_acceleration(constraint.value);
- break;
- case frc971::ConstraintType::VOLTAGE:
- future_trajectory_->set_voltage_limit(constraint.value);
- break;
- case frc971::ConstraintType::VELOCITY:
- future_trajectory_->LimitVelocity(constraint.start_distance,
- constraint.end_distance,
- constraint.value);
- break;
- }
- }
- plan_state_ = PlanningState::PLANNING_TRAJECTORY;
-
- future_trajectory_->Plan();
- plan_state_ = PlanningState::PLANNED;
- }
-}
-
void SplineDrivetrain::SetGoal(
const ::frc971::control_loops::drivetrain::Goal *goal) {
- current_spline_handle_ = goal->spline_handle();
-
- // If told to stop, set the executing spline to an invalid index and clear out
- // its plan:
- if (current_spline_handle_ == 0 &&
- (goal->spline() == nullptr ||
- current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
- current_spline_idx_ = -1;
- }
-
- ::aos::Mutex::State mutex_state = mutex_.TryLock();
- if (mutex_state == ::aos::Mutex::State::kLocked) {
- if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
- future_spline_idx_ != goal->spline()->spline_idx()) {
- CHECK_NOTNULL(goal->spline());
- goal_.spline_idx = goal->spline()->spline_idx();
- goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
-
- const frc971::MultiSpline *multispline = goal->spline()->spline();
- CHECK_NOTNULL(multispline);
-
- goal_.spline.spline_count = multispline->spline_count();
-
- CHECK_EQ(multispline->spline_x()->size(),
- static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
- CHECK_EQ(multispline->spline_y()->size(),
- static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
-
- std::copy(multispline->spline_x()->begin(),
- multispline->spline_x()->end(), goal_.spline.spline_x.begin());
- std::copy(multispline->spline_y()->begin(),
- multispline->spline_y()->end(), goal_.spline.spline_y.begin());
-
- for (size_t i = 0; i < 6; ++i) {
- if (multispline->constraints() != nullptr &&
- i < multispline->constraints()->size()) {
- multispline->constraints()->Get(i)->UnPackTo(
- &goal_.spline.constraints[i]);
- } else {
- goal_.spline.constraints[i].constraint_type =
- ConstraintType::CONSTRAINT_TYPE_UNDEFINED;
- }
- }
-
- new_goal_.Broadcast();
- if (current_spline_handle_ != current_spline_idx_) {
- // If we aren't going to actively execute the current spline, evict it's
- // plan.
- past_trajectory_ = std::move(current_trajectory_);
- past_distance_spline_ = std::move(current_distance_spline_);
- }
- }
- // If you never started executing the previous spline, you're screwed.
- if (future_trajectory_ &&
- (!current_trajectory_ ||
- current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
- current_spline_idx_ == -1)) {
- // Move current data to other variables to be cleared by worker.
- past_trajectory_ = std::move(current_trajectory_);
- past_distance_spline_ = std::move(current_distance_spline_);
-
- // Move the computed data to be executed.
- current_trajectory_ = std::move(future_trajectory_);
- current_distance_spline_ = std::move(future_distance_spline_);
- current_drive_spline_backwards_ = future_drive_spline_backwards_;
- current_spline_idx_ = future_spline_idx_;
-
- // Reset internal state to a trajectory start position.
- current_xva_ = current_trajectory_->FFAcceleration(0);
- current_xva_(1) = 0.0;
- has_started_execution_ = false;
- }
- mutex_.Unlock();
+ if (goal->has_spline_handle()) {
+ commanded_spline_ = goal->spline_handle();
} else {
- VLOG(1) << "Failed to acquire trajectory lock.";
+ commanded_spline_.reset();
}
+ UpdateSplineHandles();
+}
+
+bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
+ if (trajectory == nullptr) {
+ return false;
+ }
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ return true;
+ }
+ }
+ return false;
+}
+
+void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
+ trajectories_.emplace_back(
+ std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
+ UpdateSplineHandles();
+}
+
+void SplineDrivetrain::DeleteCurrentSpline() {
+ CHECK(current_trajectory_index_);
+ CHECK_LT(*current_trajectory_index_, trajectories_.size());
+ trajectories_.erase(trajectories_.begin() + *current_trajectory_index_);
+ executing_spline_ = false;
+ current_trajectory_index_.reset();
+ current_xva_.setZero();
+}
+
+void SplineDrivetrain::UpdateSplineHandles() {
+ // If we are currently executing a spline and have received a change
+ if (executing_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_) {
+ // If we are executing a spline, and the handle has changed, garbage
+ // collect the old spline.
+ DeleteCurrentSpline();
+ }
+ }
+ }
+ // 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_) {
+ executing_spline_ = true;
+ current_trajectory_index_ = ii;
+ }
+ }
+ // 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.
}
// TODO(alex): Hold position when done following the spline.
@@ -181,20 +105,19 @@
const ::Eigen::Matrix<double, 2, 1> &voltage_error) {
next_U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
enable_ = enable;
- if (enable && current_trajectory_) {
+ if (enable && executing_spline_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!IsAtEnd() && current_spline_handle_ == current_spline_idx_) {
- has_started_execution_ = true;
+ 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 = current_trajectory().FFVoltage(current_xva_(0));
}
const double current_distance = current_xva_(0);
::Eigen::Matrix<double, 2, 5> K =
- current_trajectory_->GainForDistance(current_distance);
+ current_trajectory().GainForDistance(current_distance);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- if (current_drive_spline_backwards_) {
+ if (current_trajectory().drive_spline_backwards()) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
goal_state(2, 0) += M_PI;
@@ -204,16 +127,16 @@
goal_state(4, 0) = -left_goal;
}
const Eigen::Matrix<double, 5, 1> relative_goal =
- current_trajectory_->StateToPathRelativeState(current_distance,
+ current_trajectory().StateToPathRelativeState(current_distance,
goal_state);
const Eigen::Matrix<double, 5, 1> relative_state =
- current_trajectory_->StateToPathRelativeState(current_distance, state);
+ current_trajectory().StateToPathRelativeState(current_distance, state);
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;
::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_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
@@ -225,7 +148,7 @@
if (!output) {
return;
}
- if (current_spline_handle_ == current_spline_idx_) {
+ if (executing_spline_) {
if (!IsAtEnd()) {
output->left_voltage = next_U_(0);
output->right_voltage = next_U_(1);
@@ -248,33 +171,39 @@
flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const {
+ int *spline_handles;
+ const flatbuffers::Offset<flatbuffers::Vector<int>> handles_vector =
+ builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
+
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ spline_handles[ii] = trajectories_[ii]->spline_handle();
+ }
+
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
- if (current_distance_spline_) {
+ if (executing_spline_) {
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
trajectory_logging_builder.add_x(goal_state(0));
trajectory_logging_builder.add_y(goal_state(1));
trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
- goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+ goal_state(2) +
+ (current_trajectory().drive_spline_backwards() ? M_PI : 0.0)));
trajectory_logging_builder.add_left_velocity(goal_state(3));
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
- trajectory_logging_builder.add_planning_state(plan_state_.load());
trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
- has_started_execution_);
- trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
- IsAtEnd());
- trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
- trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
- trajectory_logging_builder.add_distance_remaining(
- current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
- : 0.0);
-
- int32_t planning_spline_idx = planning_spline_idx_;
- if (current_spline_idx_ == planning_spline_idx) {
- trajectory_logging_builder.add_planning_spline_idx(0);
- } else {
- trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+ executing_spline_);
+ trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
+ if (commanded_spline_) {
+ trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
+ if (executing_spline_) {
+ trajectory_logging_builder.add_current_spline_idx(*commanded_spline_);
+ }
}
+ trajectory_logging_builder.add_distance_remaining(
+ executing_spline_ ? 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 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
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 9c32483..08dd3fd 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -14,14 +14,73 @@
namespace control_loops {
namespace drivetrain {
-Trajectory::Trajectory(const DistanceSpline *spline,
- const DrivetrainConfig<double> &config, double vmax,
- int num_distance)
- : spline_(spline),
- velocity_drivetrain_(
- ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>(
+namespace {
+float DefaultConstraint(ConstraintType type) {
+ switch (type) {
+ case ConstraintType::LONGITUDINAL_ACCELERATION:
+ return 2.0;
+ case ConstraintType::LATERAL_ACCELERATION:
+ return 3.0;
+ case ConstraintType::VOLTAGE:
+ return 12.0;
+ case ConstraintType::VELOCITY:
+ case ConstraintType::CONSTRAINT_TYPE_UNDEFINED:
+ LOG(FATAL) << "No default constraint value for "
+ << EnumNameConstraintType(type);
+ }
+ LOG(FATAL) << "Invalid ConstraintType " << static_cast<int>(type);
+}
+} // namespace
+
+FinishedTrajectory::FinishedTrajectory(const DrivetrainConfig<double> &config,
+ const fb::Trajectory *buffer)
+ : BaseTrajectory(CHECK_NOTNULL(CHECK_NOTNULL(buffer->spline())->spline())
+ ->constraints(),
+ config),
+ buffer_(buffer),
+ spline_(*buffer_->spline()) {}
+
+const Eigen::Matrix<double, 2, 1> BaseTrajectory::K1(
+ double current_ddtheta) const {
+ return (Eigen::Matrix<double, 2, 1>() << -robot_radius_l_ * current_ddtheta,
+ robot_radius_r_ * current_ddtheta)
+ .finished();
+}
+
+const Eigen::Matrix<double, 2, 1> BaseTrajectory::K2(
+ double current_dtheta) const {
+ return (Eigen::Matrix<double, 2, 1>()
+ << 1.0 - robot_radius_l_ * current_dtheta,
+ 1.0 + robot_radius_r_ * current_dtheta)
+ .finished();
+}
+
+void BaseTrajectory::K345(const double x, Eigen::Matrix<double, 2, 1> *K3,
+ Eigen::Matrix<double, 2, 1> *K4,
+ Eigen::Matrix<double, 2, 1> *K5) const {
+ const double current_ddtheta = spline().DDTheta(x);
+ const double current_dtheta = spline().DTheta(x);
+ // We've now got the equation:
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
+
+ const Eigen::Matrix<double, 2, 2> B_inverse =
+ velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
+
+ // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
+ *K3 = B_inverse * K1(current_ddtheta);
+ *K4 = -B_inverse * velocity_drivetrain_->plant().coefficients().A_continuous *
+ my_K2;
+ *K5 = B_inverse * my_K2;
+}
+
+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>>(
@@ -29,18 +88,43 @@
config_(config),
robot_radius_l_(config.robot_radius),
robot_radius_r_(config.robot_radius),
- longitudinal_acceleration_(3.0),
- lateral_acceleration_(2.0),
- Tlr_to_la_((::Eigen::Matrix<double, 2, 2>() << 0.5, 0.5,
- -1.0 / (robot_radius_l_ + robot_radius_r_),
- 1.0 / (robot_radius_l_ + robot_radius_r_))
- .finished()),
- Tla_to_lr_(Tlr_to_la_.inverse()),
+ lateral_acceleration_(
+ ConstraintValue(constraints, ConstraintType::LATERAL_ACCELERATION)),
+ longitudinal_acceleration_(ConstraintValue(
+ constraints, ConstraintType::LONGITUDINAL_ACCELERATION)),
+ voltage_limit_(ConstraintValue(constraints, ConstraintType::VOLTAGE)) {}
+
+Trajectory::Trajectory(const SplineGoal &spline_goal,
+ const DrivetrainConfig<double> &config)
+ : Trajectory(DistanceSpline{spline_goal.spline()}, config,
+ spline_goal.spline()->constraints(),
+ spline_goal.spline_idx()) {
+ drive_spline_backwards_ = spline_goal.drive_spline_backwards();
+}
+
+Trajectory::Trajectory(
+ 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),
+ spline_idx_(spline_idx),
+ spline_(std::move(input_spline)),
+ config_(config),
plan_(num_distance == 0
- ? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
+ ? std::max(100, static_cast<int>(spline_.length() / 0.0025))
: num_distance,
vmax),
- plan_segment_type_(plan_.size(), SegmentType::VELOCITY_LIMITED) {}
+ plan_segment_type_(plan_.size(),
+ fb::SegmentConstraint::VELOCITY_LIMITED) {
+ if (constraints != nullptr) {
+ for (const Constraint *constraint : *constraints) {
+ if (constraint->constraint_type() == ConstraintType::VELOCITY) {
+ LimitVelocity(constraint->start_distance(), constraint->end_distance(),
+ constraint->value());
+ }
+ }
+ }
+}
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
@@ -48,7 +132,7 @@
const double velocity_limit = LateralVelocityCurvature(distance);
if (velocity_limit < plan_[i]) {
plan_[i] = velocity_limit;
- plan_segment_type_[i] = CURVATURE_LIMITED;
+ plan_segment_type_[i] = fb::SegmentConstraint::CURVATURE_LIMITED;
}
}
}
@@ -59,29 +143,30 @@
const double velocity_limit = VoltageVelocityLimit(distance, limit_type);
if (velocity_limit < plan_[i]) {
plan_[i] = velocity_limit;
- plan_segment_type_[i] = VOLTAGE_LIMITED;
+ plan_segment_type_[i] = fb::SegmentConstraint::VOLTAGE_LIMITED;
}
}
}
-double Trajectory::BestAcceleration(double x, double v, bool backwards) {
- ::Eigen::Matrix<double, 2, 1> K3;
- ::Eigen::Matrix<double, 2, 1> K4;
- ::Eigen::Matrix<double, 2, 1> K5;
+double BaseTrajectory::BestAcceleration(double x, double v,
+ bool backwards) const {
+ Eigen::Matrix<double, 2, 1> K3;
+ Eigen::Matrix<double, 2, 1> K4;
+ Eigen::Matrix<double, 2, 1> K5;
K345(x, &K3, &K4, &K5);
// Now, solve for all a's and find the best one which meets our criteria.
- const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
- double min_voltage_accel = ::std::numeric_limits<double>::infinity();
+ const Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+ double min_voltage_accel = std::numeric_limits<double>::infinity();
double max_voltage_accel = -min_voltage_accel;
- for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
- (voltage_limit_ - C(1, 0)) / K5(1, 0),
- (-voltage_limit_ - C(0, 0)) / K5(0, 0),
- (-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
- const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
- if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
- min_voltage_accel = ::std::min(a, min_voltage_accel);
- max_voltage_accel = ::std::max(a, max_voltage_accel);
+ for (const double a : {(max_voltage() - C(0, 0)) / K5(0, 0),
+ (max_voltage() - C(1, 0)) / K5(1, 0),
+ (-max_voltage() - C(0, 0)) / K5(0, 0),
+ (-max_voltage() - C(1, 0)) / K5(1, 0)}) {
+ const Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
+ if ((U.array().abs() < max_voltage() + 1e-6).all()) {
+ min_voltage_accel = std::min(a, min_voltage_accel);
+ max_voltage_accel = std::max(a, max_voltage_accel);
}
}
double best_accel = backwards ? min_voltage_accel : max_voltage_accel;
@@ -89,9 +174,9 @@
double min_friction_accel, max_friction_accel;
FrictionLngAccelLimits(x, v, &min_friction_accel, &max_friction_accel);
if (backwards) {
- best_accel = ::std::max(best_accel, min_friction_accel);
+ best_accel = std::max(best_accel, min_friction_accel);
} else {
- best_accel = ::std::min(best_accel, max_friction_accel);
+ best_accel = std::min(best_accel, max_friction_accel);
}
// Ideally, the max would never be less than the min, but due to the way that
@@ -117,7 +202,7 @@
return best_accel;
}
-double Trajectory::LateralVelocityCurvature(double distance) const {
+double BaseTrajectory::LateralVelocityCurvature(double distance) const {
// To calculate these constraints, we first note that:
// wheel accels = K2 * v_robot' + K1 * v_robot^2
// All that this logic does is solve for v_robot, leaving v_robot' free,
@@ -139,37 +224,37 @@
// 3) Solving the relatively tractable remaining equation, which is
// basically just grouping all the terms together in one spot and taking the
// 4th root of everything.
- const double dtheta = spline_->DTheta(distance);
- const ::Eigen::Matrix<double, 1, 2> K2prime =
+ const double dtheta = spline().DTheta(distance);
+ const Eigen::Matrix<double, 1, 2> K2prime =
K2(dtheta).transpose() *
- (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+ (Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
// Calculate whether the wheels are spinning in opposite directions.
const bool opposites = K2prime(0) * K2prime(1) < 0;
- const ::Eigen::Matrix<double, 2, 1> K1calc = K1(spline_->DDTheta(distance));
- const double lat_accel_squared =
- ::std::pow(dtheta / lateral_acceleration_, 2);
+ const Eigen::Matrix<double, 2, 1> K1calc = K1(spline().DDTheta(distance));
+ const double lat_accel_squared = std::pow(dtheta / max_lateral_accel(), 2);
const double curvature_change_term =
(K2prime * K1calc).value() /
(K2prime *
- (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
+ (Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
.finished() *
- longitudinal_acceleration_)
+ max_longitudinal_accel())
.value();
- const double vel_inv = ::std::sqrt(
- ::std::sqrt(::std::pow(curvature_change_term, 2) + lat_accel_squared));
+ const double vel_inv = std::sqrt(
+ std::sqrt(std::pow(curvature_change_term, 2) + lat_accel_squared));
if (vel_inv == 0.0) {
- return ::std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::infinity();
}
return 1.0 / vel_inv;
}
-void Trajectory::FrictionLngAccelLimits(double x, double v, double *min_accel,
- double *max_accel) const {
+void BaseTrajectory::FrictionLngAccelLimits(double x, double v,
+ double *min_accel,
+ double *max_accel) const {
// First, calculate the max longitudinal acceleration that can be achieved
// by either wheel given the friction elliipse that we have.
- const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+ const double lateral_acceleration = v * v * spline().DDXY(x).norm();
const double max_wheel_lng_accel_squared =
- 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+ 1.0 - std::pow(lateral_acceleration / max_lateral_accel(), 2.0);
if (max_wheel_lng_accel_squared < 0.0) {
VLOG(1) << "Something (probably Runge-Kutta) queried invalid velocity " << v
<< " at distance " << x;
@@ -179,45 +264,45 @@
// how min_accel > max_accel if we reach this case) to avoid causing any
// numerical issues.
*min_accel =
- ::std::sqrt(-max_wheel_lng_accel_squared) * longitudinal_acceleration_;
+ std::sqrt(-max_wheel_lng_accel_squared) * max_longitudinal_accel();
*max_accel = -*min_accel;
return;
}
- *min_accel = -::std::numeric_limits<double>::infinity();
- *max_accel = ::std::numeric_limits<double>::infinity();
+ *min_accel = -std::numeric_limits<double>::infinity();
+ *max_accel = std::numeric_limits<double>::infinity();
// Calculate max/min accelerations by calculating what the robots overall
// longitudinal acceleration would be if each wheel were running at the max
// forwards/backwards longitudinal acceleration.
const double max_wheel_lng_accel =
- longitudinal_acceleration_ * ::std::sqrt(max_wheel_lng_accel_squared);
- const ::Eigen::Matrix<double, 2, 1> K1v2 = K1(spline_->DDTheta(x)) * v * v;
- const ::Eigen::Matrix<double, 2, 1> K2inv =
- K2(spline_->DTheta(x)).cwiseInverse();
+ max_longitudinal_accel() * std::sqrt(max_wheel_lng_accel_squared);
+ const Eigen::Matrix<double, 2, 1> K1v2 = K1(spline().DDTheta(x)) * v * v;
+ const Eigen::Matrix<double, 2, 1> K2inv =
+ K2(spline().DTheta(x)).cwiseInverse();
// Store the accelerations of the robot corresponding to each wheel being at
// the max/min acceleration. The first coefficient in each vector
// corresponds to the left wheel, the second to the right wheel.
- const ::Eigen::Matrix<double, 2, 1> accels1 =
+ const Eigen::Matrix<double, 2, 1> accels1 =
K2inv.array() * (-K1v2.array() + max_wheel_lng_accel);
- const ::Eigen::Matrix<double, 2, 1> accels2 =
+ const Eigen::Matrix<double, 2, 1> accels2 =
K2inv.array() * (-K1v2.array() - max_wheel_lng_accel);
// If either term is non-finite, that suggests that a term of K2 is zero
// (which is physically possible when turning such that one wheel is
// stationary), so just ignore that side of the drivetrain.
- if (::std::isfinite(accels1(0))) {
+ if (std::isfinite(accels1(0))) {
// The inner max/min in this case determines which of the two cases (+ or
// - acceleration on the left wheel) we care about--in a sufficiently
// tight turning radius, the left hweel may be accelerating backwards when
// the robot as a whole accelerates forwards. We then use that
// acceleration to bound the min/max accel.
- *min_accel = ::std::max(*min_accel, ::std::min(accels1(0), accels2(0)));
- *max_accel = ::std::min(*max_accel, ::std::max(accels1(0), accels2(0)));
+ *min_accel = std::max(*min_accel, std::min(accels1(0), accels2(0)));
+ *max_accel = std::min(*max_accel, std::max(accels1(0), accels2(0)));
}
// Same logic as previous if-statement, but for the right wheel.
- if (::std::isfinite(accels1(1))) {
- *min_accel = ::std::max(*min_accel, ::std::min(accels1(1), accels2(1)));
- *max_accel = ::std::min(*max_accel, ::std::max(accels1(1), accels2(1)));
+ if (std::isfinite(accels1(1))) {
+ *min_accel = std::max(*min_accel, std::min(accels1(1), accels2(1)));
+ *max_accel = std::min(*max_accel, std::max(accels1(1), accels2(1)));
}
}
@@ -254,28 +339,28 @@
// K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot +/- B * U_max
// K2prime * K1 * v_robot^2 = K2prime * (A * K2 * v_robot +/- B * U_max)
// a v_robot^2 + b v_robot +/- c = 0
- const ::Eigen::Matrix<double, 2, 2> B =
- velocity_drivetrain_->plant().coefficients().B_continuous;
- const double dtheta = spline_->DTheta(distance);
- const ::Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
+ const Eigen::Matrix<double, 2, 2> B =
+ velocity_drivetrain().plant().coefficients().B_continuous;
+ const double dtheta = spline().DTheta(distance);
+ const Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
// Because voltages can actually impact *both* wheels, in order to determine
// whether the voltages will have opposite signs, we need to use B^-1 * K2.
const bool opposite_voltages = BinvK2(0) * BinvK2(1) > 0.0;
- const ::Eigen::Matrix<double, 1, 2> K2prime =
+ const Eigen::Matrix<double, 1, 2> K2prime =
K2(dtheta).transpose() *
- (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
- const double a = K2prime * K1(spline_->DDTheta(distance));
+ (Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+ const double a = K2prime * K1(spline().DDTheta(distance));
const double b = -K2prime *
- velocity_drivetrain_->plant().coefficients().A_continuous *
+ velocity_drivetrain().plant().coefficients().A_continuous *
K2(dtheta);
- const ::Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
+ const Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
// Calculate the "positive" version of the voltage limits we will use.
- const ::Eigen::Matrix<double, 2, 1> abs_volts =
- voltage_limit_ *
- (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
+ const Eigen::Matrix<double, 2, 1> abs_volts =
+ max_voltage() *
+ (Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
.finished();
- double min_valid_vel = ::std::numeric_limits<double>::infinity();
+ double min_valid_vel = std::numeric_limits<double>::infinity();
if (limit_type == VoltageLimit::kAggressive) {
min_valid_vel = 0.0;
}
@@ -283,7 +368,7 @@
// formula. For every positive solution, adjust the velocity limit
// appropriately.
for (const double sign : {1.0, -1.0}) {
- const ::Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
+ const Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
const double prev_vel = min_valid_vel;
const double c = c_coeff * U;
const double determinant = b * b - 4 * a * c;
@@ -295,40 +380,40 @@
if (-b * c > 0.0) {
const double vel = -c / b;
if (limit_type == VoltageLimit::kConservative) {
- min_valid_vel = ::std::min(min_valid_vel, vel);
+ min_valid_vel = std::min(min_valid_vel, vel);
} else {
- min_valid_vel = ::std::max(min_valid_vel, vel);
+ min_valid_vel = std::max(min_valid_vel, vel);
}
} else if (b == 0) {
// If a and b are zero, then we are travelling in a straight line and
// have no voltage-based velocity constraints.
- min_valid_vel = ::std::numeric_limits<double>::infinity();
+ min_valid_vel = std::numeric_limits<double>::infinity();
}
} else if (determinant > 0) {
- const double sqrt_determinant = ::std::sqrt(determinant);
+ const double sqrt_determinant = std::sqrt(determinant);
const double high_vel = (-b + sqrt_determinant) / (2.0 * a);
const double low_vel = (-b - sqrt_determinant) / (2.0 * a);
if (low_vel > 0) {
if (limit_type == VoltageLimit::kConservative) {
- min_valid_vel = ::std::min(min_valid_vel, low_vel);
+ min_valid_vel = std::min(min_valid_vel, low_vel);
} else {
- min_valid_vel = ::std::max(min_valid_vel, low_vel);
+ min_valid_vel = std::max(min_valid_vel, low_vel);
}
}
if (high_vel > 0) {
if (limit_type == VoltageLimit::kConservative) {
- min_valid_vel = ::std::min(min_valid_vel, high_vel);
+ min_valid_vel = std::min(min_valid_vel, high_vel);
} else {
- min_valid_vel = ::std::max(min_valid_vel, high_vel);
+ min_valid_vel = std::max(min_valid_vel, high_vel);
}
}
} else if (determinant == 0 && -b * a > 0) {
const double vel = -b / (2.0 * a);
if (vel > 0.0) {
if (limit_type == VoltageLimit::kConservative) {
- min_valid_vel = ::std::min(min_valid_vel, vel);
+ min_valid_vel = std::min(min_valid_vel, vel);
} else {
- min_valid_vel = ::std::max(min_valid_vel, vel);
+ min_valid_vel = std::max(min_valid_vel, vel);
}
}
}
@@ -352,7 +437,7 @@
if (new_plan_velocity <= plan_[i + 1]) {
plan_[i + 1] = new_plan_velocity;
- plan_segment_type_[i] = SegmentType::ACCELERATION_LIMITED;
+ plan_segment_type_[i] = fb::SegmentConstraint::ACCELERATION_LIMITED;
}
}
}
@@ -370,12 +455,13 @@
if (new_plan_velocity <= plan_[i - 1]) {
plan_[i - 1] = new_plan_velocity;
- plan_segment_type_[i - 1] = SegmentType::DECELERATION_LIMITED;
+ plan_segment_type_[i - 1] = fb::SegmentConstraint::DECELERATION_LIMITED;
}
}
}
-::Eigen::Matrix<double, 3, 1> Trajectory::FFAcceleration(double distance) {
+Eigen::Matrix<double, 3, 1> BaseTrajectory::FFAcceleration(
+ double distance) const {
if (distance < 0.0) {
// Make sure we don't end up off the beginning of the curve.
distance = 0.0;
@@ -384,7 +470,8 @@
distance = length();
}
const size_t before_index = DistanceToSegment(distance);
- const size_t after_index = std::min(before_index + 1, plan_.size() - 1);
+ const size_t after_index =
+ std::min(before_index + 1, distance_plan_size() - 1);
const double before_distance = Distance(before_index);
const double after_distance = Distance(after_index);
@@ -397,67 +484,85 @@
// TODO(james): While technically correct for sufficiently small segment
// steps, this method of switching between limits has a tendency to produce
// sudden jumps in acceelrations, which is undesirable.
- switch (plan_segment_type_[DistanceToSegment(distance)]) {
- case SegmentType::VELOCITY_LIMITED:
+ switch (plan_constraint(DistanceToSegment(distance))) {
+ case fb::SegmentConstraint::VELOCITY_LIMITED:
acceleration = 0.0;
- velocity = (plan_[before_index] + plan_[after_index]) / 2.0;
+ velocity =
+ (plan_velocity(before_index) + plan_velocity(after_index)) / 2.0;
// TODO(austin): Accelerate or decelerate until we hit the limit in the
// time slice. Otherwise our acceleration will be lying for this slice.
// Do note, we've got small slices so the effect will be small.
break;
- case SegmentType::CURVATURE_LIMITED:
+ case fb::SegmentConstraint::CURVATURE_LIMITED:
velocity = vcurvature;
FrictionLngAccelLimits(distance, velocity, &acceleration, &acceleration);
break;
- case SegmentType::VOLTAGE_LIMITED:
+ case fb::SegmentConstraint::VOLTAGE_LIMITED:
// Normally, we expect that voltage limited plans will all get dominated
// by the acceleration/deceleration limits. This may not always be true;
// if we ever encounter this error, we just need to back out what the
// accelerations would be in this case.
LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
break;
- case SegmentType::ACCELERATION_LIMITED:
+ case fb::SegmentConstraint::ACCELERATION_LIMITED:
// TODO(james): The integration done here and in the DECELERATION_LIMITED
// can technically cause us to violate friction constraints. We currently
// don't do anything about it to avoid causing sudden jumps in voltage,
// but we probably *should* at some point.
velocity = IntegrateAccelForDistance(
[this](double x, double v) { return ForwardAcceleration(x, v); },
- plan_[before_index], before_distance, distance - before_distance);
+ plan_velocity(before_index), before_distance,
+ distance - before_distance);
acceleration = ForwardAcceleration(distance, velocity);
break;
- case SegmentType::DECELERATION_LIMITED:
+ case fb::SegmentConstraint::DECELERATION_LIMITED:
velocity = IntegrateAccelForDistance(
[this](double x, double v) { return BackwardAcceleration(x, v); },
- plan_[after_index], after_distance, distance - after_distance);
+ plan_velocity(after_index), after_distance,
+ distance - after_distance);
acceleration = BackwardAcceleration(distance, velocity);
break;
default:
- AOS_LOG(
- FATAL, "Unknown segment type %d\n",
- static_cast<int>(plan_segment_type_[DistanceToSegment(distance)]));
+ AOS_LOG(FATAL, "Unknown segment type %d\n",
+ static_cast<int>(plan_constraint(DistanceToSegment(distance))));
break;
}
- return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
+ return (Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
.finished();
}
-::Eigen::Matrix<double, 2, 1> Trajectory::FFVoltage(double distance) {
+size_t FinishedTrajectory::distance_plan_size() const {
+ return trajectory().has_distance_based_plan()
+ ? trajectory().distance_based_plan()->size()
+ : 0u;
+}
+
+fb::SegmentConstraint FinishedTrajectory::plan_constraint(size_t index) const {
+ CHECK_LT(index, distance_plan_size());
+ return trajectory().distance_based_plan()->Get(index)->segment_constraint();
+}
+
+float FinishedTrajectory::plan_velocity(size_t index) const {
+ CHECK_LT(index, distance_plan_size());
+ return trajectory().distance_based_plan()->Get(index)->velocity();
+}
+
+Eigen::Matrix<double, 2, 1> BaseTrajectory::FFVoltage(double distance) const {
const Eigen::Matrix<double, 3, 1> xva = FFAcceleration(distance);
const double velocity = xva(1);
const double acceleration = xva(2);
- ::Eigen::Matrix<double, 2, 1> K3;
- ::Eigen::Matrix<double, 2, 1> K4;
- ::Eigen::Matrix<double, 2, 1> K5;
+ Eigen::Matrix<double, 2, 1> K3;
+ Eigen::Matrix<double, 2, 1> K4;
+ Eigen::Matrix<double, 2, 1> K5;
K345(distance, &K3, &K4, &K5);
return K5 * acceleration + K3 * velocity * velocity + K4 * velocity;
}
-const ::std::vector<double> Trajectory::Distances() const {
- ::std::vector<double> d;
+const std::vector<double> Trajectory::Distances() const {
+ std::vector<double> d;
d.reserve(plan_.size());
for (size_t i = 0; i < plan_.size(); ++i) {
d.push_back(Distance(i));
@@ -465,114 +570,17 @@
return d;
}
-::Eigen::Matrix<double, 5, 5> Trajectory::ALinearizedContinuous(
- const ::Eigen::Matrix<double, 5, 1> &state) const {
- const double sintheta = ::std::sin(state(2));
- const double costheta = ::std::cos(state(2));
- const ::Eigen::Matrix<double, 2, 1> linear_angular =
- Tlr_to_la_ * state.block<2, 1>(3, 0);
-
- // When stopped, just roll with a min velocity.
- double linear_velocity = 0.0;
- constexpr double kMinVelocity = 0.1;
- if (::std::abs(linear_angular(0)) < kMinVelocity / 100.0) {
- linear_velocity = 0.1;
- } else if (::std::abs(linear_angular(0)) > kMinVelocity) {
- linear_velocity = linear_angular(0);
- } else if (linear_angular(0) > 0) {
- linear_velocity = kMinVelocity;
- } else if (linear_angular(0) < 0) {
- linear_velocity = -kMinVelocity;
- }
-
- ::Eigen::Matrix<double, 5, 5> result = ::Eigen::Matrix<double, 5, 5>::Zero();
- result(0, 2) = -sintheta * linear_velocity;
- result(0, 3) = 0.5 * costheta;
- result(0, 4) = 0.5 * costheta;
-
- result(1, 2) = costheta * linear_velocity;
- result(1, 3) = 0.5 * sintheta;
- result(1, 4) = 0.5 * sintheta;
-
- result(2, 3) = Tlr_to_la_(1, 0);
- result(2, 4) = Tlr_to_la_(1, 1);
-
- result.block<2, 2>(3, 3) =
- velocity_drivetrain_->plant().coefficients().A_continuous;
- return result;
-}
-
-::Eigen::Matrix<double, 5, 2> Trajectory::BLinearizedContinuous() const {
- ::Eigen::Matrix<double, 5, 2> result = ::Eigen::Matrix<double, 5, 2>::Zero();
- result.block<2, 2>(3, 0) =
- velocity_drivetrain_->plant().coefficients().B_continuous;
- return result;
-}
-
-void Trajectory::AB(const ::Eigen::Matrix<double, 5, 1> &state,
- ::std::chrono::nanoseconds dt,
- ::Eigen::Matrix<double, 5, 5> *A,
- ::Eigen::Matrix<double, 5, 2> *B) const {
- ::Eigen::Matrix<double, 5, 5> A_linearized_continuous =
- ALinearizedContinuous(state);
- ::Eigen::Matrix<double, 5, 2> B_linearized_continuous =
- BLinearizedContinuous();
-
- // Now, convert it to discrete.
- controls::C2D(A_linearized_continuous, B_linearized_continuous, dt, A, B);
-}
-
-::Eigen::Matrix<double, 2, 5> Trajectory::KForState(
- const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
- const ::Eigen::DiagonalMatrix<double, 5> &Q,
- const ::Eigen::DiagonalMatrix<double, 2> &R) const {
- ::Eigen::Matrix<double, 5, 5> A;
- ::Eigen::Matrix<double, 5, 2> B;
- AB(state, dt, &A, &B);
-
- ::Eigen::Matrix<double, 5, 5> S = ::Eigen::Matrix<double, 5, 5>::Zero();
- ::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
-
- int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
- if (info != 0) {
- AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
- controls::Controllability(A, B));
- // TODO(austin): Can we be more clever here? Use the last one? We should
- // collect more info about when this breaks down from logs.
- K = ::Eigen::Matrix<double, 2, 5>::Zero();
- }
- if (VLOG_IS_ON(1)) {
- ::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
- const auto eigenvalues = eigensolver.eigenvalues();
- LOG(INFO) << "Eigenvalues: " << eigenvalues;
- }
- return K;
-}
-
-const ::Eigen::Matrix<double, 5, 1> Trajectory::GoalState(double distance,
- double velocity) {
- ::Eigen::Matrix<double, 5, 1> result;
- result.block<2, 1>(0, 0) = spline_->XY(distance);
- result(2, 0) = spline_->Theta(distance);
-
- result.block<2, 1>(3, 0) =
- Tla_to_lr_ * (::Eigen::Matrix<double, 2, 1>() << velocity,
- spline_->DThetaDt(distance, velocity))
- .finished();
- return result;
-}
-
-::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
- ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
+Eigen::Matrix<double, 3, 1> BaseTrajectory::GetNextXVA(
+ std::chrono::nanoseconds dt, Eigen::Matrix<double, 2, 1> *state) const {
double dt_float = ::aos::time::DurationInSeconds(dt);
const double last_distance = (*state)(0);
// TODO(austin): This feels like something that should be pulled out into
// a library for re-use.
*state = RungeKutta(
- [this](const ::Eigen::Matrix<double, 2, 1> x) {
- ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
- return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+ [this](const Eigen::Matrix<double, 2, 1> x) {
+ Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+ return (Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
},
*state, dt_float);
// Force the distance to move forwards, to guarantee that we actually finish
@@ -582,15 +590,15 @@
(*state)(0) = last_distance + kMinDistanceIncrease;
}
- ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+ Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
(*state)(1) = result(1);
return result;
}
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
- ::std::chrono::nanoseconds dt) {
- ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
- ::std::vector<::Eigen::Matrix<double, 3, 1>> result;
+std::vector<Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+ std::chrono::nanoseconds dt) {
+ Eigen::Matrix<double, 2, 1> state = Eigen::Matrix<double, 2, 1>::Zero();
+ std::vector<Eigen::Matrix<double, 3, 1>> result;
result.emplace_back(FFAcceleration(0));
result.back()(1) = 0.0;
@@ -616,11 +624,11 @@
AOS_LOG(FATAL, "End before start: %f > %f\n", starting_distance,
ending_distance);
}
- starting_distance = ::std::min(length(), ::std::max(0.0, starting_distance));
- ending_distance = ::std::min(length(), ::std::max(0.0, ending_distance));
+ starting_distance = std::min(length(), std::max(0.0, starting_distance));
+ ending_distance = std::min(length(), std::max(0.0, ending_distance));
if (segment_length < min_length) {
const size_t plan_index = static_cast<size_t>(
- ::std::round((starting_distance + ending_distance) / 2.0 / min_length));
+ std::round((starting_distance + ending_distance) / 2.0 / min_length));
if (max_velocity < plan_[plan_index]) {
plan_[plan_index] = max_velocity;
}
@@ -630,7 +638,7 @@
if (max_velocity < plan_[i]) {
plan_[i] = max_velocity;
if (i < DistanceToSegment(ending_distance)) {
- plan_segment_type_[i] = SegmentType::VELOCITY_LIMITED;
+ plan_segment_type_[i] = fb::SegmentConstraint::VELOCITY_LIMITED;
}
}
}
@@ -641,7 +649,7 @@
Eigen::Matrix<double, 5, 5> *A,
Eigen::Matrix<double, 5, 2> *B) {
const double nominal_velocity = FFAcceleration(distance)(1);
- const double dtheta_dt = spline_->DThetaDt(distance, nominal_velocity);
+ const double dtheta_dt = spline().DThetaDt(distance, nominal_velocity);
// Calculate the "path-relative" coordinates, which are:
// [[distance along the path],
// [lateral position along path],
@@ -650,8 +658,8 @@
// [right wheel velocity]]
Eigen::Matrix<double, 5, 1> nominal_X;
nominal_X << distance, 0.0, 0.0,
- nominal_velocity - dtheta_dt * robot_radius_l_,
- nominal_velocity + dtheta_dt * robot_radius_r_;
+ nominal_velocity - dtheta_dt * robot_radius_l(),
+ nominal_velocity + dtheta_dt * robot_radius_r();
PathRelativeContinuousSystem(nominal_X, A, B);
}
@@ -663,9 +671,9 @@
const double theta = X(2);
const double ctheta = std::cos(theta);
const double stheta = std::sin(theta);
- const double curvature = spline_->DTheta(X(0));
+ const double curvature = spline().DTheta(X(0));
const double longitudinal_velocity = (X(3) + X(4)) / 2.0;
- const double diameter = robot_radius_l_ + robot_radius_r_;
+ const double diameter = robot_radius_l() + robot_radius_r();
// d_dpath / dt = (v_left + v_right) / 2.0 * cos(theta)
// (d_dpath / dt) / dv_left = cos(theta) / 2.0
(*A)(0, 3) = ctheta / 2.0;
@@ -688,24 +696,24 @@
(*A)(2, 4) = 1.0 / diameter - curvature / 2.0;
// v_{left,right} / dt = the normal LTI system.
A->block<2, 2>(3, 3) =
- velocity_drivetrain_->plant().coefficients().A_continuous;
+ velocity_drivetrain().plant().coefficients().A_continuous;
B->block<2, 2>(3, 0) =
- velocity_drivetrain_->plant().coefficients().B_continuous;
+ velocity_drivetrain().plant().coefficients().B_continuous;
}
double Trajectory::EstimateDistanceAlongPath(
double nominal_distance, const Eigen::Matrix<double, 5, 1> &state) {
- const double nominal_theta = spline_->Theta(nominal_distance);
+ const double nominal_theta = spline().Theta(nominal_distance);
const Eigen::Matrix<double, 2, 1> xy_err =
- state.block<2, 1>(0, 0) - spline_->XY(nominal_distance);
+ state.block<2, 1>(0, 0) - spline().XY(nominal_distance);
return nominal_distance + xy_err.x() * std::cos(nominal_theta) +
xy_err.y() * std::sin(nominal_theta);
}
-Eigen::Matrix<double, 5, 1> Trajectory::StateToPathRelativeState(
- double distance, const Eigen::Matrix<double, 5, 1> &state) {
- const double nominal_theta = spline_->Theta(distance);
- const Eigen::Matrix<double, 2, 1> nominal_xy = spline_->XY(distance);
+Eigen::Matrix<double, 5, 1> FinishedTrajectory::StateToPathRelativeState(
+ double distance, const Eigen::Matrix<double, 5, 1> &state) const {
+ const double nominal_theta = spline().Theta(distance);
+ const Eigen::Matrix<double, 2, 1> nominal_xy = spline().XY(distance);
const Eigen::Matrix<double, 2, 1> xy_err =
state.block<2, 1>(0, 0) - nominal_xy;
const double ctheta = std::cos(nominal_theta);
@@ -792,20 +800,99 @@
const Eigen::Matrix<double, 2, 2> RBPBinv =
(R + B_discrete.transpose() * P * B_discrete).inverse();
plan_gains_[i].first = distance;
- plan_gains_[i].second = RBPBinv * APB.transpose();
- P = AP * A_discrete - APB * plan_gains_[i].second + Q;
+ const Eigen::Matrix<double, 2, 5> K = RBPBinv * APB.transpose();
+ plan_gains_[i].second = K.cast<float>();
+ P = AP * A_discrete - APB * K + Q;
}
}
-Eigen::Matrix<double, 2, 5> Trajectory::GainForDistance(double distance) {
- CHECK(!plan_gains_.empty());
+Eigen::Matrix<double, 2, 5> FinishedTrajectory::GainForDistance(
+ double distance) const {
+ const flatbuffers::Vector<flatbuffers::Offset<fb::GainPoint>> &gains =
+ *CHECK_NOTNULL(trajectory().gains());
+ CHECK_LT(0u, gains.size());
size_t index = 0;
- for (index = 0; index < plan_gains_.size() - 1; ++index) {
- if (plan_gains_[index + 1].first > distance) {
+ for (index = 0; index < gains.size() - 1; ++index) {
+ if (gains[index + 1]->distance() > distance) {
break;
}
}
- return plan_gains_[index].second;
+ // ColMajor is the default storage order, but call it out explicitly here.
+ return Eigen::Matrix<float, 2, 5, Eigen::ColMajor>{
+ gains[index]->gains()->data()}
+ .cast<double>();
+}
+
+namespace {
+flatbuffers::Offset<Constraint> MakeWholeLengthConstraint(
+ flatbuffers::FlatBufferBuilder *fbb, ConstraintType constraint_type,
+ float value) {
+ Constraint::Builder builder(*fbb);
+ builder.add_constraint_type(constraint_type);
+ builder.add_value(value);
+ return builder.Finish();
+}
+} // namespace
+
+flatbuffers::Offset<fb::Trajectory> Trajectory::Serialize(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ std::array<flatbuffers::Offset<Constraint>, 3> constraints_offsets = {
+ MakeWholeLengthConstraint(fbb, ConstraintType::LONGITUDINAL_ACCELERATION,
+ max_longitudinal_accel()),
+ MakeWholeLengthConstraint(fbb, ConstraintType::LATERAL_ACCELERATION,
+ max_lateral_accel()),
+ MakeWholeLengthConstraint(fbb, ConstraintType::VOLTAGE, max_voltage())};
+ const auto constraints = fbb->CreateVector<Constraint>(
+ constraints_offsets.data(), constraints_offsets.size());
+ const flatbuffers::Offset<fb::DistanceSpline> spline_offset =
+ spline().Serialize(fbb, constraints);
+
+ std::vector<flatbuffers::Offset<fb::PlanPoint>> plan_points;
+ for (size_t ii = 0; ii < distance_plan_size(); ++ii) {
+ plan_points.push_back(fb::CreatePlanPoint(
+ *fbb, Distance(ii), plan_velocity(ii), plan_constraint(ii)));
+ }
+
+ // TODO(james): What is an appropriate cap?
+ CHECK_LT(plan_gains_.size(), 5000u);
+ CHECK_LT(0u, plan_gains_.size());
+ std::vector<flatbuffers::Offset<fb::GainPoint>> gain_points;
+ const size_t matrix_size = plan_gains_[0].second.size();
+ for (size_t ii = 0; ii < plan_gains_.size(); ++ii) {
+ gain_points.push_back(fb::CreateGainPoint(
+ *fbb, plan_gains_[ii].first,
+ fbb->CreateVector(plan_gains_[ii].second.data(), matrix_size)));
+ }
+
+ return fb::CreateTrajectory(*fbb, spline_idx_, fbb->CreateVector(plan_points),
+ fbb->CreateVector(gain_points), spline_offset,
+ drive_spline_backwards_);
+}
+
+float BaseTrajectory::ConstraintValue(
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ ConstraintType type) {
+ if (constraints != nullptr) {
+ for (const Constraint *constraint : *constraints) {
+ if (constraint->constraint_type() == type) {
+ return constraint->value();
+ }
+ }
+ }
+ return DefaultConstraint(type);
+}
+
+const Eigen::Matrix<double, 5, 1> BaseTrajectory::GoalState(
+ double distance, double velocity) const {
+ Eigen::Matrix<double, 5, 1> result;
+ result.block<2, 1>(0, 0) = spline().XY(distance);
+ 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();
+ return result;
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/trajectory.fbs b/frc971/control_loops/drivetrain/trajectory.fbs
new file mode 100644
index 0000000..ab14238
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.fbs
@@ -0,0 +1,44 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain.fb;
+
+enum SegmentConstraint : byte {
+ VELOCITY_LIMITED,
+ CURVATURE_LIMITED,
+ ACCELERATION_LIMITED,
+ DECELERATION_LIMITED,
+ VOLTAGE_LIMITED,
+}
+
+table PlanPoint {
+ // Distance along the path of this point.
+ distance: float (id: 0);
+ velocity: float (id: 1);
+ segment_constraint: SegmentConstraint (id: 2);
+}
+
+table GainPoint {
+ distance: float (id: 0);
+ // Column-major matrix (should be 2 rows x 5 columns).
+ gains: [float] (id: 1);
+}
+
+table DistanceSpline {
+ spline:frc971.MultiSpline (id: 0);
+ distances:[float] (id: 1);
+}
+
+table Trajectory {
+ // Unique ID of the trajectory, same as that defined in the SplineGoal.
+ handle:int (id: 0);
+ // Spline plan, indexed on distance.
+ distance_based_plan : [PlanPoint] (id : 1);
+ // Gains, indexed on time.
+ gains: [GainPoint] (id: 2);
+ spline:DistanceSpline (id: 3);
+
+ // Whether to follow the spline driving forwards or backwards.
+ drive_spline_backwards:bool (id: 4);
+}
+
+root_type Trajectory;
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index ef02db5..00b37f6 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -3,9 +3,12 @@
#include <chrono>
+#include "aos/flatbuffers.h"
#include "Eigen/Dense"
#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/hybrid_state_feedback_loop.h"
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -27,34 +30,23 @@
// true because when starting from a stop, under sane
// accelerations, we can assume that we will start with a
// constant acceleration. So, hard-code it.
- if (::std::abs(y) < 1e-6) {
+ if (std::abs(y) < 1e-6) {
return 0.0;
}
return (fn(t, y) - a0) / y;
},
v, x, dx) -
v) +
- ::std::sqrt(2.0 * a0 * dx + v * v);
+ std::sqrt(2.0 * a0 * dx + v * v);
}
-// Class to plan and hold the velocity plan for a spline.
-class Trajectory {
+class BaseTrajectory {
public:
- Trajectory(const DistanceSpline *spline,
- const DrivetrainConfig<double> &config, double vmax = 10.0,
- int num_distance = 0);
- // Sets the plan longitudinal acceleration limit
- void set_longitudinal_acceleration(double longitudinal_acceleration) {
- longitudinal_acceleration_ = longitudinal_acceleration;
- }
- // Sets the plan lateral acceleration limit
- void set_lateral_acceleration(double lateral_acceleration) {
- lateral_acceleration_ = lateral_acceleration;
- }
- // Sets the voltage limit
- void set_voltage_limit(double voltage_limit) {
- voltage_limit_ = voltage_limit;
- }
+ BaseTrajectory(
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ const DrivetrainConfig<double> &config);
+
+ virtual ~BaseTrajectory() = default;
// Returns the friction-constrained velocity limit at a given distance along
// the path. At the returned velocity, one or both wheels will be on the edge
@@ -74,6 +66,184 @@
void FrictionLngAccelLimits(double x, double v, double *min_accel,
double *max_accel) const;
+ // Returns the forwards/backwards acceleration for a distance along the spline
+ // taking into account the lateral acceleration, longitudinal acceleration,
+ // and voltage limits.
+ double BestAcceleration(double x, double v, bool backwards) const;
+ double BackwardAcceleration(double x, double v) const {
+ return BestAcceleration(x, v, true);
+ }
+ double ForwardAcceleration(double x, double v) const {
+ return BestAcceleration(x, v, false);
+ }
+
+ const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>
+ &velocity_drivetrain() const {
+ return *velocity_drivetrain_;
+ }
+
+ // Returns K1 and K2.
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const;
+ const Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const;
+
+ // Computes K3, K4, and K5 for the provided distance.
+ // K5 a + K3 v^2 + K4 v = U
+ void K345(const double x, Eigen::Matrix<double, 2, 1> *K3,
+ Eigen::Matrix<double, 2, 1> *K4,
+ Eigen::Matrix<double, 2, 1> *K5) const;
+
+ virtual const DistanceSpline &spline() const = 0;
+
+ // Returns the length of the path in meters.
+ double length() const { return spline().length(); }
+
+ // Returns whether a state represents a state at the end of the spline.
+ bool is_at_end(Eigen::Matrix<double, 2, 1> state) const {
+ return state(0) > length() - 1e-4;
+ }
+
+ // Returns true if the state is invalid or unreasonable in some way.
+ bool state_is_faulted(Eigen::Matrix<double, 2, 1> state) const {
+ // Consider things faulted if the current velocity implies we are going
+ // backwards or if any infinities/NaNs have crept in.
+ return state(1) < 0 || !state.allFinite();
+ }
+
+ virtual float plan_velocity(size_t index) const = 0;
+ virtual size_t distance_plan_size() const = 0;
+
+ // Sets the plan longitudinal acceleration limit
+ void set_longitudinal_acceleration(double longitudinal_acceleration) {
+ longitudinal_acceleration_ = longitudinal_acceleration;
+ }
+ // Sets the plan lateral acceleration limit
+ void set_lateral_acceleration(double lateral_acceleration) {
+ lateral_acceleration_ = lateral_acceleration;
+ }
+ // Sets the voltage limit
+ void set_voltage_limit(double voltage_limit) {
+ voltage_limit_ = voltage_limit;
+ }
+
+ float max_lateral_accel() const { return lateral_acceleration_; }
+
+ float max_longitudinal_accel() const { return longitudinal_acceleration_; }
+
+ float max_voltage() const { return voltage_limit_; }
+
+ // Return the next position, velocity, acceleration based on the current
+ // state. Updates the passed in state for the next iteration.
+ Eigen::Matrix<double, 3, 1> GetNextXVA(
+ std::chrono::nanoseconds dt, Eigen::Matrix<double, 2, 1> *state) const;
+
+ // Returns the distance for an index in the plan.
+ double Distance(int index) const {
+ return static_cast<double>(index) * length() /
+ static_cast<double>(distance_plan_size() - 1);
+ }
+
+ virtual fb::SegmentConstraint plan_constraint(size_t index) const = 0;
+
+ // Returns the feed forwards position, velocity, acceleration for an explicit
+ // distance.
+ Eigen::Matrix<double, 3, 1> FFAcceleration(double distance) const;
+
+ // Returns the feed forwards voltage for an explicit distance.
+ Eigen::Matrix<double, 2, 1> FFVoltage(double distance) const;
+
+ // Computes alpha for a distance.
+ size_t DistanceToSegment(double distance) const {
+ return std::max(
+ static_cast<size_t>(0),
+ std::min(distance_plan_size() - 1,
+ static_cast<size_t>(std::floor(distance / length() *
+ (distance_plan_size() - 1)))));
+ }
+
+ // Returns the goal state as a function of path distance, velocity.
+ const ::Eigen::Matrix<double, 5, 1> GoalState(double distance,
+ double velocity) const;
+
+ protected:
+ double robot_radius_l() const { return robot_radius_l_; }
+ double robot_radius_r() const { return robot_radius_r_; }
+
+ private:
+ static float ConstraintValue(
+ const flatbuffers::Vector<flatbuffers::Offset<Constraint>> *constraints,
+ ConstraintType type);
+
+ std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
+
+ const DrivetrainConfig<double> config_;
+
+ // Robot radiuses.
+ const double robot_radius_l_;
+ const double robot_radius_r_;
+ float lateral_acceleration_ = 3.0;
+ float longitudinal_acceleration_ = 2.0;
+ float voltage_limit_ = 12.0;
+};
+
+// A wrapper around the Trajectory flatbuffer to allow for controlling to a
+// spline using a pre-generated trajectory.
+class FinishedTrajectory : public BaseTrajectory {
+ 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);
+
+ virtual ~FinishedTrajectory() = default;
+
+ // Takes the 5-element state that is [x, y, theta, v_left, v_right] and
+ // converts it to a path-relative state, using distance as a linearization
+ // point (i.e., distance should be roughly equal to the actual distance along
+ // the path).
+ Eigen::Matrix<double, 5, 1> StateToPathRelativeState(
+ double distance, const Eigen::Matrix<double, 5, 1> &state) const;
+
+ // Retrieves the gain matrix K for a given distance along the path.
+ Eigen::Matrix<double, 2, 5> GainForDistance(double distance) const;
+
+ size_t distance_plan_size() const override;
+ float plan_velocity(size_t index) const override;
+ fb::SegmentConstraint plan_constraint(size_t index) const override;
+
+ bool drive_spline_backwards() const {
+ return trajectory().drive_spline_backwards();
+ }
+
+ int spline_handle() const { return trajectory().handle(); }
+ const fb::Trajectory &trajectory() const { return *buffer_; }
+
+ private:
+ const DistanceSpline &spline() const override { return spline_; }
+ const fb::Trajectory *buffer_;
+ const DistanceSpline spline_;
+};
+
+// Class to handle plannign a trajectory and producing a flatbuffer containing
+// all the information required to create a FinishedTrajectory;
+class Trajectory : public BaseTrajectory {
+ public:
+ Trajectory(const SplineGoal &spline_goal,
+ const DrivetrainConfig<double> &config);
+ Trajectory(
+ 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);
+
+ virtual ~Trajectory() = default;
+
+ std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(
+ std::chrono::nanoseconds dt);
+
enum class VoltageLimit {
kConservative,
kAggressive,
@@ -104,17 +274,6 @@
// Runs the forwards pass, setting the starting velocity to 0 m/s
void ForwardPass();
- // Returns the forwards/backwards acceleration for a distance along the spline
- // taking into account the lateral acceleration, longitudinal acceleration,
- // and voltage limits.
- double BestAcceleration(double x, double v, bool backwards);
- double BackwardAcceleration(double x, double v) {
- return BestAcceleration(x, v, true);
- }
- double ForwardAcceleration(double x, double v) {
- return BestAcceleration(x, v, false);
- }
-
// Runs the forwards pass, setting the ending velocity to 0 m/s
void BackwardPass();
@@ -127,115 +286,18 @@
CalculatePathGains();
}
- // Returns the feed forwards position, velocity, acceleration for an explicit
- // distance.
- ::Eigen::Matrix<double, 3, 1> FFAcceleration(double distance);
-
- // Returns the feed forwards voltage for an explicit distance.
- ::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
-
- // Returns whether a state represents a state at the end of the spline.
- bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
- return state(0) > length() - 1e-4;
- }
-
- // Returns true if the state is invalid or unreasonable in some way.
- bool state_is_faulted(::Eigen::Matrix<double, 2, 1> state) const {
- // Consider things faulted if the current velocity implies we are going
- // backwards or if any infinities/NaNs have crept in.
- return state(1) < 0 || !state.allFinite();
- }
-
- // Returns the length of the path in meters.
- double length() const { return spline_->length(); }
-
// Returns a list of the distances. Mostly useful for plotting.
- const ::std::vector<double> Distances() const;
+ const std::vector<double> Distances() const;
// Returns the distance for an index in the plan.
double Distance(int index) const {
return static_cast<double>(index) * length() /
static_cast<double>(plan_.size() - 1);
}
- // Returns the plan. This is the pathwise velocity as a function of distance.
- // To get the distance for an index, use the Distance(index) function provided
- // with the index.
- const ::std::vector<double> plan() const { return plan_; }
-
- // Returns the left, right to linear, angular transformation matrix.
- const ::Eigen::Matrix<double, 2, 2> &Tlr_to_la() const { return Tlr_to_la_; }
- // Returns the linear, angular to left, right transformation matrix.
- const ::Eigen::Matrix<double, 2, 2> &Tla_to_lr() const { return Tla_to_lr_; }
-
- // Returns the goal state as a function of path distance, velocity.
- const ::Eigen::Matrix<double, 5, 1> GoalState(double distance,
- double velocity);
-
- // Returns the velocity drivetrain in use.
- const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>
- &velocity_drivetrain() const {
- return *velocity_drivetrain_;
- }
-
- // Returns the continuous statespace A and B matricies for [x, y, theta, vl,
- // vr] for the linearized system (around the provided state).
- ::Eigen::Matrix<double, 5, 5> ALinearizedContinuous(
- const ::Eigen::Matrix<double, 5, 1> &state) const;
- ::Eigen::Matrix<double, 5, 2> BLinearizedContinuous() const;
-
- // Returns the discrete time A and B matricies for the provided state,
- // assuming the provided timestep.
- void AB(const ::Eigen::Matrix<double, 5, 1> &state,
- ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 5, 5> *A,
- ::Eigen::Matrix<double, 5, 2> *B) const;
-
- // Returns the lqr controller for the current state, timestep, and Q and R
- // gains. This controller is currently not used--we instead are experimenting
- // with the path-relative LQR (and potentially MPC, in the future) controller,
- // which uses the methods defined below.
- // TODO(austin): This feels like it should live somewhere else, but I'm not
- // sure where. So, throw it here...
- ::Eigen::Matrix<double, 2, 5> KForState(
- const ::Eigen::Matrix<double, 5, 1> &state, ::std::chrono::nanoseconds dt,
- const ::Eigen::DiagonalMatrix<double, 5> &Q,
- const ::Eigen::DiagonalMatrix<double, 2> &R) const;
-
- // Return the next position, velocity, acceleration based on the current
- // state. Updates the passed in state for the next iteration.
- ::Eigen::Matrix<double, 3, 1> GetNextXVA(
- ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
- ::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
- ::std::chrono::nanoseconds dt);
-
- enum SegmentType : uint8_t {
- VELOCITY_LIMITED,
- CURVATURE_LIMITED,
- ACCELERATION_LIMITED,
- DECELERATION_LIMITED,
- VOLTAGE_LIMITED,
- };
-
- const ::std::vector<SegmentType> &plan_segment_type() const {
+ const std::vector<fb::SegmentConstraint> &plan_segment_type() const {
return plan_segment_type_;
}
- // Returns K1 and K2.
- // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
- return (::Eigen::Matrix<double, 2, 1>()
- << -robot_radius_l_ * current_ddtheta,
- robot_radius_r_ * current_ddtheta)
- .finished();
- }
-
- const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
- return (::Eigen::Matrix<double, 2, 1>()
- << 1.0 - robot_radius_l_ * current_dtheta,
- 1.0 + robot_radius_r_ * current_dtheta)
- .finished();
- }
-
// The controller represented by these functions uses a discrete-time,
// finite-horizon LQR with states that are relative to the predicted path
// of the robot to produce a gain to be used on the error.
@@ -278,60 +340,31 @@
// phase, and is a relatively expensive operation.
void CalculatePathGains();
- // Retrieves the gain matrix K for a given distance along the path.
- Eigen::Matrix<double, 2, 5> GainForDistance(double distance);
+ flatbuffers::Offset<fb::Trajectory> Serialize(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ const std::vector<double> plan() const { return plan_; }
+
+ const DistanceSpline &spline() const override { return spline_; }
private:
- // Computes alpha for a distance.
- size_t DistanceToSegment(double distance) const {
- return ::std::max(
- static_cast<size_t>(0),
- ::std::min(plan_segment_type_.size() - 1,
- static_cast<size_t>(::std::floor(distance / length() *
- (plan_.size() - 1)))));
+
+ 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 {
+ return plan_segment_type_[index];
}
- // Computes K3, K4, and K5 for the provided distance.
- // K5 a + K3 v^2 + K4 v = U
- void K345(const double x, ::Eigen::Matrix<double, 2, 1> *K3,
- ::Eigen::Matrix<double, 2, 1> *K4,
- ::Eigen::Matrix<double, 2, 1> *K5) {
- const double current_ddtheta = spline_->DDTheta(x);
- const double current_dtheta = spline_->DTheta(x);
- // We've now got the equation:
- // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- const ::Eigen::Matrix<double, 2, 1> my_K2 = K2(current_dtheta);
-
- const ::Eigen::Matrix<double, 2, 2> B_inverse =
- velocity_drivetrain_->plant().coefficients().B_continuous.inverse();
-
- // Now, rephrase it as K5 a + K3 v^2 + K4 v = U
- *K3 = B_inverse * K1(current_ddtheta);
- *K4 = -B_inverse *
- velocity_drivetrain_->plant().coefficients().A_continuous * my_K2;
- *K5 = B_inverse * my_K2;
- }
+ const int spline_idx_;
// The spline we are planning for.
- const DistanceSpline *spline_;
- // The drivetrain we are controlling.
- ::std::unique_ptr<
- StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>
- velocity_drivetrain_;
+ const DistanceSpline spline_;
const DrivetrainConfig<double> config_;
- // Robot radiuses.
- const double robot_radius_l_;
- const double robot_radius_r_;
- // Acceleration limits.
- double longitudinal_acceleration_;
- double lateral_acceleration_;
- // Transformation matrix from left, right to linear, angular
- const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
- // Transformation matrix from linear, angular to left, right
- const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
// Velocities in the plan (distance for each index is defined by Distance())
std::vector<double> plan_;
// Gain matrices to use for the path-relative state error at each stage in the
@@ -339,24 +372,24 @@
// config_.dt in time.
// The first value in the pair is the distance along the path corresponding to
// the gain matrix; the second value is the gain itself.
- std::vector<std::pair<double, Eigen::Matrix<double, 2, 5>>> plan_gains_;
- std::vector<SegmentType> plan_segment_type_;
- // Plan voltage limit.
- double voltage_limit_ = 12.0;
+ std::vector<std::pair<double, Eigen::Matrix<float, 2, 5>>> plan_gains_;
+ std::vector<fb::SegmentConstraint> plan_segment_type_;
+
+ bool drive_spline_backwards_ = false;
};
// Returns the continuous time dynamics given the [x, y, theta, vl, vr] state
// and the [vl, vr] input voltage.
-inline ::Eigen::Matrix<double, 5, 1> ContinuousDynamics(
+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, 2, 2> &Tlr_to_la,
+ 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;
- return (::Eigen::Matrix<double, 5, 1>() << ::std::cos(theta) * la(0),
- ::std::sin(theta) * la(0), la(1),
+ Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;
+ return (Eigen::Matrix<double, 5, 1>() << std::cos(theta) * la(0),
+ std::sin(theta) * la(0), la(1),
(velocity_drivetrain.coefficients().A_continuous * velocity +
velocity_drivetrain.coefficients().B_continuous * U))
.finished();
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
new file mode 100644
index 0000000..16b6809
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -0,0 +1,30 @@
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+TrajectoryGenerator::TrajectoryGenerator(aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &config)
+ : event_loop_(event_loop),
+ dt_config_(config),
+ trajectory_sender_(
+ event_loop_->MakeSender<fb::Trajectory>("/drivetrain")) {
+ event_loop_->MakeWatcher("/drivetrain", [this](const SplineGoal &goal) {
+ HandleSplineGoal(goal);
+ });
+}
+
+void TrajectoryGenerator::HandleSplineGoal(const SplineGoal &goal) {
+ Trajectory trajectory(goal, dt_config_);
+ trajectory.Plan();
+
+ aos::Sender<fb::Trajectory>::Builder builder =
+ trajectory_sender_.MakeBuilder();
+
+ CHECK(builder.Send(trajectory.Serialize(builder.fbb())));
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.h b/frc971/control_loops/drivetrain/trajectory_generator.h
new file mode 100644
index 0000000..0bf5e92
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory_generator.h
@@ -0,0 +1,28 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/spline_goal_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+class TrajectoryGenerator {
+ public:
+ TrajectoryGenerator(aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &config);
+ void HandleSplineGoal(const SplineGoal &goal);
+ private:
+ aos::EventLoop *const event_loop_;
+ const DrivetrainConfig<double> dt_config_;
+ aos::Sender<fb::Trajectory> trajectory_sender_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_TRAJECTORY_GENERATOR_H_
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 6aff128..6fe1fbf 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -29,10 +29,6 @@
// https://photos.google.com/share/AF1QipPl34MOTPem2QmmTC3B21dL7GV2_HjxnseRrqxgR60TUasyIPliIuWmnH3yxuSNZw?key=cVhZLUYycXBIZlNTRy10cjZlWm0tcmlqQl9MTE13
DEFINE_bool(plot, true, "If true, plot");
-DEFINE_double(qx, 0.05, "Q_xpos");
-DEFINE_double(qy, 0.05, "Q_ypos");
-DEFINE_double(qt, 0.2, "Q_thetapos");
-DEFINE_double(qv, 0.5, "Q_vel");
DEFINE_double(dx, 0.0, "Amount to disturb x at the start");
DEFINE_double(dy, 0.0, "Amount to disturb y at the start");
@@ -49,13 +45,14 @@
namespace drivetrain {
void Main() {
- DistanceSpline distance_spline(Spline(
- Spline4To6((::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())));
+ const DrivetrainConfig<double> config =
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig();
Trajectory trajectory(
- &distance_spline,
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
+ DistanceSpline(Spline(Spline4To6(
+ (::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);
trajectory.set_lateral_acceleration(2.0);
trajectory.set_longitudinal_acceleration(1.0);
@@ -66,8 +63,9 @@
::std::vector<double> spline_theta;
for (const double distance : distances) {
- const ::Eigen::Matrix<double, 2, 1> point = distance_spline.XY(distance);
- const double theta = distance_spline.Theta(distance);
+ const ::Eigen::Matrix<double, 2, 1> point =
+ trajectory.spline().XY(distance);
+ const double theta = trajectory.spline().Theta(distance);
spline_x.push_back(point(0));
spline_y.push_back(point(1));
spline_theta.push_back(theta);
@@ -88,7 +86,7 @@
::std::vector<double> plan_segment_center_distance;
::std::vector<double> plan_type;
- for (Trajectory::SegmentType segment_type : trajectory.plan_segment_type()) {
+ for (fb::SegmentConstraint segment_type : trajectory.plan_segment_type()) {
plan_type.push_back(static_cast<int>(segment_type));
}
for (size_t i = 0; i < distances.size() - 1; ++i) {
@@ -128,24 +126,14 @@
}
}
- const double kXPos = FLAGS_qx;
- const double kYPos = FLAGS_qy;
- const double kThetaPos = FLAGS_qt;
- const double kVel = FLAGS_qv;
- const ::Eigen::DiagonalMatrix<double, 5> Q =
- (::Eigen::DiagonalMatrix<double, 5>().diagonal()
- << 1.0 / ::std::pow(kXPos, 2),
- 1.0 / ::std::pow(kYPos, 2), 1.0 / ::std::pow(kThetaPos, 2),
- 1.0 / ::std::pow(kVel, 2), 1.0 / ::std::pow(kVel, 2))
- .finished()
- .asDiagonal();
+ flatbuffers::FlatBufferBuilder fbb;
- const ::Eigen::DiagonalMatrix<double, 2> R =
- (::Eigen::DiagonalMatrix<double, 2>().diagonal()
- << 1.0 / ::std::pow(12.0, 2),
- 1.0 / ::std::pow(12.0, 2))
- .finished()
- .asDiagonal();
+ fbb.Finish(trajectory.Serialize(&fbb));
+
+ aos::FlatbufferDetachedBuffer<fb::Trajectory> trajectory_buffer(
+ fbb.Release());
+
+ FinishedTrajectory finished_trajectory(config, &trajectory_buffer.message());
::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
state(0, 0) = FLAGS_dx;
@@ -186,17 +174,17 @@
error_velocity_l.push_back(state_error(3));
error_velocity_r.push_back(state_error(4));
- ::Eigen::Matrix<double, 2, 5> K =
- trajectory.KForState(state, chrono::microseconds(5050), Q, R);
+ const ::Eigen::Matrix<double, 2, 5> K =
+ finished_trajectory.GainForDistance(distance);
const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory.FFVoltage(distance);
const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
state = RungeKuttaU(
- [&trajectory](const ::Eigen::Matrix<double, 5, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
+ [&trajectory, &config](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
return ContinuousDynamics(trajectory.velocity_drivetrain().plant(),
- trajectory.Tlr_to_la(), X, U);
+ config.Tlr_to_la(), X, U);
},
state, U, kDtDouble);
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 2770f63..c2f6655 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -63,18 +63,22 @@
public:
::aos::testing::TestSharedMemory shm_;
- ::std::unique_ptr<DistanceSpline> distance_spline_;
+ const DistanceSpline *distance_spline_;
::std::unique_ptr<Trajectory> trajectory_;
+ std::unique_ptr<aos::FlatbufferDetachedBuffer<fb::Trajectory>>
+ trajectory_buffer_;
+ ::std::unique_ptr<FinishedTrajectory> finished_trajectory_;
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
ParameterizedSplineTest() : dt_config_(GetTestDrivetrainConfig()) {}
void SetUp() {
- distance_spline_ = ::std::unique_ptr<DistanceSpline>(
- new DistanceSpline(Spline(GetParam().control_points)));
+ const int spline_index = 12345;
// Run lots of steps to make the feedforwards terms more accurate.
trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
- distance_spline_.get(), dt_config_, GetParam().velocity_limit));
+ DistanceSpline(GetParam().control_points), dt_config_,
+ /*constraints=*/nullptr, spline_index, GetParam().velocity_limit));
+ distance_spline_ = &trajectory_->spline();
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
trajectory_->set_longitudinal_acceleration(
GetParam().longitudinal_acceleration);
@@ -98,6 +102,18 @@
trajectory_->CalculatePathGains();
length_plan_xva_ = trajectory_->PlanXVA(dt_config_.dt);
+
+ flatbuffers::FlatBufferBuilder fbb;
+
+ fbb.Finish(trajectory_->Serialize(&fbb));
+
+ trajectory_buffer_ = std::make_unique<aos::FlatbufferDetachedBuffer<
+ frc971::control_loops::drivetrain::fb::Trajectory>>(fbb.Release());
+
+ EXPECT_EQ(spline_index, trajectory_buffer_->message().handle());
+
+ finished_trajectory_ = std::make_unique<FinishedTrajectory>(
+ dt_config_, &trajectory_buffer_->message());
}
void TearDown() {
@@ -118,7 +134,7 @@
::std::vector<double> plan_segment_center_distance;
::std::vector<double> plan_type;
- for (Trajectory::SegmentType segment_type :
+ for (fb::SegmentConstraint segment_type :
trajectory_->plan_segment_type()) {
plan_type.push_back(static_cast<int>(segment_type));
}
@@ -351,12 +367,13 @@
// to the right point.
TEST_P(ParameterizedSplineTest, FFSpline) {
::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
- state = trajectory_->GoalState(0.0, 0.0);
+ state = finished_trajectory_->GoalState(0.0, 0.0);
for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
const double distance = length_plan_xva_[i](0);
- const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+ const ::Eigen::Matrix<double, 2, 1> U_ff =
+ finished_trajectory_->FFVoltage(distance);
const ::Eigen::Matrix<double, 2, 1> U = U_ff;
length_plan_vl_.push_back(U(0));
@@ -365,50 +382,17 @@
[this](const ::Eigen::Matrix<double, 5, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) {
return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
- trajectory_->Tlr_to_la(), X, U);
+ dt_config_.Tlr_to_la(), X, U);
},
state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
}
- EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
+ EXPECT_LT((state - finished_trajectory_->GoalState(
+ finished_trajectory_->length(), 0.0))
+ .norm(),
4e-2);
}
-// Tests that following a spline with both feed forwards and feed back gets
-// pretty darn close to the right point.
-TEST_P(ParameterizedSplineTest, FBSpline) {
- ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
-
- for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
- const double distance = length_plan_xva_[i](0);
- const double velocity = length_plan_xva_[i](1);
- const ::Eigen::Matrix<double, 5, 1> goal_state =
- trajectory_->GoalState(distance, velocity);
-
- const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
-
- const ::Eigen::Matrix<double, 2, 5> K =
- trajectory_->KForState(state, dt_config_.dt, Q, R);
-
- const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
- const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
- const ::Eigen::Matrix<double, 2, 1> U = U_ff + U_fb;
-
- length_plan_vl_.push_back(U(0));
- length_plan_vr_.push_back(U(1));
- state = RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 5, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
- trajectory_->Tlr_to_la(), X, U);
- },
- state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
- }
-
- EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
- 2e-2);
-}
-
// Tests that Iteratively computing the XVA plan is the same as precomputing it.
TEST_P(ParameterizedSplineTest, IterativeXVA) {
for (double v : trajectory_->plan()) {
@@ -417,7 +401,7 @@
::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
::Eigen::Matrix<double, 3, 1> xva =
- trajectory_->GetNextXVA(dt_config_.dt, &state);
+ finished_trajectory_->GetNextXVA(dt_config_.dt, &state);
EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
}
}
@@ -455,20 +439,22 @@
continue;
}
- const Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+ const Eigen::Matrix<double, 2, 1> U_ff =
+ finished_trajectory_->FFVoltage(distance);
const Eigen::Matrix<double, 2, 1> U = U_ff;
absolute_state = RungeKuttaU(
[this](const Eigen::Matrix<double, 5, 1> &X,
const Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
- trajectory_->Tlr_to_la(), X, U);
+ return ContinuousDynamics(finished_trajectory_->velocity_drivetrain().plant(),
+ dt_config_.Tlr_to_la(), X, U);
},
absolute_state, U, aos::time::DurationInSeconds(dt_config_.dt));
const Eigen::Matrix<double, 5, 1> goal_absolute_state =
- trajectory_->GoalState(distance, velocity);
+ finished_trajectory_->GoalState(distance, velocity);
const Eigen::Matrix<double, 5, 1> goal_relative_state =
- trajectory_->StateToPathRelativeState(distance, goal_absolute_state);
+ finished_trajectory_->StateToPathRelativeState(distance,
+ goal_absolute_state);
ASSERT_EQ(distance, goal_relative_state(0));
ASSERT_EQ(0.0, goal_relative_state(1));
ASSERT_NEAR(goal_absolute_state(2), goal_relative_state(2), 1e-2);
@@ -497,15 +483,18 @@
const double velocity = length_plan_xva_[i](1);
const Eigen::Matrix<double, 5, 1> goal_absolute_state =
- trajectory_->GoalState(distance, velocity);
+ finished_trajectory_->GoalState(distance, velocity);
const Eigen::Matrix<double, 5, 1> goal_relative_state =
- trajectory_->StateToPathRelativeState(distance, goal_absolute_state);
+ finished_trajectory_->StateToPathRelativeState(distance,
+ goal_absolute_state);
const Eigen::Matrix<double, 5, 1> current_relative_state =
- trajectory_->StateToPathRelativeState(distance, absolute_state);
+ finished_trajectory_->StateToPathRelativeState(distance,
+ absolute_state);
- const Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
+ const Eigen::Matrix<double, 2, 1> U_ff =
+ finished_trajectory_->FFVoltage(distance);
Eigen::Matrix<double, 2, 1> U_fb =
- trajectory_->GainForDistance(distance) *
+ finished_trajectory_->GainForDistance(distance) *
(goal_relative_state - current_relative_state);
if (i == 0) {
initial_error = (goal_relative_state - current_relative_state).norm();
@@ -516,15 +505,15 @@
[this](const Eigen::Matrix<double, 5, 1> &X,
const Eigen::Matrix<double, 2, 1> &U) {
return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
- trajectory_->Tlr_to_la(), X, U);
+ dt_config_.Tlr_to_la(), X, U);
},
absolute_state, U, aos::time::DurationInSeconds(dt_config_.dt));
}
- EXPECT_LT(
- (absolute_state - trajectory_->GoalState(trajectory_->length(), 0.0))
- .norm(),
- 4e-2 + initial_error * 0.5);
+ EXPECT_LT((absolute_state - finished_trajectory_->GoalState(
+ finished_trajectory_->length(), 0.0))
+ .norm(),
+ 4e-2 + initial_error * 0.5);
}
SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index cf69a5c..de44d72 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -23,7 +23,7 @@
[2.0, 3.0, 4.0, 5.0, 6.0, 7.0]])
spline = Spline(points)
dSpline = DistanceSpline([spline])
- assert_almost_equal(dSpline.Length(), 5 * math.sqrt(2))
+ assert_almost_equal(dSpline.Length(), 5 * math.sqrt(2), decimal=5)
class TestTrajectory(unittest.TestCase):
@@ -35,7 +35,7 @@
trajectory = Trajectory(dSpline)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05 * 1e-3)
- self.assertEqual(plan.shape, (3, 624))
+ self.assertEqual(plan.shape, (3, 745))
def testLimits(self):
""" A plan with a lower limit should take longer. """
@@ -47,7 +47,7 @@
trajectory.LimitVelocity(0, trajectory.Length(), 3)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05 * 1e-3)
- self.assertEqual(plan.shape, (3, 656))
+ self.assertEqual(plan.shape, (3, 753))
if __name__ == '__main__':