Add tests for various spline types
And an ability to plot them to debug. Note, there's an ODR violation
somewhere that linking everything statically fixes. Not cool, but
whatever.
Change-Id: I9ea891a77279b17f6851b760c2be38c5b76a2d2b
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 577250a..ac6e2d3 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -2,6 +2,7 @@
load("//aos/build:queues.bzl", "queue_library")
load("//tools:environments.bzl", "mcu_cpus")
+load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
cc_binary(
name = "replay_drivetrain",
@@ -346,11 +347,24 @@
srcs = [
"trajectory_test.cc",
],
+ defines =
+ cpu_select({
+ "amd64": [
+ "SUPPORT_PLOT=1",
+ ],
+ "arm": [],
+ }),
+ linkstatic = True,
deps = [
":trajectory",
"//aos/testing:googletest",
"//aos/testing:test_shm",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
- ],
+ ] + cpu_select({
+ "amd64": [
+ "//third_party/matplotlib-cpp",
+ ],
+ "arm": [],
+ }),
)
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index 520ee1a9..8ba9fca 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -4,13 +4,19 @@
#include <vector>
#include "aos/testing/test_shm.h"
+#include "gflags/gflags.h"
#include "gtest/gtest.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+DEFINE_bool(plot, false, "If true, plot");
+
namespace frc971 {
namespace control_loops {
namespace drivetrain {
@@ -66,28 +72,80 @@
return kDrivetrainConfig;
};
-class SplineTest : public ::testing::Test {
- protected:
+struct SplineTestParams {
+ ::Eigen::Matrix<double, 2, 4> control_points;
+ double lateral_acceleration;
+ double longitudal_acceleration;
+ double velocity_limit;
+ double voltage_limit;
+};
+
+class ParameterizedSplineTest
+ : public ::testing::TestWithParam<SplineTestParams> {
+ public:
::aos::testing::TestSharedMemory shm_;
static constexpr chrono::nanoseconds kDt =
chrono::duration_cast<chrono::nanoseconds>(
chrono::duration<double>(::y2016::control_loops::drivetrain::kDt));
- DistanceSpline distance_spline_;
- Trajectory trajectory_;
+ ::std::unique_ptr<DistanceSpline> distance_spline_;
+ ::std::unique_ptr<Trajectory> trajectory_;
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
- SplineTest()
- : distance_spline_(Spline((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
- -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
- .finished())),
- trajectory_(&distance_spline_, GetDrivetrainConfig()) {
- trajectory_.set_lateral_acceleration(2.0);
- trajectory_.set_longitudal_acceleration(1.0);
- trajectory_.LateralAccelPass();
- trajectory_.ForwardPass();
- trajectory_.BackwardPass();
- length_plan_xva_ = trajectory_.PlanXVA(kDt);
+ ParameterizedSplineTest() {}
+
+ void SetUp() {
+ distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ new DistanceSpline(Spline(GetParam().control_points)));
+ trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(distance_spline_.get(), GetDrivetrainConfig(),
+ GetParam().velocity_limit));
+ trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
+ trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+ trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+ trajectory_->set_voltage_limit(GetParam().voltage_limit);
+
+ initial_plan_ = trajectory_->plan();
+ trajectory_->LateralAccelPass();
+ curvature_plan_ = trajectory_->plan();
+ trajectory_->ForwardPass();
+ forward_plan_ = trajectory_->plan();
+ trajectory_->BackwardPass();
+ backward_plan_ = trajectory_->plan();
+
+ length_plan_xva_ = trajectory_->PlanXVA(kDt);
+ }
+
+ void TearDown() {
+#if defined(SUPPORT_PLOT)
+ if (FLAGS_plot) {
+ ::std::vector<double> distances = trajectory_->Distances();
+
+ for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+ length_plan_t_.push_back(i * ::y2016::control_loops::drivetrain::kDt);
+ length_plan_x_.push_back(length_plan_xva_[i](0));
+ length_plan_v_.push_back(length_plan_xva_[i](1));
+ length_plan_a_.push_back(length_plan_xva_[i](2));
+ }
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
+ matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
+ matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
+ matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(length_plan_t_, length_plan_x_, {{"label", "x"}});
+ matplotlibcpp::plot(length_plan_t_, length_plan_v_, {{"label", "v"}});
+ matplotlibcpp::plot(length_plan_t_, length_plan_a_, {{"label", "a"}});
+ matplotlibcpp::plot(length_plan_t_, length_plan_vl_, {{"label", "Vl"}});
+ matplotlibcpp::plot(length_plan_t_, length_plan_vr_, {{"label", "Vr"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+#endif
}
static constexpr double kXPos = 0.05;
@@ -108,65 +166,120 @@
1.0 / ::std::pow(12.0, 2))
.finished()
.asDiagonal();
+
+ ::std::vector<double> initial_plan_;
+ ::std::vector<double> curvature_plan_;
+ ::std::vector<double> forward_plan_;
+ ::std::vector<double> backward_plan_;
+
+ ::std::vector<double> length_plan_t_;
+ ::std::vector<double> length_plan_x_;
+ ::std::vector<double> length_plan_v_;
+ ::std::vector<double> length_plan_a_;
+ ::std::vector<double> length_plan_vl_;
+ ::std::vector<double> length_plan_vr_;
+
};
-constexpr chrono::nanoseconds SplineTest::kDt;
+constexpr chrono::nanoseconds ParameterizedSplineTest::kDt;
// Tests that following a spline with feed forwards only gets pretty darn close
// to the right point.
-TEST_F(SplineTest, FFSpline) {
+TEST_P(ParameterizedSplineTest, FFSpline) {
::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 ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(distance);
+ const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
const ::Eigen::Matrix<double, 2, 1> U = U_ff;
+
+ 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);
+ return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
+ trajectory_->Tlr_to_la(), X, U);
},
state, U, ::y2016::control_loops::drivetrain::kDt);
}
- EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+ EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
2e-2);
}
// Tests that following a spline with both feed forwards and feed back gets
// pretty darn close to the right point.
-TEST_F(SplineTest, FBSpline) {
+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);
+ 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, SplineTest::kDt, Q, R);
+ trajectory_->KForState(state, ParameterizedSplineTest::kDt, Q, R);
- const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_.FFVoltage(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;
+
+ 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);
+ return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
+ trajectory_->Tlr_to_la(), X, U);
},
state, U, ::y2016::control_loops::drivetrain::kDt);
}
- EXPECT_LT((state - trajectory_.GoalState(trajectory_.length(), 0.0)).norm(),
+ EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
1.1e-2);
}
+SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
+ return params;
+}
+
+INSTANTIATE_TEST_CASE_P(
+ SplineTest, ParameterizedSplineTest,
+ ::testing::Values(
+ // Normal spline.
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
+ -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 2.0 /*lateral acceleration*/,
+ 1.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */}),
+ // Be velocity limited.
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
+ -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 2.0 /*lateral acceleration*/,
+ 1.0 /*longitudinal acceleration*/,
+ 0.5 /* velocity limit */, 12.0 /* volts */}),
+ // Hit the voltage limit.
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
+ -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 2.0 /*lateral acceleration*/,
+ 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 5.0 /* volts */}),
+ // Hit the curvature limit
+ MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
+ -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
+ .finished(),
+ 1.0 /*lateral acceleration*/,
+ 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */})));
+
// TODO(austin): Handle saturation. 254 does this by just not going that
// fast... We want to maybe replan when we get behind, or something. Maybe
// stop moving the setpoint like our 2018 arm?