Arm works
Added gravity and calibrated it. Terifying...
Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index c8b0b84..1d52ab4 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -26,6 +26,7 @@
"//aos/common/controls:control_loop",
"//frc971/control_loops:queues",
"//y2018:constants",
+ "//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 57b60e6..997d940 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -9,6 +9,7 @@
visibility = ["//visibility:public"],
deps = [
":dynamics",
+ "//aos/common/logging",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
"//third_party/eigen",
@@ -42,6 +43,7 @@
deps = [
"//frc971/control_loops:runge_kutta",
"//third_party/eigen",
+ "//third_party/gflags",
],
)
@@ -76,6 +78,7 @@
":ekf",
":trajectory",
"//third_party/eigen",
+ "//third_party/gflags",
"//third_party/matplotlib-cpp",
],
)
@@ -110,3 +113,24 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "arm",
+ srcs = [
+ "arm.cc",
+ ],
+ hdrs = [
+ "arm.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":demo_path",
+ ":ekf",
+ ":graph",
+ ":trajectory",
+ "//aos/common/logging:queue_logging",
+ "//frc971/zeroing",
+ "//y2018:constants",
+ "//y2018/control_loops/superstructure:superstructure_queue",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
new file mode 100644
index 0000000..484202e
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -0,0 +1,237 @@
+#include "y2018/control_loops/superstructure/arm/arm.h"
+
+#include <chrono>
+#include <iostream>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+SearchGraph::Edge MakeEdge(::std::vector<TrajectoryPair> *trajectories,
+ size_t start, size_t end, ::std::unique_ptr<Path> forwards_path,
+ ::std::unique_ptr<Path> backwards_path) {
+ trajectories->emplace_back(::std::move(forwards_path),
+ ::std::move(backwards_path), 0.005);
+
+ return SearchGraph::Edge{start, end,
+ trajectories->back().forwards.path().length()};
+}
+
+SearchGraph MakeSearchGraph(::std::vector<TrajectoryPair> *trajectories) {
+ ::std::vector<SearchGraph::Edge> edges;
+
+ // TODO(austin): Add more trajectories here.
+ edges.push_back(
+ MakeEdge(trajectories, 0, 1, MakeReversedDemoPath(), MakeDemoPath()));
+
+ return SearchGraph(2, ::std::move(edges));
+}
+
+Arm::Arm()
+ : proximal_zeroing_estimator_(constants::GetValues().arm_proximal.zeroing),
+ distal_zeroing_estimator_(constants::GetValues().arm_distal.zeroing),
+ alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
+ 0.0, 0.0, 1.0 / kAlpha1Max())
+ .finished()),
+ search_graph_(MakeSearchGraph(&trajectories_)),
+ // Go to the start of the first trajectory.
+ follower_(trajectories_[0].forwards.path().Theta(0)) {
+ // TODO(austin): Stop talking about indeces in the list and start talking
+ // about points/search for the nearest point.
+ for (TrajectoryPair &trajectory_pair : trajectories_) {
+ trajectory_pair.forwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
+ trajectory_pair.backwards.OptimizeTrajectory(alpha_unitizer_, kVMax());
+ }
+}
+
+void Arm::Reset() { state_ = State::UNINITIALIZED; }
+
+void Arm::Iterate(const uint32_t *unsafe_goal,
+ const control_loops::ArmPosition *position,
+ double *proximal_output, double *distal_output,
+ bool *release_arm_brake, control_loops::ArmStatus *status) {
+ ::Eigen::Matrix<double, 2, 1> Y;
+
+ Y << position->proximal.encoder + proximal_offset_,
+ position->distal.encoder + distal_offset_;
+
+ proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
+ distal_zeroing_estimator_.UpdateEstimate(position->distal);
+
+ if (proximal_output != nullptr) {
+ *proximal_output = 0.0;
+ }
+ if (distal_output != nullptr) {
+ *distal_output = 0.0;
+ }
+
+ arm_ekf_.Correct(Y, kDt());
+
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) <= 0.05 &&
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) <= 0.05) {
+ close_enough_for_full_power_ = true;
+ }
+ if (::std::abs(arm_ekf_.X_hat(0) - follower_.theta(0)) >= 1.10 ||
+ ::std::abs(arm_ekf_.X_hat(2) - follower_.theta(1)) >= 1.10) {
+ close_enough_for_full_power_ = false;
+ }
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ state_ = State::ZEROING;
+ proximal_zeroing_estimator_.Reset();
+ distal_zeroing_estimator_.Reset();
+ // TODO(austin): Go to the nearest node. For now, we are always going to
+ // go to node 0.
+ break;
+
+ case State::ZEROING:
+ // Zero by not moving.
+ if (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed()) {
+ state_ = State::RUNNING;
+
+ proximal_offset_ = proximal_zeroing_estimator_.offset();
+ distal_offset_ = distal_zeroing_estimator_.offset();
+
+ Y << position->proximal.encoder + proximal_offset_,
+ position->distal.encoder + distal_offset_;
+
+ // TODO(austin): Offset ekf rather than reset it. Since we aren't
+ // moving at this point, it's pretty safe to do this.
+ ::Eigen::Matrix<double, 4, 1> X;
+ X << Y(0), 0.0, Y(1), 0.0;
+ arm_ekf_.Reset(X);
+ } else {
+ break;
+ }
+
+ case State::RUNNING:
+ // ESTOP if we hit the hard limits.
+ // TODO(austin): Pick some sane limits.
+ if (proximal_zeroing_estimator_.error() ||
+ distal_zeroing_estimator_.error()) {
+ LOG(ERROR, "Zeroing error ESTOP\n");
+ state_ = State::ESTOP;
+ } else if (unsafe_goal != nullptr) {
+ if (!follower_.has_path()) {
+ // If we don't have a path and are far from the goal, don't update the
+ // path.
+ // TODO(austin): Once we get close to our first setpoint, crank the
+ // power back up. Otherwise we'll be weak at startup.
+ LOG(INFO, "No path.\n");
+ if (!close_enough_for_full_power_) {
+ break;
+ }
+ }
+ if (current_node_ != *unsafe_goal) {
+ LOG(INFO, "Goal is different\n");
+ if (*unsafe_goal >= search_graph_.num_vertexes()) {
+ LOG(ERROR, "goal out of range ESTOP\n");
+ state_ = State::ESTOP;
+ break;
+ }
+
+ if (follower_.path_distance_to_go() > 1e-3) {
+ LOG(INFO, " Distance to go %f\n", follower_.path_distance_to_go());
+ // Still on the old path segment. Can't change yet.
+ break;
+ }
+
+ search_graph_.SetGoal(*unsafe_goal);
+
+ size_t min_edge = 0;
+ double min_cost = -1.0;
+ for (const SearchGraph::HalfEdge &edge :
+ search_graph_.Neighbors(current_node_)) {
+ const double cost = search_graph_.GetCostToGoal(edge.dest);
+ if (min_cost == -1 || cost < min_cost) {
+ min_edge = edge.edge_id;
+ min_cost = cost;
+ }
+ }
+ // Ok, now we know which edge we are on. Figure out the path and
+ // trajectory.
+ const SearchGraph::Edge &next_edge = search_graph_.edges()[min_edge];
+ if (next_edge.start == current_node_) {
+ follower_.SwitchTrajectory(&trajectories_[min_edge].forwards);
+ current_node_ = next_edge.end;
+ } else {
+ follower_.SwitchTrajectory(&trajectories_[min_edge].backwards);
+ current_node_ = next_edge.start;
+ }
+ }
+ }
+ break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ break;
+ }
+
+ const bool disable =
+ ((proximal_output == nullptr) || (distal_output == nullptr) ||
+ (release_arm_brake == nullptr)) ||
+ state_ != State::RUNNING;
+ if (disable) {
+ close_enough_for_full_power_ = false;
+ }
+
+ follower_.Update(
+ arm_ekf_.X_hat(), disable, kDt(), kVMax(),
+ close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ LOG(INFO, "Max voltage: %f\n",
+ close_enough_for_full_power_ ? kOperatingVoltage() : kPathlessVMax());
+ status->goal_theta0 = follower_.theta(0);
+ status->goal_theta1 = follower_.theta(1);
+ status->goal_omega0 = follower_.omega(0);
+ status->goal_omega1 = follower_.omega(1);
+
+ status->theta0 = arm_ekf_.X_hat(0);
+ status->theta1 = arm_ekf_.X_hat(2);
+ status->omega0 = arm_ekf_.X_hat(1);
+ status->omega1 = arm_ekf_.X_hat(3);
+ status->voltage_error0 = arm_ekf_.X_hat(4);
+ status->voltage_error1 = arm_ekf_.X_hat(5);
+
+ if (!disable) {
+ *proximal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(0)));
+ *distal_output = ::std::max(
+ -kOperatingVoltage(), ::std::min(kOperatingVoltage(), follower_.U(1)));
+ *release_arm_brake = true;
+ }
+
+ status->proximal_estimator_state =
+ proximal_zeroing_estimator_.GetEstimatorState();
+ status->distal_estimator_state =
+ distal_zeroing_estimator_.GetEstimatorState();
+
+ status->path_distance_to_go = follower_.path_distance_to_go();
+ status->current_node = current_node_;
+
+ status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ status->estopped = (state_ == State::ESTOP);
+ status->state = static_cast<int32_t>(state_);
+ status->failed_solutions = follower_.failed_solutions();
+
+ arm_ekf_.Predict(follower_.U(), kDt());
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
new file mode 100644
index 0000000..d794c3e
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -0,0 +1,89 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+
+#include "frc971/zeroing/zeroing.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/graph.h"
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+struct TrajectoryPair {
+ TrajectoryPair(::std::unique_ptr<Path> forwards_path,
+ ::std::unique_ptr<Path> backwards_path, double step_size)
+ : forwards(::std::move(forwards_path), step_size),
+ backwards(::std::move(backwards_path), step_size) {}
+
+ Trajectory forwards;
+ Trajectory backwards;
+};
+
+class Arm {
+ public:
+ Arm();
+
+ // The operating voltage.
+ static constexpr double kOperatingVoltage() { return 12.0; }
+ static constexpr double kDt() { return 0.00505; }
+ static constexpr double kAlpha0Max() { return 25.0; }
+ static constexpr double kAlpha1Max() { return 25.0; }
+ static constexpr double kVMax() { return 11.0; }
+ static constexpr double kPathlessVMax() { return 4.0; }
+
+ void Iterate(const uint32_t *unsafe_goal,
+ const control_loops::ArmPosition *position,
+ double *proximal_output, double *distal_output,
+ bool *release_arm_brake,
+ control_loops::ArmStatus *status);
+
+ void Reset();
+
+ enum class State : int32_t {
+ UNINITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ State state_ = State::UNINITIALIZED;
+
+ ::aos::monotonic_clock::time_point last_time_ =
+ ::aos::monotonic_clock::min_time;
+
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator proximal_zeroing_estimator_;
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator distal_zeroing_estimator_;
+
+ double proximal_offset_ = 0.0;
+ double distal_offset_ = 0.0;
+
+ const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+
+ ::std::vector<TrajectoryPair> trajectories_;
+ SearchGraph search_graph_;
+
+ bool close_enough_for_full_power_ = false;
+
+ EKF arm_ekf_;
+ TrajectoryFollower follower_;
+
+ // Start at the 0th index.
+ uint32_t current_node_ = 0;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/y2018/control_loops/superstructure/arm/demo_path.cc
index bcdda8c..03856c9 100644
--- a/y2018/control_loops/superstructure/arm/demo_path.cc
+++ b/y2018/control_loops/superstructure/arm/demo_path.cc
@@ -2,6 +2,7 @@
#include <array>
#include <initializer_list>
+#include <memory>
namespace y2018 {
namespace control_loops {
@@ -25,8 +26,8 @@
}
-Path MakeDemoPath() {
- return Path(FlipPath(
+::std::unique_ptr<Path> MakeDemoPath() {
+ return ::std::unique_ptr<Path>(new Path(FlipPath(
{{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
-0.77079007974101643, -0.21483375398380378, -0.17756921128311187}},
{{1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
@@ -204,7 +205,11 @@
{{1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
-0.67932264404179699, -0.18184420373071258, 0.19643734513631667}},
{{0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
- -0.67025262732336799, -0.18446052990619061, 0.20424250843873848}}}));
+ -0.67025262732336799, -0.18446052990619061, 0.20424250843873848}}})));
+}
+
+::std::unique_ptr<Path> MakeReversedDemoPath() {
+ return ::std::unique_ptr<Path>(new Path(Path::Reversed(*MakeDemoPath())));
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
index ccf4276..0c18103 100644
--- a/y2018/control_loops/superstructure/arm/demo_path.h
+++ b/y2018/control_loops/superstructure/arm/demo_path.h
@@ -1,6 +1,8 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
+#include <memory>
+
#include "y2018/control_loops/superstructure/arm/trajectory.h"
namespace y2018 {
@@ -8,7 +10,8 @@
namespace superstructure {
namespace arm {
-Path MakeDemoPath();
+::std::unique_ptr<Path> MakeDemoPath();
+::std::unique_ptr<Path> MakeReversedDemoPath();
} // namespace arm
} // namespace superstructure
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
index 87d2178..02a2861 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ b/y2018/control_loops/superstructure/arm/dynamics.cc
@@ -1,5 +1,7 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
+DEFINE_bool(gravity, true, "If true, enable gravity.");
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
index d0194b0..3624f58 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ b/y2018/control_loops/superstructure/arm/dynamics.h
@@ -4,6 +4,9 @@
#include "Eigen/Dense"
#include "frc971/control_loops/runge_kutta.h"
+#include "third_party/gflags/include/gflags/gflags.h"
+
+DECLARE_bool(gravity);
namespace y2018 {
namespace control_loops {
@@ -36,7 +39,8 @@
static constexpr double kG2 = 90.0;
// MiniCIM motor constants.
- static constexpr double kStallTorque = 1.41;
+ static constexpr double kEfficiencyTweak = 0.95;
+ static constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
static constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
static constexpr double kStallCurrent = 89.0;
static constexpr double kResistance = 12.0 / kStallCurrent;
@@ -95,9 +99,10 @@
(::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
const ::Eigen::Matrix<double, 2, 1> torque = K3 * U - K4 * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque - K2 * velocity);
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
accel(1, 0))
@@ -122,9 +127,11 @@
(U +
(::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
K4 * velocity;
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque =
+ GravityTorque(X.block<4, 1>(0, 0));
const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque - K2 * velocity);
+ K1.inverse() * (torque + gravity_torque - K2 * velocity);
return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
accel(1, 0), 0.0, 0.0)
@@ -142,7 +149,21 @@
MatriciesForState(X, &K1, &K2);
- return K3_inverse * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
+ const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
+
+ return K3_inverse *
+ (K1 * alpha_t + K2 * omega_t + K4 * omega_t - gravity_torque);
+ }
+
+ static ::Eigen::Matrix<double, 2, 1> GravityTorque(
+ const ::Eigen::Matrix<double, 4, 1> &X) {
+ constexpr double kAccelDueToGravity = 9.8 * kEfficiencyTweak;
+ return (::Eigen::Matrix<double, 2, 1>() << (r1 * kM1 + kL1 * kM2) *
+ ::std::sin(X(0)) *
+ kAccelDueToGravity,
+ r2 * kM2 * ::std::sin(X(2)) * kAccelDueToGravity)
+ .finished() *
+ (FLAGS_gravity ? 1.0 : 0.0);
}
static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
index 065db72..23e5adc 100644
--- a/y2018/control_loops/superstructure/arm/dynamics_test.cc
+++ b/y2018/control_loops/superstructure/arm/dynamics_test.cc
@@ -23,6 +23,26 @@
::Eigen::Matrix<double, 4, 1>::Zero(),
::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
.isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ const ::Eigen::Matrix<double, 4, 1> X =
+ (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
+
+ ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ << ::std::endl;
+
+ ::std::cout << dynamics.UnboundedDiscreteDynamics(
+ X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ << ::std::endl;
+
+ EXPECT_TRUE(dynamics
+ .UnboundedDiscreteDynamics(
+ X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero()),
+ 0.01)
+ .isApprox(X));
}
} // namespace testing
diff --git a/y2018/control_loops/superstructure/arm/ekf.cc b/y2018/control_loops/superstructure/arm/ekf.cc
index 93c88d4..48a71f4 100644
--- a/y2018/control_loops/superstructure/arm/ekf.cc
+++ b/y2018/control_loops/superstructure/arm/ekf.cc
@@ -6,6 +6,11 @@
#include "frc971/control_loops/jacobian.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
+DEFINE_double(proximal_voltage_error_uncertainty, 8.0,
+ "Proximal joint voltage error uncertainty.");
+DEFINE_double(distal_voltage_error_uncertainty, 2.0,
+ "Distal joint voltage error uncertainty.");
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -15,17 +20,26 @@
// TODO(austin): When tuning this, make sure to verify that you are waiting
// enough cycles to make sure it converges at startup. Otherwise you will have a
// bad day.
-const ::Eigen::Matrix<double, 6, 6> Q_covariance(
+::Eigen::Matrix<double, 6, 6> Q_covariance(
(::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
- ::std::pow(0.80, 2), ::std::pow(0.70, 2))
+ ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
+ ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
.finished()
.asDiagonal());
} // namespace
EKF::EKF() {
X_hat_.setZero();
+ Q_covariance =
+ ((::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
+ ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
+ ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
+ ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
+ .finished()
+ .asDiagonal());
P_ = Q_covariance;
+ P_reset_ = P_;
//::std::cout << "Reset P: " << P_ << ::std::endl;
// TODO(austin): Running the EKF 2000 cycles works, but isn't super clever.
// We could just solve the DARE.
@@ -34,20 +48,21 @@
Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
}
+ P_half_converged_ = P_;
//::std::cout << "Stabilized P: " << P_ << ::std::endl;
for (int i = 0; i < 1000; ++i) {
Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
}
//::std::cout << "Really stabilized P: " << P_ << ::std::endl;
- P_reset_ = P_;
+ P_converged_ = P_;
Reset(::Eigen::Matrix<double, 4, 1>::Zero());
}
void EKF::Reset(const ::Eigen::Matrix<double, 4, 1> &X) {
X_hat_.setZero();
- P_ = P_reset_;
+ P_ = P_converged_;
X_hat_.block<4, 1>(0, 0) = X;
}
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/y2018/control_loops/superstructure/arm/ekf.h
index 9f45303..9ae8b47 100644
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ b/y2018/control_loops/superstructure/arm/ekf.h
@@ -29,10 +29,19 @@
const ::Eigen::Matrix<double, 6, 1> &X_hat() const { return X_hat_; }
double X_hat(int i) const { return X_hat_(i); }
const ::Eigen::Matrix<double, 6, 6> &P() const { return P_; }
+ const ::Eigen::Matrix<double, 6, 6> &P_reset() const { return P_reset_; }
+ const ::Eigen::Matrix<double, 6, 6> &P_half_converged() const {
+ return P_half_converged_;
+ }
+ const ::Eigen::Matrix<double, 6, 6> &P_converged() const {
+ return P_converged_;
+ }
private:
::Eigen::Matrix<double, 6, 1> X_hat_;
::Eigen::Matrix<double, 6, 6> P_;
+ ::Eigen::Matrix<double, 6, 6> P_half_converged_;
+ ::Eigen::Matrix<double, 6, 6> P_converged_;
::Eigen::Matrix<double, 6, 6> P_reset_;
};
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
index a1e6fb2..6544a8e 100644
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ b/y2018/control_loops/superstructure/arm/graph.cc
@@ -8,11 +8,14 @@
namespace superstructure {
namespace arm {
-SearchGraph::SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges)
- : edges_(edges) {
+SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
+ : SearchGraph(num_vertexes, ::std::move(::std::vector<Edge>(edges))) {}
+
+SearchGraph::SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges)
+ : edges_(::std::move(edges)) {
vertexes_.resize(num_vertexes);
size_t i = 0;
- for (const auto &edge : edges) {
+ for (const auto &edge : edges_) {
assert(edge.start < num_vertexes);
assert(edge.end < num_vertexes);
assert(edge.start != edge.end);
@@ -22,15 +25,19 @@
++i;
}
for (auto &vertex : vertexes_) {
- std::sort(vertex.forward.begin(), vertex.forward.end());
- std::sort(vertex.reverse.begin(), vertex.reverse.end());
+ ::std::sort(vertex.forward.begin(), vertex.forward.end());
+ ::std::sort(vertex.reverse.begin(), vertex.reverse.end());
}
vertex_heap_.Reserve(num_vertexes);
+ SetGoal(0);
}
SearchGraph::~SearchGraph() {}
void SearchGraph::SetGoal(size_t vertex) {
+ if (last_searched_vertex_ == vertex) {
+ return;
+ }
assert(vertex < vertexes_.size());
vertex_heap_.Clear();
@@ -52,6 +59,7 @@
}
}
}
+ last_searched_vertex_ = vertex;
}
double SearchGraph::GetCostToGoal(size_t vertex) {
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/y2018/control_loops/superstructure/arm/graph.h
index 1c2f9e5..d295cfc 100644
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ b/y2018/control_loops/superstructure/arm/graph.h
@@ -33,7 +33,7 @@
public:
QueueEntry(T *value) : value_(value) { value_->entry_ = this; }
- QueueEntry(QueueEntry &&o) : value_(o.value_) {
+ QueueEntry(QueueEntry &&o) : value_(::std::move(o.value_)) {
if (value_) {
value_->entry_ = this;
}
@@ -133,7 +133,14 @@
bool operator<(const HalfEdge &o) const { return dest < o.dest; }
};
- SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges);
+ SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges);
+ SearchGraph(size_t num_vertexes, ::std::initializer_list<Edge> edges);
+ SearchGraph(SearchGraph &&o)
+ : goal_vertex_(o.goal_vertex_),
+ vertexes_(::std::move(o.vertexes_)),
+ edges_(::std::move(o.edges_)),
+ vertex_heap_(::std::move(o.vertex_heap_)) {}
+
~SearchGraph();
// Set the goal vertex.
@@ -150,6 +157,8 @@
size_t num_vertexes() const { return vertexes_.size(); }
+ const std::vector<Edge> &edges() const { return edges_; }
+
private:
size_t goal_vertex_ = std::numeric_limits<size_t>::max();
struct Vertex {
@@ -169,6 +178,8 @@
std::vector<Edge> edges_;
IntrusivePriorityQueue<Vertex> vertex_heap_;
+
+ size_t last_searched_vertex_ = 1;
};
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/y2018/control_loops/superstructure/arm/graph_test.cc
index 1f04be2..8f246a5 100644
--- a/y2018/control_loops/superstructure/arm/graph_test.cc
+++ b/y2018/control_loops/superstructure/arm/graph_test.cc
@@ -14,12 +14,7 @@
void SetCost(double cost) { cost_ = cost; }
- bool operator<(const HeapTest &o) {
- bool value = cost_ > o.cost_;
- // printf("compare: %zu - %g < %zu %g -> %s\n", id_, cost_, o.id_, o.cost_,
- // value ? "true" : "false");
- return value;
- }
+ bool operator<(const HeapTest &o) { return cost_ > o.cost_; }
private:
size_t id_;
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index 80fca9c..f7746cb 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -1,15 +1,37 @@
#include "y2018/control_loops/superstructure/arm/trajectory.h"
#include "Eigen/Dense"
+#include "aos/common/logging/logging.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/jacobian.h"
+#include "third_party/gflags/include/gflags/gflags.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
+DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
+DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
+DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
namespace arm {
+Path Path::Reversed(const Path &p) {
+ ::std::vector<::std::array<double, 6>> list(p.distances().size());
+ for (ssize_t i = list.size() - 1; i >= 0; --i) {
+ list[i][0] = p.thetas_[list.size() - 1 - i][0];
+ list[i][1] = p.thetas_[list.size() - 1 - i][1];
+
+ list[i][2] = -p.omegas_[list.size() - 1 - i][0];
+ list[i][3] = -p.omegas_[list.size() - 1 - i][1];
+
+ list[i][4] = p.alphas_[list.size() - 1 - i][0];
+ list[i][5] = p.alphas_[list.size() - 1 - i][1];
+ }
+ return Path(list);
+}
+
Path::Path(::std::vector<::std::array<double, 6>> list) {
distances_.reserve(list.size());
::Eigen::Matrix<double, 2, 1> last_theta;
@@ -63,22 +85,25 @@
}
::std::vector<double> Trajectory::CurvatureOptimizationPass(
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax) {
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
::std::vector<double> max_dvelocity_unfiltered;
max_dvelocity_unfiltered.reserve(num_plan_points_);
for (size_t i = 0; i < num_plan_points_; ++i) {
const double distance = DistanceForIndex(i);
// TODO(austin): We are looking up the index in the path 3 times here.
- const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path_->Alpha(distance);
+ const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(distance);
+ const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(distance);
+ const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(distance);
const ::Eigen::Matrix<double, 4, 1> X =
(::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
.finished();
::Eigen::Matrix<double, 2, 2> K1;
::Eigen::Matrix<double, 2, 2> K2;
+ const ::Eigen::Matrix<double, 2, 1> gravity_volts =
+ Dynamics::K3_inverse * Dynamics::GravityTorque(X);
+
Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
const ::Eigen::Matrix<double, 2, 2> omega_square =
@@ -90,23 +115,26 @@
// Normalize so that the max accel is 1, and take magnitudes. This
// gives us the max velocity we can be at each point due to
// curvature.
+ double min_good_ddot =
+ ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
+
const ::Eigen::Matrix<double, 2, 1> vk1 =
Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
const ::Eigen::Matrix<double, 2, 1> vk2 =
Dynamics::K3_inverse * Dynamics::K4 * omega;
- double min_good_ddot =
- ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
-
- // Loop through all the various vmin, vmax combinations.
- for (const double c : {-vmax, vmax}) {
+ // Loop through all the various vmin, plan_vmax combinations.
+ for (const double c : {-plan_vmax, plan_vmax}) {
// Also loop through saturating theta0 and theta1
- for (const ::std::pair<double, double> ab :
- {::std::pair<double, double>{vk1(0, 0), vk2(0, 0)},
- ::std::pair<double, double>{vk1(1, 0), vk2(1, 0)}}) {
- const double a = ab.first;
- const double b = ab.second;
- const double sqrt_number = b * b - 4.0 * a * c;
+ for (const ::std::tuple<double, double, double> abgravity :
+ {::std::tuple<double, double, double>{vk1(0), vk2(0),
+ gravity_volts(0)},
+ ::std::tuple<double, double, double>{vk1(1), vk2(1),
+ gravity_volts(1)}}) {
+ const double a = ::std::get<0>(abgravity);
+ const double b = ::std::get<1>(abgravity);
+ const double gravity = ::std::get<2>(abgravity);
+ const double sqrt_number = b * b - 4.0 * a * (c - gravity);
// Throw out imaginary solutions to the quadratic.
if (sqrt_number > 0) {
const double sqrt_result = ::std::sqrt(sqrt_number);
@@ -115,9 +143,10 @@
// Loop through both solutions.
for (const double ddot : {ddot1, ddot2}) {
const ::Eigen::Matrix<double, 2, 1> U =
- vk1 * ddot * ddot + vk2 * ddot;
+ vk1 * ddot * ddot + vk2 * ddot - gravity_volts;
+
// Finally, make sure the velocity is positive and valid.
- if ((U.array().abs() <= vmax + 1e-6).all() && ddot > 0.0) {
+ if ((U.array().abs() <= plan_vmax + 1e-6).all() && ddot > 0.0) {
min_good_ddot = ::std::min(min_good_ddot, ddot);
}
}
@@ -131,12 +160,12 @@
}
double Trajectory::FeasableForwardsAcceleration(
- double goal_distance, double goal_velocity, double vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) {
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
// TODO(austin): We are looking up the index in the path 3 times here.
- const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path_->Alpha(goal_distance);
+ const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
+ const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
const ::Eigen::Matrix<double, 4, 1> X =
(::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
@@ -152,20 +181,22 @@
.finished();
const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse * ((K1 * alpha + K2 * omega_square * omega) *
- goal_velocity * goal_velocity +
- Dynamics::K4 * omega * goal_velocity);
+ Dynamics::K3_inverse *
+ ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
+ goal_velocity +
+ Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
const ::Eigen::Matrix<double, 2, 1> k_scalar =
Dynamics::K3_inverse * K1 * omega;
- double goal_acceleration =
+ const double constraint_goal_acceleration =
::std::sqrt(
::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
goal_velocity * goal_velocity,
2))) /
(alpha_unitizer * omega).norm();
- for (double c : {-vmax, vmax}) {
+ double goal_acceleration = -::std::numeric_limits<double>::infinity();
+ for (double c : {-plan_vmax, plan_vmax}) {
for (const ::std::pair<double, double> ab :
{::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
@@ -175,70 +206,84 @@
const ::Eigen::Matrix<double, 2, 1> U =
k_constant + k_scalar * voltage_accel;
- if (voltage_accel > 0.0 && (U.array().abs() <= vmax + 1e-6).all())
- goal_acceleration = ::std::min(voltage_accel, goal_acceleration);
+ if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+ goal_acceleration = ::std::max(voltage_accel, goal_acceleration);
+ }
}
}
+ if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+ // TODO(austin): The math above doesn't always give a valid solution.
+ // Figure out why. It should be an affine transformation.
+ return constraint_goal_acceleration;
+ }
- return goal_acceleration;
+ return ::std::min(constraint_goal_acceleration, goal_acceleration);
}
double Trajectory::FeasableBackwardsAcceleration(
- double goal_distance, double goal_velocity, double vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) {
- const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path_->Alpha(goal_distance);
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
- .finished();
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
+ const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
+ const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
+ const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
+ const ::Eigen::Matrix<double, 4, 1> X =
+ (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+ .finished();
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
+ Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
- const ::Eigen::Matrix<double, 2, 2> omega_square =
- (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
- .finished();
+ const ::Eigen::Matrix<double, 2, 2> omega_square =
+ (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+ .finished();
- const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
- ((K1 * alpha + K2 * omega_square * omega) * goal_velocity * goal_velocity +
- Dynamics::K4 * omega * goal_velocity);
- const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
+ const ::Eigen::Matrix<double, 2, 1> k_constant =
+ Dynamics::K3_inverse *
+ ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
+ goal_velocity +
+ Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
+ const ::Eigen::Matrix<double, 2, 1> k_scalar =
+ Dynamics::K3_inverse * K1 * omega;
- double goal_acceleration =
- ::std::sqrt(
- ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
- goal_velocity * goal_velocity),
- 2))) /
- (alpha_unitizer * omega).norm();
- for (double c : {-vmax, vmax}) {
- for (const ::std::pair<double, double> ab :
- {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
- ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
- const double a = ab.first;
- const double b = ab.second;
- // This time, we are doing the other pass. So, find all the
- // decelerations (and flip them) to find the prior velocity.
- const double voltage_accel = (c - a) / b;
+ const double constraint_goal_acceleration =
+ ::std::sqrt(
+ ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
+ goal_velocity * goal_velocity),
+ 2))) /
+ (alpha_unitizer * omega).norm();
- const ::Eigen::Matrix<double, 2, 1> U =
- k_constant + k_scalar * voltage_accel;
+ double goal_acceleration = -::std::numeric_limits<double>::infinity();
+ for (double c : {-plan_vmax, plan_vmax}) {
+ for (const ::std::pair<double, double> ab :
+ {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
+ ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
+ const double a = ab.first;
+ const double b = ab.second;
+ // This time, we are doing the other pass. So, find all the
+ // decelerations (and flip them) to find the prior velocity.
+ const double voltage_accel = (c - a) / b;
- if (voltage_accel < 0.0 && (U.array().abs() <= vmax + 1e-6).all()) {
- goal_acceleration = ::std::min(-voltage_accel, goal_acceleration);
- }
+ const ::Eigen::Matrix<double, 2, 1> U =
+ k_constant + k_scalar * voltage_accel;
+
+ // TODO(austin): This doesn't always give a valid solution. It really
+ // should. Figure out why.
+ if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+ goal_acceleration = ::std::max(-voltage_accel, goal_acceleration);
}
}
+ }
+ if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+ return constraint_goal_acceleration;
+ }
- return goal_acceleration;
+ return ::std::min(goal_acceleration, constraint_goal_acceleration);
}
::std::vector<double> Trajectory::BackwardsOptimizationPass(
const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax) {
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
// Now, iterate over the list of velocities and constrain the acceleration.
@@ -254,7 +299,7 @@
for (int j = 0; j < kNumSteps; ++j) {
// Compute the acceleration with respect to time.
const double acceleration = FeasableBackwardsAcceleration(
- prev_distance - integrated_distance, integrated_velocity, vmax,
+ prev_distance - integrated_distance, integrated_velocity, plan_vmax,
alpha_unitizer);
// And then integrate it with respect to distance.
@@ -274,7 +319,7 @@
::std::vector<double> Trajectory::ForwardsOptimizationPass(
const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax) {
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
::std::vector<double> max_dvelocity_forward_pass = max_dvelocity;
// Now, iterate over the list of velocities and constrain the acceleration.
for (size_t i = 1; i < num_plan_points_; ++i) {
@@ -289,8 +334,8 @@
for (int j = 0; j < kNumSteps; ++j) {
// Compute the acceleration with respect to time.
const double acceleration = FeasableForwardsAcceleration(
- previous_distance + integrated_distance, integrated_velocity, vmax,
- alpha_unitizer);
+ previous_distance + integrated_distance, integrated_velocity,
+ plan_vmax, alpha_unitizer);
// And then integrate it with respect to distance.
const double integration_step_size_distance =
@@ -310,20 +355,27 @@
void TrajectoryFollower::Reset() {
next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
- U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+ U_unsaturated_ = U_ff_ = U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
goal_acceleration_ = 0.0;
+ saturation_fraction_along_path_ = 0.0;
+ omega_.setZero();
+ if (trajectory_ != nullptr) {
+ theta_ = trajectory_->ThetaT(goal_(0));
+ }
}
::Eigen::Matrix<double, 2, 6> TrajectoryFollower::K_at_state(
const ::Eigen::Matrix<double, 6, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) {
- constexpr double q_pos = 0.2;
- constexpr double q_vel = 4.0;
+ const double kProximalPos = FLAGS_lqr_proximal_pos;
+ const double kProximalVel = FLAGS_lqr_proximal_vel;
+ const double kDistalPos = FLAGS_lqr_distal_pos;
+ const double kDistalVel = FLAGS_lqr_distal_vel;
const ::Eigen::DiagonalMatrix<double, 4> Q =
(::Eigen::DiagonalMatrix<double, 4>().diagonal()
- << 1.0 / ::std::pow(q_pos, 2),
- 1.0 / ::std::pow(q_vel, 2), 1.0 / ::std::pow(q_pos, 2),
- 1.0 / ::std::pow(q_vel, 2))
+ << 1.0 / ::std::pow(kProximalPos, 2),
+ 1.0 / ::std::pow(kProximalVel, 2), 1.0 / ::std::pow(kDistalPos, 2),
+ 1.0 / ::std::pow(kDistalVel, 2))
.finished()
.asDiagonal();
@@ -343,25 +395,32 @@
::Eigen::Matrix<double, 4, 4> S;
::Eigen::Matrix<double, 2, 4> sub_K;
- ::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S);
+ if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
+ ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+ final_A - final_B * sub_K);
- ::Eigen::Matrix<double, 2, 6> K;
- K.setZero();
- K.block<2, 4>(0, 0) = sub_K;
- K(0, 4) = 1.0;
- K(1, 5) = 1.0;
- return K;
+ ::Eigen::Matrix<double, 2, 6> K;
+ K.setZero();
+ K.block<2, 4>(0, 0) = sub_K;
+ K(0, 4) = 1.0;
+ K(1, 5) = 1.0;
+ failed_solutions_ = 0;
+ last_K_ = K;
+ } else {
+ ++failed_solutions_;
+ }
+ return last_K_;
}
void TrajectoryFollower::USaturationSearch(
double goal_distance, double last_goal_distance, double goal_velocity,
- double last_goal_velocity, double fraction_along_path,
+ double last_goal_velocity, double saturation_fraction_along_path,
const ::Eigen::Matrix<double, 2, 6> &K,
const ::Eigen::Matrix<double, 6, 1> &X, const Trajectory &trajectory,
::Eigen::Matrix<double, 2, 1> *U, double *saturation_goal_velocity,
double *saturation_goal_acceleration) {
double saturation_goal_distance =
- ((goal_distance - last_goal_distance) * fraction_along_path +
+ ((goal_distance - last_goal_distance) * saturation_fraction_along_path +
last_goal_distance);
const ::Eigen::Matrix<double, 2, 1> theta_t =
@@ -384,14 +443,14 @@
}
::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
- const ::Eigen::Matrix<double, 2, 1> &goal, double vmax, double dt) {
+ const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt) {
// Figure out where we would be if we were to accelerate as fast as
// our constraints allow.
::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
- [this, &vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
+ [this, &plan_vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
return (::Eigen::Matrix<double, 2, 1>() << X(1),
- trajectory_->FeasableForwardsAcceleration(X(0), X(1), vmax,
- alpha_unitizer_))
+ trajectory_->FeasableForwardsAcceleration(
+ X(0), X(1), plan_vmax, trajectory_->alpha_unitizer()))
.finished();
},
goal, dt);
@@ -412,30 +471,76 @@
}
void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 6, 1> &X,
- double dt, double vmax) {
+ bool disabled, double dt, double plan_vmax,
+ double voltage_limit) {
+ last_goal_ = goal_;
+
+ if (!has_path()) {
+ // No path, so go to the last theta (no velocity).
+ last_goal_.setZero();
+ next_goal_.setZero();
+ goal_.setZero();
+ goal_acceleration_ = 0.0;
+ saturation_fraction_along_path_ = 0.0;
+
+ if (disabled) {
+ U_ff_.setZero();
+ U_.setZero();
+ U_unsaturated_.setZero();
+ LOG(INFO, "Disabled\n");
+ } else {
+ const ::Eigen::Matrix<double, 6, 1> R =
+ trajectory_->R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
+
+ U_ff_ = Dynamics::FF_U(X.block<4, 1>(0, 0),
+ ::Eigen::Matrix<double, 2, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero());
+ const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
+ U_ = U_unsaturated_ = U_ff_ + K * (R - X);
+
+ U_ = U_.array().max(-voltage_limit).min(voltage_limit);
+ }
+ return;
+ }
+
+ if (disabled) {
+ // If we are disabled, it's likely for a bit of time. So, lets freeze
+ // ourselves on the path (accept the previous motion, but then zero out the
+ // velocity). Set all outputs to 0 as well.
+ next_goal_(1) = 0.0;
+ goal_ = next_goal_;
+ goal_acceleration_ = 0.0;
+ U_unsaturated_.setZero();
+ U_.setZero();
+ U_ff_.setZero();
+ saturation_fraction_along_path_ = 1.0;
+ theta_ = trajectory_->ThetaT(goal_(0));
+ omega_.setZero();
+ return;
+ }
+
// To avoid exposing the new goals before the outer code has a chance to
// querry the internal state, move to the new goals here.
- last_goal_ = goal_;
goal_ = next_goal_;
- if (::std::abs(goal_(0) - path_->length()) < 1e-2) {
+ if (::std::abs(goal_(0) - path()->length()) < 1e-2) {
// If we go backwards along the path near the goal, snap us to the end
// point or we'll never actually finish.
if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
- goal_(0) > path_->length()) {
- goal_(0) = path_->length();
+ goal_(0) > path()->length()) {
+ goal_(0) = path()->length();
goal_(1) = 0.0;
}
}
- if (goal_(0) == path_->length()) {
+ if (goal_(0) == path()->length()) {
next_goal_(0) = goal_(0);
next_goal_(1) = 0.0;
goal_acceleration_ = 0.0;
} else {
// Figure out where we would be if we were to accelerate as fast as
// our constraints allow.
- next_goal_ = PlanNextGoal(goal_, vmax, dt);
+ next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
goal_acceleration_ = trajectory_->InterpolateAcceleration(
goal_(0), next_goal_(0), goal_(1), next_goal_(1));
@@ -457,18 +562,18 @@
// Ok, now we know if we are staturated or not. If we are, time to search
// between here and our previous goal either until we find a state where we
// aren't saturated, or we are really close to our starting point.
- fraction_along_path_ = 1.0;
- if ((U_.array().abs() > 12.0).any()) {
+ saturation_fraction_along_path_ = 1.0;
+ if ((U_.array().abs() > voltage_limit).any()) {
// Saturated. Let's do a binary search.
double step_size;
if ((goal_(0) - last_goal_(0)) < 1e-8) {
// print "Not bothering to move"
// Avoid the divide by 0 when interpolating. Just don't move since we
// are saturated.
- fraction_along_path_ = 0.0;
+ saturation_fraction_along_path_ = 0.0;
step_size = 0.0;
} else {
- fraction_along_path_ = 0.5;
+ saturation_fraction_along_path_ = 0.5;
step_size = 0.5;
}
@@ -477,28 +582,30 @@
double saturation_goal_acceleration;
while (step_size > 0.01) {
USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
- fraction_along_path_, K, X, *trajectory_, &U_,
- &saturation_goal_velocity,
+ saturation_fraction_along_path_, K, X, *trajectory_,
+ &U_, &saturation_goal_velocity,
&saturation_goal_acceleration);
step_size = step_size * 0.5;
- if ((U_.array().abs() > 12.0).any()) {
- fraction_along_path_ -= step_size;
+ if ((U_.array().abs() > voltage_limit).any()) {
+ saturation_fraction_along_path_ -= step_size;
} else {
- fraction_along_path_ += step_size;
+ saturation_fraction_along_path_ += step_size;
}
}
- goal_(0) =
- ((goal_(0) - last_goal_(0)) * fraction_along_path_ + last_goal_(0));
+ goal_(0) = ((goal_(0) - last_goal_(0)) * saturation_fraction_along_path_ +
+ last_goal_(0));
goal_(1) = saturation_goal_velocity;
- next_goal_ = PlanNextGoal(goal_, vmax, dt);
+ next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
goal_acceleration_ = trajectory_->InterpolateAcceleration(
goal_(0), next_goal_(0), goal_(1), next_goal_(1));
- U_ = U_.array().max(-12.0).min(12.0);
+ U_ = U_.array().max(-voltage_limit).min(voltage_limit);
}
+ theta_ = trajectory_->ThetaT(goal_(0));
+ omega_ = trajectory_->OmegaT(goal_(0), goal_(1));
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index b699fe0..d9e918d 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -3,6 +3,7 @@
#include <array>
#include <initializer_list>
+#include <memory>
#include <vector>
#include "Eigen/Dense"
@@ -24,6 +25,8 @@
// doesn't want that.
Path(::std::vector<::std::array<double, 6>> list);
+ static Path Reversed(const Path &p);
+
// Returns the length of the path in radians.
double length() const { return distances_.back(); }
@@ -86,28 +89,36 @@
public:
// Constructs a trajectory (but doesn't calculate it) given a path and a step
// size.
- Trajectory(Path *path, double gridsize)
- : path_(path),
+ Trajectory(::std::unique_ptr<const Path> path, double gridsize)
+ : path_(::std::move(path)),
num_plan_points_(
static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
step_size_(path_->length() /
- static_cast<double>(num_plan_points_ - 1)) {}
+ static_cast<double>(num_plan_points_ - 1)) {
+ alpha_unitizer_.setZero();
+ }
// Optimizes the trajectory. The path will adhere to the constraints that
// || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
- // vmax.
+ // plan_vmax.
void OptimizeTrajectory(const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,
- double vmax) {
- max_dvelocity_unfiltered_ = CurvatureOptimizationPass(alpha_unitizer, vmax);
+ double plan_vmax) {
+ if (path_ == nullptr) {
+ abort();
+ }
+ alpha_unitizer_ = alpha_unitizer;
+
+ max_dvelocity_unfiltered_ =
+ CurvatureOptimizationPass(alpha_unitizer, plan_vmax);
// We need to start and end the trajectory with 0 velocity.
max_dvelocity_unfiltered_[0] = 0.0;
max_dvelocity_unfiltered_[max_dvelocity_unfiltered_.size() - 1] = 0.0;
max_dvelocity_ = BackwardsOptimizationPass(max_dvelocity_unfiltered_,
- alpha_unitizer, vmax);
- max_dvelocity_forward_pass_ = ForwardsOptimizationPass(
- max_dvelocity_, alpha_unitizer, vmax);
+ alpha_unitizer, plan_vmax);
+ max_dvelocity_forward_pass_ =
+ ForwardsOptimizationPass(max_dvelocity_, alpha_unitizer, plan_vmax);
}
// Returns an array of the distances used in the plan. The starting point
@@ -123,37 +134,37 @@
// Computes the maximum velocity that we can follow the path while adhering to
// the constraints that || angular acceleration * alpha_unitizer || < 1, and
- // the applied voltage < vmax. Returns the velocities.
+ // the applied voltage < plan_vmax. Returns the velocities.
::std::vector<double> CurvatureOptimizationPass(
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax);
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
// Computes the maximum forwards feasable acceleration at a given position
- // while adhering to the vmax and alpha_unitizer constraints.
+ // while adhering to the plan_vmax and alpha_unitizer constraints.
// This gives us the maximum path distance acceleration (d^2d/dt^2) for any
// initial position and velocity for the forwards path.
double FeasableForwardsAcceleration(
- double goal_distance, double goal_velocity, double vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer);
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
// Computes the maximum backwards feasable acceleration at a given position
- // while adhering to the vmax and alpha_unitizer constraints.
+ // while adhering to the plan_vmax and alpha_unitizer constraints.
// This gives us the maximum path distance acceleration (d^2d/dt^2) for any
// initial position and velocity for the backwards path.
// Note: positive acceleration means speed up while going in the negative
// direction on the path.
double FeasableBackwardsAcceleration(
- double goal_distance, double goal_velocity, double vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer);
+ double goal_distance, double goal_velocity, double plan_vmax,
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
// Executes the backwards path optimization pass.
::std::vector<double> BackwardsOptimizationPass(
const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax);
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
// Executes the forwards path optimization pass.
::std::vector<double> ForwardsOptimizationPass(
const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax);
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
// Returns the number of points used for the plan.
size_t num_plan_points() const { return num_plan_points_; }
@@ -191,10 +202,13 @@
// Computes the path distance velocity of the plan as a function of the
// distance.
- double GetDVelocity(double d) {
+ double GetDVelocity(double distance) const {
+ return GetDVelocity(distance, max_dvelocity_);
+ }
+ double GetDVelocity(double d, const ::std::vector<double> &plan) const {
::std::pair<size_t, size_t> indices = IndicesForDistance(d);
- const double v0 = max_dvelocity_[indices.first];
- const double v1 = max_dvelocity_[indices.second];
+ const double v0 = plan[indices.first];
+ const double v1 = plan[indices.second];
const double d0 = DistanceForIndex(indices.first);
const double d1 = DistanceForIndex(indices.second);
return InterpolateVelocity(d, d0, d1, v0, v1);
@@ -203,9 +217,12 @@
// Computes the path distance acceleration of the plan as a function of the
// distance.
double GetDAcceleration(double distance) const {
+ return GetDAcceleration(distance, max_dvelocity_);
+ }
+ double GetDAcceleration(double distance, const ::std::vector<double> &plan) const {
::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
- const double v0 = max_dvelocity_[indices.first];
- const double v1 = max_dvelocity_[indices.second];
+ const double v0 = plan[indices.first];
+ const double v1 = plan[indices.second];
const double d0 = DistanceForIndex(indices.first);
const double d1 = DistanceForIndex(indices.second);
return InterpolateAcceleration(d0, d1, v0, v1);
@@ -252,6 +269,12 @@
.finished();
}
+ const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer() const {
+ return alpha_unitizer_;
+ }
+
+ const Path &path() const { return *path_; }
+
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
@@ -275,7 +298,7 @@
}
// The path to follow.
- const Path *path_ = nullptr;
+ ::std::unique_ptr<const Path> path_;
// The number of points in the plan.
const size_t num_plan_points_;
// A cached version of the step size since we need this a *lot*.
@@ -284,25 +307,40 @@
::std::vector<double> max_dvelocity_unfiltered_;
::std::vector<double> max_dvelocity_;
::std::vector<double> max_dvelocity_forward_pass_;
+
+ ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
};
// This class tracks the current goal along trajectories and paths.
class TrajectoryFollower {
public:
- TrajectoryFollower(Path *const path, Trajectory *const trajectory,
- const ::Eigen::Matrix<double, 2, 2> alpha_unitizer)
- : path_(path), trajectory_(trajectory), alpha_unitizer_(alpha_unitizer) {
+ TrajectoryFollower(const ::Eigen::Matrix<double, 2, 1> &theta)
+ : trajectory_(nullptr), theta_(theta) {
+ omega_.setZero();
+ last_K_.setZero();
Reset();
}
+ TrajectoryFollower(Trajectory *const trajectory) : trajectory_(trajectory) {
+ last_K_.setZero();
+ Reset();
+ }
+
+ bool has_path() const { return trajectory_ != nullptr; }
+
// Returns the goal distance along the path.
const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
double goal(int i) const { return goal_(i); }
// Starts over at the beginning of the path.
- // TODO(austin): Allow switching the path.
void Reset();
+ // Switches paths and starts at the beginning of the path.
+ void SwitchTrajectory(const Trajectory *trajectory) {
+ trajectory_ = trajectory;
+ Reset();
+ }
+
// Returns the controller gain at the provided state.
::Eigen::Matrix<double, 2, 6> K_at_state(
const ::Eigen::Matrix<double, 6, 1> &X,
@@ -312,7 +350,7 @@
// along the path.
void USaturationSearch(double goal_distance, double last_goal_distance,
double goal_velocity, double last_goal_velocity,
- double fraction_along_path,
+ double saturation_fraction_along_path,
const ::Eigen::Matrix<double, 2, 6> &K,
const ::Eigen::Matrix<double, 6, 1> &X,
const Trajectory &trajectory,
@@ -320,32 +358,52 @@
double *saturation_goal_velocity,
double *saturation_goal_acceleration);
- // Returns the next goal given a planning vmax and timestep. This ignores the
+ // Returns the next goal given a planning plan_vmax and timestep. This
+ // ignores the
// backwards pass.
::Eigen::Matrix<double, 2, 1> PlanNextGoal(
- const ::Eigen::Matrix<double, 2, 1> &goal, double vmax, double dt);
+ const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt);
// Plans the next cycle and updates the internal state for consumption.
- void Update(const ::Eigen::Matrix<double, 6, 1> &X, double dt, double vmax);
+ void Update(const ::Eigen::Matrix<double, 6, 1> &X, bool disabled, double dt,
+ double plan_vmax, double voltage_limit);
// Returns the goal acceleration for this cycle.
double goal_acceleration() const { return goal_acceleration_; }
// Returns U(s) for this cycle.
const ::Eigen::Matrix<double, 2, 1> &U() const { return U_; }
+ double U(int i) const { return U_(i); }
const ::Eigen::Matrix<double, 2, 1> &U_unsaturated() const {
return U_unsaturated_;
}
const ::Eigen::Matrix<double, 2, 1> &U_ff() const { return U_ff_; }
- double fraction_along_path() const { return fraction_along_path_; }
+ double saturation_fraction_along_path() const {
+ return saturation_fraction_along_path_;
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> &theta() const { return theta_; }
+ double theta(int i) const { return theta_(i); }
+ const ::Eigen::Matrix<double, 2, 1> &omega() const { return omega_; }
+ double omega(int i) const { return omega_(i); }
+
+ // Distance left on the path before we get to the end of the path.
+ double path_distance_to_go() const {
+ if (has_path()) {
+ return ::std::max(0.0, path()->length() - goal_(0));
+ } else {
+ return 0.0;
+ }
+ }
+
+ const Path *path() const { return &trajectory_->path(); }
+
+ int failed_solutions() const { return failed_solutions_; }
private:
- // The path to follow.
- Path *const path_ = nullptr;
// The trajectory plan.
- Trajectory *const trajectory_ = nullptr;
- const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+ const Trajectory *trajectory_ = nullptr;
// The current goal.
::Eigen::Matrix<double, 2, 1> goal_;
@@ -359,7 +417,14 @@
::Eigen::Matrix<double, 2, 1> U_ff_;
double goal_acceleration_ = 0.0;
- double fraction_along_path_ = 1.0;
+ double saturation_fraction_along_path_ = 1.0;
+
+ // Holds the last valid goal position for when we loose our path.
+ ::Eigen::Matrix<double, 2, 1> theta_;
+ ::Eigen::Matrix<double, 2, 1> omega_;
+
+ ::Eigen::Matrix<double, 2, 6> last_K_;
+ int failed_solutions_ = 0;
};
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index bfcf915..47e6fb7 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,22 +1,27 @@
#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "third_party/gflags/include/gflags/gflags.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/ekf.h"
+DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
+DEFINE_bool(plot, true, "If true, plot");
+DEFINE_bool(plot_thetas, true, "If true, plot the angles");
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
namespace arm {
void Main() {
- Path p = MakeDemoPath();
- Trajectory trajectory(&p, 0.001);
+ Trajectory trajectory(
+ FLAGS_forwards ? MakeDemoPath() : MakeReversedDemoPath(), 0.001);
- constexpr double kAlpha0Max = 40.0;
- constexpr double kAlpha1Max = 60.0;
- constexpr double vmax = 11.95;
+ constexpr double kAlpha0Max = 30.0;
+ constexpr double kAlpha1Max = 50.0;
+ constexpr double vmax = 10.5;
constexpr double sim_dt = 0.00505;
const ::Eigen::Matrix<double, 2, 2> alpha_unitizer =
@@ -35,9 +40,9 @@
::std::vector<double> alpha1_array;
for (const double d : distance_array) {
- const ::Eigen::Matrix<double, 2, 1> theta = p.Theta(d);
- const ::Eigen::Matrix<double, 2, 1> omega = p.Omega(d);
- const ::Eigen::Matrix<double, 2, 1> alpha = p.Alpha(d);
+ const ::Eigen::Matrix<double, 2, 1> theta = trajectory.path().Theta(d);
+ const ::Eigen::Matrix<double, 2, 1> omega = trajectory.path().Omega(d);
+ const ::Eigen::Matrix<double, 2, 1> alpha = trajectory.path().Alpha(d);
theta0_array.push_back(theta(0, 0));
theta1_array.push_back(theta(1, 0));
omega0_array.push_back(omega(0, 0));
@@ -47,12 +52,18 @@
}
// Next step: see what U is as a function of distance.
+ ::std::vector<double> Uff0_distance_array_curvature;
+ ::std::vector<double> Uff1_distance_array_curvature;
+ ::std::vector<double> Uff0_distance_array_backwards_only;
+ ::std::vector<double> Uff1_distance_array_backwards_only;
::std::vector<double> Uff0_distance_array;
::std::vector<double> Uff1_distance_array;
for (const double distance : distance_array) {
- const double goal_velocity = trajectory.GetDVelocity(distance);
- const double goal_acceleration = trajectory.GetDAcceleration(distance);
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_forward_pass());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_forward_pass());
const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
const ::Eigen::Matrix<double, 2, 1> omega_t =
trajectory.OmegaT(distance, goal_velocity);
@@ -61,12 +72,50 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
Uff0_distance_array.push_back(U(0));
Uff1_distance_array.push_back(U(1));
}
+ for (const double distance : distance_array) {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity_unfiltered());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity_unfiltered());
+ const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
+ const ::Eigen::Matrix<double, 2, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 2, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 2, 1> U =
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_curvature.push_back(U(0));
+ Uff1_distance_array_curvature.push_back(U(1));
+ }
+
+ for (const double distance : distance_array) {
+ const double goal_velocity = trajectory.GetDVelocity(
+ distance, trajectory.max_dvelocity());
+ const double goal_acceleration = trajectory.GetDAcceleration(
+ distance, trajectory.max_dvelocity());
+ const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
+ const ::Eigen::Matrix<double, 2, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 2, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 2, 1> U =
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
+
+ Uff0_distance_array_backwards_only.push_back(U(0));
+ Uff1_distance_array_backwards_only.push_back(U(1));
+ }
+
double t = 0;
::Eigen::Matrix<double, 4, 1> X;
{
@@ -74,7 +123,7 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- TrajectoryFollower follower(&p, &trajectory, alpha_unitizer);
+ TrajectoryFollower follower(&trajectory);
::std::vector<double> t_array;
::std::vector<double> theta0_goal_t_array;
@@ -107,13 +156,16 @@
EKF arm_ekf;
arm_ekf.Reset(X);
+ ::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
+ ::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
+ ::std::cout << "Really stabilized P: " << arm_ekf.P_converged()
+ << ::std::endl;
- while (t < 3.0) {
+ while (t < 1.5) {
t_array.push_back(t);
arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
sim_dt);
- // TODO(austin): Compensate for torque disturbance.
- follower.Update(arm_ekf.X_hat(), sim_dt, vmax);
+ follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
const ::Eigen::Matrix<double, 2, 1> theta_t =
trajectory.ThetaT(follower.goal()(0));
@@ -148,7 +200,7 @@
::Eigen::Matrix<double, 2, 1> actual_U = follower.U();
// Add in a disturbance force to see how well the arm learns it.
- actual_U.array() += 1.0;
+ actual_U(0) += 1.0;
const ::Eigen::Matrix<double, 4, 1> xdot =
Dynamics::Acceleration(X, actual_U);
@@ -167,89 +219,108 @@
t += sim_dt;
}
- matplotlibcpp::figure();
- matplotlibcpp::title("Trajectory");
- matplotlibcpp::plot(theta0_array, theta1_array, {{"label", "desired path"}});
- matplotlibcpp::plot(theta0_t_array, theta1_t_array,
- {{"label", "actual path"}});
- matplotlibcpp::legend();
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Trajectory");
+ matplotlibcpp::plot(theta0_array, theta1_array,
+ {{"label", "desired path"}});
+ matplotlibcpp::plot(theta0_t_array, theta1_t_array,
+ {{"label", "actual path"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(distance_array, theta0_array, {{"label", "theta0"}});
- matplotlibcpp::plot(distance_array, theta1_array, {{"label", "theta1"}});
- matplotlibcpp::plot(distance_array, omega0_array, {{"label", "omega0"}});
- matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
- matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
- matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
- matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distance_array, theta0_array, {{"label", "theta0"}});
+ matplotlibcpp::plot(distance_array, theta1_array, {{"label", "theta1"}});
+ matplotlibcpp::plot(distance_array, omega0_array, {{"label", "omega0"}});
+ matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
+ matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
+ matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_unfiltered(),
- {{"label", "pass0"}});
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity(),
- {{"label", "passb"}});
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_forward_pass(),
- {{"label", "passf"}});
- matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_unfiltered(),
+ {{"label", "pass0"}});
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity(),
+ {{"label", "passb"}});
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_forward_pass(),
+ {{"label", "passf"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(t_array, alpha0_goal_t_array,
- {{"label", "alpha0_t_goal"}});
- matplotlibcpp::plot(t_array, alpha0_t_array, {{"label", "alpha0_t"}});
- matplotlibcpp::plot(t_array, alpha1_goal_t_array,
- {{"label", "alpha1_t_goal"}});
- matplotlibcpp::plot(t_array, alpha1_t_array, {{"label", "alpha1_t"}});
- matplotlibcpp::plot(t_array, distance_t_array, {{"label", "distance_t"}});
- matplotlibcpp::plot(t_array, velocity_t_array, {{"label", "velocity_t"}});
- matplotlibcpp::plot(t_array, acceleration_t_array,
- {{"label", "acceleration_t"}});
- matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_array, alpha0_goal_t_array,
+ {{"label", "alpha0_t_goal"}});
+ matplotlibcpp::plot(t_array, alpha0_t_array, {{"label", "alpha0_t"}});
+ matplotlibcpp::plot(t_array, alpha1_goal_t_array,
+ {{"label", "alpha1_t_goal"}});
+ matplotlibcpp::plot(t_array, alpha1_t_array, {{"label", "alpha1_t"}});
+ matplotlibcpp::plot(t_array, distance_t_array, {{"label", "distance_t"}});
+ matplotlibcpp::plot(t_array, velocity_t_array, {{"label", "velocity_t"}});
+ matplotlibcpp::plot(t_array, acceleration_t_array,
+ {{"label", "acceleration_t"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::title("Angular Velocities");
- matplotlibcpp::plot(t_array, omega0_goal_t_array,
- {{"label", "omega0_t_goal"}});
- matplotlibcpp::plot(t_array, omega0_t_array, {{"label", "omega0_t"}});
- matplotlibcpp::plot(t_array, omega0_hat_t_array, {{"label", "omega0_hat_t"}});
- matplotlibcpp::plot(t_array, omega1_goal_t_array,
- {{"label", "omega1_t_goal"}});
- matplotlibcpp::plot(t_array, omega1_t_array, {{"label", "omega1_t"}});
- matplotlibcpp::plot(t_array, omega1_hat_t_array, {{"label", "omega1_hat_t"}});
- matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Angular Velocities");
+ matplotlibcpp::plot(t_array, omega0_goal_t_array,
+ {{"label", "omega0_t_goal"}});
+ matplotlibcpp::plot(t_array, omega0_t_array, {{"label", "omega0_t"}});
+ matplotlibcpp::plot(t_array, omega0_hat_t_array,
+ {{"label", "omega0_hat_t"}});
+ matplotlibcpp::plot(t_array, omega1_goal_t_array,
+ {{"label", "omega1_t_goal"}});
+ matplotlibcpp::plot(t_array, omega1_t_array, {{"label", "omega1_t"}});
+ matplotlibcpp::plot(t_array, omega1_hat_t_array,
+ {{"label", "omega1_hat_t"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::title("Voltages");
- matplotlibcpp::plot(t_array, u0_unsaturated_array, {{"label", "u0_full"}});
- matplotlibcpp::plot(t_array, u0_array, {{"label", "u0"}});
- matplotlibcpp::plot(t_array, uff0_array, {{"label", "uff0"}});
- matplotlibcpp::plot(t_array, u1_unsaturated_array, {{"label", "u1_full"}});
- matplotlibcpp::plot(t_array, u1_array, {{"label", "u1"}});
- matplotlibcpp::plot(t_array, uff1_array, {{"label", "uff1"}});
- matplotlibcpp::plot(t_array, torque0_hat_t_array, {{"label", "torque0_hat"}});
- matplotlibcpp::plot(t_array, torque1_hat_t_array, {{"label", "torque1_hat"}});
- matplotlibcpp::legend();
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Voltages");
+ matplotlibcpp::plot(t_array, u0_unsaturated_array, {{"label", "u0_full"}});
+ matplotlibcpp::plot(t_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(t_array, uff0_array, {{"label", "uff0"}});
+ matplotlibcpp::plot(t_array, u1_unsaturated_array, {{"label", "u1_full"}});
+ matplotlibcpp::plot(t_array, u1_array, {{"label", "u1"}});
+ matplotlibcpp::plot(t_array, uff1_array, {{"label", "uff1"}});
+ matplotlibcpp::plot(t_array, torque0_hat_t_array,
+ {{"label", "torque0_hat"}});
+ matplotlibcpp::plot(t_array, torque1_hat_t_array,
+ {{"label", "torque1_hat"}});
+ matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::title("Angles");
- matplotlibcpp::plot(t_array, theta0_goal_t_array,
- {{"label", "theta0_t_goal"}});
- matplotlibcpp::plot(t_array, theta0_t_array, {{"label", "theta0_t"}});
- matplotlibcpp::plot(t_array, theta0_hat_t_array, {{"label", "theta0_hat_t"}});
- matplotlibcpp::plot(t_array, theta1_goal_t_array,
- {{"label", "theta1_t_goal"}});
- matplotlibcpp::plot(t_array, theta1_t_array, {{"label", "theta1_t"}});
- matplotlibcpp::plot(t_array, theta1_hat_t_array, {{"label", "theta1_hat_t"}});
- matplotlibcpp::legend();
+ if (FLAGS_plot_thetas) {
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Angles");
+ matplotlibcpp::plot(t_array, theta0_goal_t_array,
+ {{"label", "theta0_t_goal"}});
+ matplotlibcpp::plot(t_array, theta0_t_array, {{"label", "theta0_t"}});
+ matplotlibcpp::plot(t_array, theta0_hat_t_array,
+ {{"label", "theta0_hat_t"}});
+ matplotlibcpp::plot(t_array, theta1_goal_t_array,
+ {{"label", "theta1_t_goal"}});
+ matplotlibcpp::plot(t_array, theta1_t_array, {{"label", "theta1_t"}});
+ matplotlibcpp::plot(t_array, theta1_hat_t_array,
+ {{"label", "theta1_hat_t"}});
+ matplotlibcpp::legend();
+ }
-/*
- matplotlibcpp::figure();
- matplotlibcpp::title("ff for distance");
- matplotlibcpp::plot(distance_array, Uff0_distance_array, {{"label", "ff0"}});
- matplotlibcpp::plot(distance_array, Uff1_distance_array, {{"label", "ff1"}});
- matplotlibcpp::legend();
-*/
+ matplotlibcpp::figure();
+ matplotlibcpp::title("ff for distance");
+ matplotlibcpp::plot(distance_array, Uff0_distance_array, {{"label",
+ "ff0"}});
+ matplotlibcpp::plot(distance_array, Uff1_distance_array, {{"label",
+ "ff1"}});
+ matplotlibcpp::plot(distance_array, Uff0_distance_array_backwards_only,
+ {{"label", "ff0_back"}});
+ matplotlibcpp::plot(distance_array, Uff1_distance_array_backwards_only,
+ {{"label", "ff1_back"}});
+ matplotlibcpp::plot(distance_array, Uff0_distance_array_curvature,
+ {{"label", "ff0_curve"}});
+ matplotlibcpp::plot(distance_array, Uff1_distance_array_curvature,
+ {{"label", "ff1_curve"}});
+ matplotlibcpp::legend();
- matplotlibcpp::show();
+ matplotlibcpp::show();
+ }
}
} // namespace arm
@@ -257,7 +328,8 @@
} // namespace control_loops
} // namespace y2018
-int main(int /*argc*/, const char * /*argv*/ []) {
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
::y2018::control_loops::superstructure::arm::Main();
return 0;
}
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
index 9687e40..b91c5a4 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_test.cc
@@ -83,7 +83,7 @@
{{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
{{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- Trajectory t(&p, 0.1);
+ Trajectory t(::std::unique_ptr<Path>(new Path(p)), 0.1);
// 0 - 3.0 every 0.1 should be 31 points.
EXPECT_EQ(t.num_plan_points(), 31);
@@ -135,11 +135,27 @@
EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
}
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, ReversedPath) {
+ // Tests that a reversed path is actually reversed.
+ ::std::unique_ptr<Path> path = MakeDemoPath();
+ ::std::unique_ptr<Path> reversed_path(
+ new Path(Path::Reversed(*MakeDemoPath())));
+
+ EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
+
+ for (double d = 0; d < path->length(); d += 0.01) {
+ EXPECT_TRUE(path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
+ EXPECT_TRUE(path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
+ EXPECT_TRUE(path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
+ }
+}
+
// Tests that we can follow a path. Look at :trajectory_plot if you want to see
// the path.
TEST(TrajectoryTest, RunTrajectory) {
- Path path = MakeDemoPath();
- Trajectory trajectory(&path, 0.001);
+ ::std::unique_ptr<Path> path = MakeDemoPath();
+ Trajectory trajectory(::std::move(path), 0.001);
constexpr double kAlpha0Max = 40.0;
constexpr double kAlpha1Max = 60.0;
@@ -161,12 +177,12 @@
EKF arm_ekf;
arm_ekf.Reset(X);
- TrajectoryFollower follower(&path, &trajectory, alpha_unitizer);
+ TrajectoryFollower follower(&trajectory);
constexpr double sim_dt = 0.00505;
while (t < 1.0) {
arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
sim_dt);
- follower.Update(arm_ekf.X_hat(), sim_dt, vmax);
+ follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
t += sim_dt;
@@ -174,7 +190,7 @@
::Eigen::Matrix<double, 4, 1> final_X;
::Eigen::Matrix<double, 2, 1> final_theta_t =
- trajectory.ThetaT(path.length());
+ trajectory.ThetaT(trajectory.path().length());
final_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
// Verify that we got to the end.
@@ -182,7 +198,8 @@
<< ": X is " << X.transpose() << " final_X is " << final_X.transpose();
// Verify that our goal is at the end.
- EXPECT_TRUE(final_theta_t.isApprox(path.Theta(follower.goal(0))));
+ EXPECT_TRUE(
+ final_theta_t.isApprox(trajectory.path().Theta(follower.goal(0))));
}
} // namespace testing
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 157a46e..25cc500 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -31,6 +31,7 @@
LOG(ERROR, "WPILib reset, restarting\n");
intake_left_.Reset();
intake_right_.Reset();
+ arm_.Reset();
}
intake_left_.Iterate(unsafe_goal != nullptr
@@ -47,10 +48,26 @@
output != nullptr ? &(output->intake.right) : nullptr,
&(status->right_intake));
- status->estopped =
- status->left_intake.estopped || status->right_intake.estopped;
+ intake_right_.Iterate(unsafe_goal != nullptr
+ ? &(unsafe_goal->intake.right_intake_angle)
+ : nullptr,
+ &(position->intake.right),
+ output != nullptr ? &(output->intake.right) : nullptr,
+ &(status->left_intake));
- status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed;
+ arm_.Iterate(
+ unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
+ &(position->arm),
+ output != nullptr ? &(output->voltage_proximal) : nullptr,
+ output != nullptr ? &(output->voltage_distal) : nullptr,
+ output != nullptr ? &(output->release_arm_brake) : nullptr,
+ &(status->arm));
+
+ status->estopped = status->left_intake.estopped ||
+ status->right_intake.estopped || status->arm.estopped;
+
+ status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
+ status->arm.zeroed;
if (output && unsafe_goal) {
output->intake.left.voltage_rollers = ::std::max(
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 0cc504d..ade5047 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -5,6 +5,7 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
@@ -21,6 +22,7 @@
const intake::IntakeSide &intake_left() const { return intake_left_; }
const intake::IntakeSide &intake_right() const { return intake_right_; }
+ const arm::Arm &arm() const { return arm_; }
protected:
virtual void RunIteration(
@@ -32,6 +34,7 @@
private:
intake::IntakeSide intake_left_;
intake::IntakeSide intake_right_;
+ arm::Arm arm_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 3eaeb7e..93d5935 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -71,10 +71,42 @@
};
struct ArmStatus {
- // TODO(austin): Fill in more state once we know what it is.
// State of the estimators.
.frc971.AbsoluteEstimatorState proximal_estimator_state;
.frc971.AbsoluteEstimatorState distal_estimator_state;
+
+ // The node we are currently going to.
+ uint32_t current_node;
+ // Distance (in radians) to the end of the path.
+ float path_distance_to_go;
+ // Goal position and velocity (radians)
+ float goal_theta0;
+ float goal_theta1;
+ float goal_omega0;
+ float goal_omega1;
+
+ // Current position and velocity (radians)
+ float theta0;
+ float theta1;
+
+ float omega0;
+ float omega1;
+
+ // Estimated voltage error for the two joints.
+ float voltage_error0;
+ float voltage_error1;
+
+ // True if we are zeroed.
+ bool zeroed;
+
+ // True if the arm is zeroed.
+ bool estopped;
+
+ // The current state machine state.
+ uint32_t state;
+
+ // The number of times the LQR solver failed.
+ uint32_t failed_solutions;
};
struct ArmPosition {
@@ -112,7 +144,7 @@
IntakeGoal intake;
// Used to identiy a position in the planned set of positions on the arm.
- int32_t arm_goal_position;
+ uint32_t arm_goal_position;
bool open_claw;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 2fa19ca..09809e6 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -11,6 +11,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -109,6 +110,74 @@
zeroing_constants_;
};
+class ArmSimulation {
+ public:
+ explicit ArmSimulation(
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &proximal_zeroing_constants,
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &distal_zeroing_constants)
+ : proximal_zeroing_constants_(proximal_zeroing_constants),
+ proximal_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kProximalEncoderRatio()),
+ distal_zeroing_constants_(distal_zeroing_constants),
+ distal_pot_encoder_(M_PI * 2.0 *
+ constants::Values::kDistalEncoderRatio()) {
+ X_.setZero();
+ }
+
+ void InitializePosition(::Eigen::Matrix<double, 2, 1> position) {
+ X_ << position(0), 0.0, position(1), 0.0;
+
+ proximal_pot_encoder_.Initialize(
+ X_(0), kNoiseScalar, 0.0,
+ proximal_zeroing_constants_.measured_absolute_position);
+ distal_pot_encoder_.Initialize(
+ X_(2), kNoiseScalar, 0.0,
+ distal_zeroing_constants_.measured_absolute_position);
+ }
+
+ void GetSensorValues(ArmPosition *position) {
+ proximal_pot_encoder_.GetSensorValues(&position->proximal);
+ distal_pot_encoder_.GetSensorValues(&position->distal);
+ }
+
+ double proximal_position() const { return X_(0, 0); }
+ double proximal_velocity() const { return X_(1, 0); }
+ double distal_position() const { return X_(2, 0); }
+ double distal_velocity() const { return X_(3, 0); }
+
+ //void set_voltage_offset(double voltage_offset) {
+ //plant_.set_voltage_offset(voltage_offset);
+ //}
+
+ void Simulate(::Eigen::Matrix<double, 2, 1> U) {
+ constexpr double voltage_check =
+ superstructure::arm::Arm::kOperatingVoltage();
+
+ CHECK_LE(::std::abs(U(0)), voltage_check);
+ CHECK_LE(::std::abs(U(1)), voltage_check);
+
+ X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+
+ // TODO(austin): Estop on grose out of bounds.
+ proximal_pot_encoder_.MoveTo(X_(0));
+ distal_pot_encoder_.MoveTo(X_(2));
+ }
+
+ private:
+ ::Eigen::Matrix<double, 4, 1> X_;
+
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ proximal_zeroing_constants_;
+ PositionSensorSimulator proximal_pot_encoder_;
+
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ distal_zeroing_constants_;
+ PositionSensorSimulator distal_pot_encoder_;
+};
+
+
class SuperstructureSimulation {
public:
SuperstructureSimulation()
@@ -118,6 +187,8 @@
right_intake_(::y2018::control_loops::superstructure::intake::
MakeDelayedIntakePlant(),
constants::GetValues().right_intake.zeroing),
+ arm_(constants::GetValues().arm_proximal.zeroing,
+ constants::GetValues().arm_distal.zeroing),
superstructure_queue_(".y2018.control_loops.superstructure", 0xdeadbeef,
".y2018.control_loops.superstructure.goal",
".y2018.control_loops.superstructure.position",
@@ -127,6 +198,9 @@
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
2.0);
+
+ InitializeArmPosition(
+ (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished());
}
void InitializeIntakePosition(double start_pos) {
@@ -134,12 +208,18 @@
right_intake_.InitializePosition(start_pos);
}
+ void InitializeArmPosition(::Eigen::Matrix<double, 2, 1> position) {
+ arm_.InitializePosition(position);
+ }
+
void SendPositionMessage() {
::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
superstructure_queue_.position.MakeMessage();
left_intake_.GetSensorValues(&position->intake.left);
right_intake_.GetSensorValues(&position->intake.right);
+ arm_.GetSensorValues(&position->arm);
+
LOG_STRUCT(INFO, "sim position", *position);
position.Send();
}
@@ -156,6 +236,8 @@
const IntakeSideSimulation &left_intake() const { return left_intake_; }
const IntakeSideSimulation &right_intake() const { return right_intake_; }
+ const ArmSimulation &arm() const { return arm_; }
+
// Simulates the intake for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
@@ -163,11 +245,16 @@
left_intake_.Simulate(superstructure_queue_.output->intake.left);
right_intake_.Simulate(superstructure_queue_.output->intake.right);
+ arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
+ << superstructure_queue_.output->voltage_proximal,
+ superstructure_queue_.output->voltage_distal)
+ .finished());
}
private:
IntakeSideSimulation left_intake_;
IntakeSideSimulation right_intake_;
+ ArmSimulation arm_;
SuperstructureQueue superstructure_queue_;
};
@@ -188,7 +275,7 @@
superstructure_queue_.goal.FetchLatest();
superstructure_queue_.status.FetchLatest();
- ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr);
+ ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
// Left side test.
EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
@@ -250,6 +337,8 @@
// Tests that the intake loop can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
+ superstructure_plant_.InitializeArmPosition(
+ (::Eigen::Matrix<double, 2, 1>() << 0.5, M_PI).finished());
// Set a reasonable goal.
{
auto goal = superstructure_queue_.goal.MakeMessage();
@@ -270,6 +359,8 @@
// position.
TEST_F(SuperstructureTest, OffsetStartReachesGoal) {
superstructure_plant_.InitializeIntakePosition(0.5);
+ superstructure_plant_.InitializeArmPosition(
+ (::Eigen::Matrix<double, 2, 1>() << 0.5, M_PI).finished());
// Set a reasonable goal.
{
@@ -408,6 +499,51 @@
VerifyNearGoal();
}
+// Tests that we don't freak out without a goal.
+TEST_F(SuperstructureTest, ArmNoGoalTest) {
+ RunForTime(chrono::seconds(5));
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = 0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ EXPECT_EQ(arm::Arm::State::RUNNING, superstructure_.arm().state());
+}
+
+// Tests that we can can execute a move.
+TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
+ superstructure_plant_.InitializeArmPosition(
+ (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.5).finished());
+
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = 1;
+ ASSERT_TRUE(goal.Send());
+
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 0.0;
+ goal->intake.right_intake_angle = 0.0;
+ goal->arm_goal_position = 0;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+// TODO(austin): Test multiple path segments.
+// TODO(austin): Disable in the middle and test recovery.
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops