Add double jointed arm dynamics.
Change-Id: I882d23955f4ebd367b01b7506d9957ec6f3eed40
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 2862a6a..cbb443d 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -20,3 +20,28 @@
"//third_party/eigen",
],
)
+
+cc_library(
+ name = "dynamics",
+ srcs = [
+ "dynamics.cc",
+ ],
+ hdrs = [
+ "dynamics.h",
+ ],
+ deps = [
+ "//frc971/control_loops:runge_kutta",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "dynamics_test",
+ srcs = [
+ "dynamics_test.cc",
+ ],
+ deps = [
+ ":dynamics",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
new file mode 100644
index 0000000..5086e78
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/dynamics.cc
@@ -0,0 +1,31 @@
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+const ::Eigen::Matrix<double, 2, 2> Dynamics::K3 =
+ (::Eigen::Matrix<double, 2, 2>()
+ << Dynamics::kG1 * Dynamics::Kt / Dynamics::kResistance,
+ 0.0, 0.0, Dynamics::kG2 *Dynamics::kNumDistalMotors *Dynamics::Kt /
+ Dynamics::kResistance)
+ .finished();
+
+const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
+ (::Eigen::Matrix<double, 2, 2>()
+ << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
+ (Dynamics::Kv * Dynamics::kResistance),
+ 0.0, 0.0,
+ Dynamics::kG2 *Dynamics::kG2 *Dynamics::Kt *Dynamics::kNumDistalMotors /
+ (Dynamics::Kv * Dynamics::kResistance))
+ .finished();
+
+constexpr double Dynamics::kAlpha;
+constexpr double Dynamics::kBeta;
+constexpr double Dynamics::kGamma;
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
new file mode 100644
index 0000000..7ad6696
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/dynamics.h
@@ -0,0 +1,133 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
+
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/runge_kutta.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+// This class captures the dynamics of our system. It doesn't actually need to
+// store state yet, so everything can be constexpr and/or static.
+class Dynamics {
+ public:
+ // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
+ // Length of the joints in meters.
+ static constexpr double kL1 = 46.25 * 0.0254;
+ static constexpr double kL2 = 41.80 * 0.0254;
+
+ // Mass of the joints in kilograms.
+ static constexpr double kM1 = 9.34 / 2.2;
+ static constexpr double kM2 = 9.77 / 2.2;
+
+ // Moment of inertia of the joints in kg m^2
+ static constexpr double kJ1 = 2957.05 * 0.0002932545454545454;
+ static constexpr double kJ2 = 2824.70 * 0.0002932545454545454;
+
+ // Radius of the center of mass of the joints in meters.
+ static constexpr double r1 = 21.64 * 0.0254;
+ static constexpr double r2 = 26.70 * 0.0254;
+
+ // Gear ratios for the two joints.
+ static constexpr double kG1 = 140.0;
+ static constexpr double kG2 = 90.0;
+
+ // MiniCIM motor constants.
+ static constexpr double kStallTorque = 1.41;
+ 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;
+ static constexpr double Kv = kFreeSpeed / 12.0;
+ static constexpr double Kt = kStallTorque / kStallCurrent;
+
+ // Number of motors on the distal joint.
+ static constexpr double kNumDistalMotors = 2.0;
+
+ static constexpr double kAlpha = kJ1 + r1 * r1 * kM1 + kL1 * kL1 * kM2;
+ static constexpr double kBeta = kL1 * r2 * kM2;
+ static constexpr double kGamma = kJ2 + r2 * r2 * kM2;
+
+ // K3, K4 matricies described below.
+ static const ::Eigen::Matrix<double, 2, 2> K3;
+ static const ::Eigen::Matrix<double, 2, 2> K4;
+
+ // Generates K1-2 for the arm ODE.
+ // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
+ // These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
+ // You probbaly want MatriciesForState.
+ static void NormilizedMatriciesForState(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) {
+ const double angle = X(0, 0) - X(2, 0);
+ const double s = ::std::sin(angle);
+ const double c = ::std::cos(angle);
+ *K1_result << kAlpha, c * kBeta, c * kBeta, kGamma;
+ *K2_result << 0.0, s * kBeta, -s * kBeta, 0.0;
+ }
+
+ // Generates K1-2 for the arm ODE.
+ // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
+ static void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
+ ::Eigen::Matrix<double, 2, 2> *K1_result,
+ ::Eigen::Matrix<double, 2, 2> *K2_result) {
+ NormilizedMatriciesForState(X, K1_result, K2_result);
+ (*K2_result)(1, 0) *= X(1, 0);
+ (*K2_result)(0, 1) *= X(3, 0);
+ }
+
+ // TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
+
+ // Calculates the acceleration given the current state and control input.
+ static const ::Eigen::Matrix<double, 4, 1> Acceleration(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::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> accel =
+ K1.inverse() * (torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0))
+ .finished();
+ }
+
+ // Calculates the voltage required to follow the trajectory. This requires
+ // knowing the current state, desired angular velocity and acceleration.
+ static const ::Eigen::Matrix<double, 2, 1> FF_U(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &omega_t,
+ const ::Eigen::Matrix<double, 2, 1> &alpha_t) {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X, &K1, &K2);
+
+ return K3.inverse() * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
+ }
+
+ static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
+ return ::frc971::control_loops::RungeKutta(Dynamics::Acceleration, X, U,
+ dt);
+ }
+};
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
new file mode 100644
index 0000000..065db72
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/dynamics_test.cc
@@ -0,0 +1,32 @@
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+#include "gtest/gtest.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace testing {
+
+// Tests that zero inputs result in no acceleration and no motion.
+// This isn't all that rigerous, but it's a good start.
+TEST(DynamicsTest, Acceleration) {
+ Dynamics dynamics;
+
+ EXPECT_TRUE(dynamics
+ .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero())
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+
+ EXPECT_TRUE(dynamics
+ .UnboundedDiscreteDynamics(
+ ::Eigen::Matrix<double, 4, 1>::Zero(),
+ ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
+ .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
+}
+
+} // namespace testing
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018