Add feedback to the spline drivetrain.
Change-Id: I11bc55bd6d8ca866dbd85d2b1efcd8e278f8021f
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f81d..d097ec8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,7 +337,8 @@
dt_closedloop_.Update(output != NULL && controller_type == 1);
- dt_spline_.Update(output != NULL && controller_type == 2);
+ dt_spline_.Update(output != NULL && controller_type == 2,
+ xytheta_state_.block<5, 1>(0, 0));
switch (controller_type) {
case 0:
@@ -389,6 +390,7 @@
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);
+ dt_spline_.PopulateStatus(status);
}
double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 88547d5..0f57bf9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -111,6 +111,15 @@
return angular;
}
+ Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
+ return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
+ -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+ }
+
+ Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
+ return Tlr_to_la().inverse();
+ }
+
// Converts the linear and angular position, velocity to the top 4 states of
// the robot state.
Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 17da3ab..6c16667 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/queues/gyro.q.h"
#include "y2016/constants.h"
@@ -108,7 +109,15 @@
".frc971.control_loops.drivetrain_queue.position",
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
- gyro_reading_(::frc971::sensors::gyro_reading.name()) {
+ gyro_reading_(::frc971::sensors::gyro_reading.name()),
+ 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>>(
+ GetDrivetrainConfig()
+ .make_hybrid_drivetrain_velocity_loop()))) {
Reinitialize();
last_U_.setZero();
}
@@ -188,6 +197,16 @@
U(0, 0) += drivetrain_plant_->left_voltage_offset();
U(1, 0) += drivetrain_plant_->right_voltage_offset();
drivetrain_plant_->Update(U);
+ double dt_float =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ GetDrivetrainConfig().dt).count();
+ state = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(),
+ GetDrivetrainConfig().Tlr_to_la(), X, U);
+ },
+ state, U, dt_float);
}
void set_left_voltage_offset(double left_voltage_offset) {
@@ -199,10 +218,18 @@
::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
+ ::Eigen::Matrix<double, 2, 1> GetPosition() const {
+ return state.block<2,1>(0,0);
+ }
+
private:
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+ ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+ ::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>> velocity_drivetrain_;
double last_left_position_;
double last_right_position_;
@@ -259,6 +286,12 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), expected(0), 1e-2);
+ EXPECT_NEAR(actual(1), expected(1), 1e-2);
+ }
+
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
};
@@ -526,6 +559,135 @@
VerifyNearGoal();
}
+// Tests that simple spline converges on a goal.
+TEST_F(DrivetrainTest, SplineSimple) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it doesn't start where it thinks.
+TEST_F(DrivetrainTest, SplineOffset) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.5, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it starts to the side of where it
+// thinks.
+TEST_F(DrivetrainTest, SplineSideOffset) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that a multispline converges on a goal.
+TEST_F(DrivetrainTest, MultiSpline) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 2;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+
+ RunForTime(chrono::milliseconds(4000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
+// Tests that several splines converges on a goal.
+TEST_F(DrivetrainTest, SequentialSplines) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ second_spline_goal->controller_type = 2;
+ second_spline_goal->spline.spline_idx = 2;
+ second_spline_goal->spline.spline_count = 1;
+ second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal.Send();
+ RunIteration();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ second_start_goal->controller_type = 2;
+ second_start_goal->spline_handle = 2;
+ second_start_goal.Send();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0d6b6ae..b45f218 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,5 +1,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+#include <iostream>
+
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -12,7 +14,17 @@
namespace drivetrain {
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config) {}
+ : dt_config_(dt_config),
+ current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+
+void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+ bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
+ ::std::abs((*U)(1, 0)) > 12.0;
+
+ if (output_was_capped) {
+ *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
+ }
+}
// TODO(alex): put in another thread to avoid malloc in RT.
void SplineDrivetrain::SetGoal(
@@ -65,17 +77,36 @@
current_trajectory_->Plan();
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
}
}
-void SplineDrivetrain::Update(bool enable) {
- if (enable && current_trajectory_ &&
- !current_trajectory_->is_at_end(current_state_)) {
+// TODO(alex): Hold position when done following the spline.
+// TODO(Austin): Compensate for voltage error.
+void SplineDrivetrain::Update(bool enable,
+ const ::Eigen::Matrix<double, 5, 1> &state) {
+ enable_ = enable;
+ if (enable && current_trajectory_) {
+ ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+ if (!current_trajectory_->is_at_end(current_state_)) {
+ // 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));
+ }
+ ::Eigen::Matrix<double, 2, 5> K =
+ current_trajectory_->KForState(state, dt_config_.dt, Q, R);
+ ::Eigen::Matrix<double, 5, 1> goal_state =
+ current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+ ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ next_U_ = U_ff + U_fb;
+ uncapped_U_ = next_U_;
+ ScaleCapU(&next_U_);
+
next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
}
}
-// TODO(alex): Handle drift.
void SplineDrivetrain::SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output) {
if (!output) {
@@ -86,16 +117,22 @@
}
if (current_spline_handle_ == current_spline_idx_) {
if (!current_trajectory_->is_at_end(current_state_)) {
- double current_distance = current_xva_(0);
- ::Eigen::Matrix<double, 2, 1> FFVoltage =
- current_trajectory_->FFVoltage(current_distance);
- output->left_voltage = FFVoltage(0);
- output->right_voltage = FFVoltage(1);
+ output->left_voltage = next_U_(0);
+ output->right_voltage = next_U_(1);
current_xva_ = next_xva_;
}
}
}
+void SplineDrivetrain::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ if (status && enable_) {
+ status->uncapped_left_voltage = uncapped_U_(0);
+ status->uncapped_right_voltage = uncapped_U_(1);
+ status->robot_speed = current_xva_(1);
+ }
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 4bb7ac9..a065167 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -19,7 +19,7 @@
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
- void Update(bool enabled);
+ void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
void SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -27,6 +27,8 @@
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
private:
+ void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
+
const DrivetrainConfig<double> dt_config_;
uint32_t current_spline_handle_; // Current spline told to excecute.
@@ -35,6 +37,25 @@
::std::unique_ptr<Trajectory> current_trajectory_;
::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
::Eigen::Matrix<double, 2, 1> current_state_;
+ ::Eigen::Matrix<double, 2, 1> next_U_;
+ ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+
+ bool enable_;
+
+ // TODO(alex): pull this out of dt_config.
+ const ::Eigen::DiagonalMatrix<double, 5> Q =
+ (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+ << 1.0 / ::std::pow(0.05, 2),
+ 1.0 / ::std::pow(0.05, 2), 1.0 / ::std::pow(0.2, 2),
+ 1.0 / ::std::pow(0.5, 2), 1.0 / ::std::pow(0.5, 2))
+ .finished()
+ .asDiagonal();
+ 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();
};
} // namespace drivetrain