Fix backwards spline feedback
We weren't handling the logic for feedback on backwards splines
correctly.
TODO: Clean up some of the backwards logic to make it possible to
follow.
Change-Id: I0c18b8c245010151356935dfcbf8babdb98d0846
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index b08e96f..4e6f373 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -643,6 +643,7 @@
":drivetrain_config",
":spline_goal_fbs",
":trajectory_fbs",
+ "//aos/util:math",
"//frc971/control_loops:c2d",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:hybrid_state_feedback_loop",
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7dd5a52..d57e81e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -811,9 +811,25 @@
CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->has_y());
}
+class DrivetrainBackwardsParamTest
+ : public DrivetrainTest,
+ public ::testing::WithParamInterface<bool> {};
+
// Tests that simple spline converges when it doesn't start where it thinks.
-TEST_F(DrivetrainTest, SplineOffset) {
+TEST_P(DrivetrainBackwardsParamTest, SplineOffset) {
SetEnabled(true);
+ if (GetParam()) {
+ // Turn the robot around if we are backwards.
+ (*drivetrain_plant_.mutable_state())(2) += M_PI;
+
+ auto builder = localizer_control_sender_.MakeBuilder();
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(drivetrain_plant_.state()(0));
+ localizer_control_builder.add_y(drivetrain_plant_.state()(1));
+ localizer_control_builder.add_theta(drivetrain_plant_.state()(2));
+ ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+ }
{
auto builder = trajectory_goal_sender_.MakeBuilder();
@@ -834,7 +850,7 @@
SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
spline_goal_builder.add_spline_idx(1);
- spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_drive_spline_backwards(GetParam());
spline_goal_builder.add_spline(multispline_offset);
ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
}
@@ -852,6 +868,10 @@
VerifyNearSplineGoal();
}
+INSTANTIATE_TEST_CASE_P(DriveSplinesForwardsAndBackwards,
+ DrivetrainBackwardsParamTest,
+ ::testing::Values(false, true));
+
// Tests that simple spline converges when it starts to the side of where it
// thinks.
TEST_F(DrivetrainTest, SplineSideOffset) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..fd0ad6e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -117,7 +117,8 @@
::Eigen::Matrix<double, 2, 5> K =
current_trajectory().GainForDistance(current_distance);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- if (current_trajectory().drive_spline_backwards()) {
+ const bool backwards = current_trajectory().drive_spline_backwards();
+ if (backwards) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
goal_state(2, 0) += M_PI;
@@ -128,13 +129,19 @@
}
const Eigen::Matrix<double, 5, 1> relative_goal =
current_trajectory().StateToPathRelativeState(current_distance,
- goal_state);
+ goal_state, backwards);
const Eigen::Matrix<double, 5, 1> relative_state =
- current_trajectory().StateToPathRelativeState(current_distance, state);
+ current_trajectory().StateToPathRelativeState(current_distance, state,
+ backwards);
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;
+ if (backwards) {
+ Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
+ U_fb = -swapU;
+ }
+
::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_U_ = U_ff + U_fb - voltage_error;
@@ -184,11 +191,17 @@
::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_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));
+ if (current_trajectory().drive_spline_backwards()) {
+ trajectory_logging_builder.add_left_velocity(-goal_state(4));
+ trajectory_logging_builder.add_right_velocity(-goal_state(3));
+ trajectory_logging_builder.add_theta(
+ ::aos::math::NormalizeAngle(goal_state(2) + M_PI));
+ } else {
+ trajectory_logging_builder.add_theta(
+ ::aos::math::NormalizeAngle(goal_state(2)));
+ trajectory_logging_builder.add_left_velocity(goal_state(3));
+ trajectory_logging_builder.add_right_velocity(goal_state(4));
+ }
}
trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
executing_spline_);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 08dd3fd..a096fec 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -2,6 +2,7 @@
#include <chrono>
+#include "aos/util/math.h"
#include "Eigen/Dense"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/dlqr.h"
@@ -711,7 +712,8 @@
}
Eigen::Matrix<double, 5, 1> FinishedTrajectory::StateToPathRelativeState(
- double distance, const Eigen::Matrix<double, 5, 1> &state) const {
+ double distance, const Eigen::Matrix<double, 5, 1> &state,
+ bool drive_backwards) 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 =
@@ -721,9 +723,16 @@
Eigen::Matrix<double, 5, 1> path_state;
path_state(0) = distance + xy_err.x() * ctheta + xy_err.y() * stheta;
path_state(1) = -xy_err.x() * stheta + xy_err.y() * ctheta;
- path_state(2) = state(2) - nominal_theta;
+ path_state(2) = aos::math::NormalizeAngle(state(2) - nominal_theta +
+ (drive_backwards ? M_PI : 0.0));
+ path_state(2) = aos::math::NormalizeAngle(state(2) - nominal_theta);
path_state(3) = state(3);
path_state(4) = state(4);
+ if (drive_backwards) {
+ std::swap(path_state(3), path_state(4));
+ path_state(3) *= -1.0;
+ path_state(4) *= -1.0;
+ }
return path_state;
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 00b37f6..60695de 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -206,7 +206,8 @@
// 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;
+ double distance, const Eigen::Matrix<double, 5, 1> &state,
+ bool drive_backwards) const;
// Retrieves the gain matrix K for a given distance along the path.
Eigen::Matrix<double, 2, 5> GainForDistance(double distance) const;
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index c2f6655..c13d844 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -453,8 +453,8 @@
const Eigen::Matrix<double, 5, 1> goal_absolute_state =
finished_trajectory_->GoalState(distance, velocity);
const Eigen::Matrix<double, 5, 1> goal_relative_state =
- finished_trajectory_->StateToPathRelativeState(distance,
- goal_absolute_state);
+ finished_trajectory_->StateToPathRelativeState(
+ distance, goal_absolute_state, false);
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);
@@ -485,11 +485,11 @@
const Eigen::Matrix<double, 5, 1> goal_absolute_state =
finished_trajectory_->GoalState(distance, velocity);
const Eigen::Matrix<double, 5, 1> goal_relative_state =
- finished_trajectory_->StateToPathRelativeState(distance,
- goal_absolute_state);
+ finished_trajectory_->StateToPathRelativeState(
+ distance, goal_absolute_state, false);
const Eigen::Matrix<double, 5, 1> current_relative_state =
- finished_trajectory_->StateToPathRelativeState(distance,
- absolute_state);
+ finished_trajectory_->StateToPathRelativeState(distance, absolute_state,
+ false);
const Eigen::Matrix<double, 2, 1> U_ff =
finished_trajectory_->FFVoltage(distance);