Added trajectory planner for the arm.
This computes the velocity as a function of path distance.
I'm not quite certain how to verify this other than the fact that I've
plotted it and the python code, and they produce the same answers.
Change-Id: Ibdbdfc78eb0c142c7e6a30047afa099b73964811
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
index 5086e78..87d2178 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ b/y2018/control_loops/superstructure/arm/dynamics.cc
@@ -12,6 +12,8 @@
Dynamics::kResistance)
.finished();
+const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
+
const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
(::Eigen::Matrix<double, 2, 2>()
<< Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /