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.h b/y2018/control_loops/superstructure/arm/dynamics.h
index 7ad6696..832796a 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ b/y2018/control_loops/superstructure/arm/dynamics.h
@@ -52,6 +52,7 @@
// K3, K4 matricies described below.
static const ::Eigen::Matrix<double, 2, 2> K3;
+ static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
static const ::Eigen::Matrix<double, 2, 2> K4;
// Generates K1-2 for the arm ODE.
@@ -114,7 +115,7 @@
MatriciesForState(X, &K1, &K2);
- return K3.inverse() * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
+ return K3_inverse * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
}
static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(