Fixed horizon and other bugs in examples

examples/example_cart_pole_trajectory_optimization/main.cpp

@@ -56,41 +56,41 @@ int main(int argc, char** argv) {
   std::cout << "Goal State: " << X_T.transpose() << std::endl << std::endl;
 
   // Create trajectory factor graph.
-  auto graph_builder = DynamicsGraph();
+  auto graph_builder = DynamicsGraph(gravity);
   auto graph = graph_builder.trajectoryFG(
-      cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity);
+      cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal);
 
   // Set the pendulum joint to be unactuated.
   for (int t = 0; t <= t_steps; t++)
-    graph.addPrior(TorqueKey(j1_id, t), 0.0, Constrained::All(1));
+    graph.addPrior(internal::TorqueKey(j1_id, t), 0.0, Constrained::All(1));
 
   // Add initial conditions to trajectory factor graph.
-  graph.addPrior(JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
-  graph.addPrior(JointVelKey(j0_id, 0), X_i[1], dynamics_model);
-  graph.addPrior(JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
-  graph.addPrior(JointVelKey(j1_id, 0), X_i[4], dynamics_model);
+  graph.addPrior(internal::JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
+  graph.addPrior(internal::JointVelKey(j0_id, 0), X_i[1], dynamics_model);
+  graph.addPrior(internal::JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
+  graph.addPrior(internal::JointVelKey(j1_id, 0), X_i[4], dynamics_model);
 
   // Add terminal conditions to the factor graph.
-  graph.addPrior(JointVelKey(j0_id, t_steps), X_T[1], objectives_model);
-  graph.addPrior(JointAccelKey(j0_id, t_steps), X_T[2], objectives_model);
-  graph.addPrior(JointVelKey(j1_id, t_steps), X_T[4], objectives_model);
-  graph.addPrior(JointAccelKey(j1_id, t_steps), X_T[5], objectives_model);
+  graph.addPrior(internal::JointVelKey(j0_id, t_steps), X_T[1], objectives_model);
+  graph.addPrior(internal::JointAccelKey(j0_id, t_steps), X_T[2], objectives_model);
+  graph.addPrior(internal::JointVelKey(j1_id, t_steps), X_T[4], objectives_model);
+  graph.addPrior(internal::JointAccelKey(j1_id, t_steps), X_T[5], objectives_model);
 
   // Insert position objective (x, theta) factor at every timestep or only at
   // the terminal state. Adding the position objective at every timestep will
   // force the system to converge to the desired state quicker at the cost of
   // more impulsive control actions.
   bool apply_pos_objective_all_dt = false;
-  graph.addPrior(JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model);
-  graph.addPrior(JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model);
+  graph.addPrior(internal::JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model);
+  graph.addPrior(internal::JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model);
   if (apply_pos_objective_all_dt) {
     for (int t = 0; t < t_steps; t++) {
-      graph.addPrior(JointAngleKey(j0_id, t), X_T[0], pos_objectives_model);
-      graph.addPrior(JointAngleKey(j1_id, t), X_T[3], pos_objectives_model);
+      graph.addPrior(internal::JointAngleKey(j0_id, t), X_T[0], pos_objectives_model);
+      graph.addPrior(internal::JointAngleKey(j1_id, t), X_T[3], pos_objectives_model);
     }
   }
   for (int t = 0; t <= t_steps; t++)
-    graph.emplace_shared<MinTorqueFactor>(TorqueKey(j0_id, t), control_model);
+    graph.emplace_shared<MinTorqueFactor>(internal::TorqueKey(j0_id, t), control_model);
 
   // Initialize solution.
   auto init_vals = ZeroValuesTrajectory(cp, t_steps, 0, 0.0);
@@ -107,10 +107,10 @@ int main(int argc, char** argv) {
   double t_elapsed = 0;
   for (int t = 0; t <= t_steps; t++, t_elapsed += dt) {
     std::vector<gtsam::Key> keys = {
-        JointAngleKey(j0_id, t), JointVelKey(j0_id, t),
-        JointAccelKey(j0_id, t), TorqueKey(j0_id, t),
-        JointAngleKey(j1_id, t), JointVelKey(j1_id, t),
-        JointAccelKey(j1_id, t), TorqueKey(j1_id, t)};
+        internal::JointAngleKey(j0_id, t), internal::JointVelKey(j0_id, t),
+        internal::JointAccelKey(j0_id, t), internal::TorqueKey(j0_id, t),
+        internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t),
+        internal::JointAccelKey(j1_id, t), internal::TorqueKey(j1_id, t)};
     std::vector<std::string> vals = {std::to_string(t_elapsed)};
     for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k)));
     traj_file << boost::algorithm::join(vals, ",") << "\n";

examples/example_forward_dynamics/main.cpp

@@ -12,11 +12,11 @@ int main(int argc, char** argv) {
   auto simple_rr = CreateRobotFromFile("../simple_rr.sdf", "simple_rr_sdf");
   simple_rr.print();
 
-  auto graph_builder = DynamicsGraph();
   gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished();
+  auto graph_builder = DynamicsGraph(gravity);
   auto kdfg = graph_builder.dynamicsFactorGraph(simple_rr,
-                                                0,  // timestep
-                                                gravity);
+                                                0  // timestep
+                                                );
 
   // Specify the forward dynamics priors and add them to the factor graph.
   gtsam::Vector theta = (gtsam::Vector(2) << 0, 0).finished();
@@ -35,4 +35,4 @@ int main(int argc, char** argv) {
   graph_builder.printValues(results);
 
   return 0;
-}
+}

examples/example_inverted_pendulum_trajectory_optimization/main.cpp

@@ -52,26 +52,25 @@ int main(int argc, char** argv) {
   double theta_T = M_PI, dtheta_T = 0, ddtheta_T = 0;
 
   // Create trajectory factor graph.
-  auto graph_builder = DynamicsGraph();
+  auto graph_builder = DynamicsGraph(gravity, planar_axis);
   auto graph = graph_builder.trajectoryFG(
-      ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity,
-      planar_axis);
+      ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal);
 
   // Add initial conditions to trajectory factor graph.
-  graph.addPrior(JointAngleKey(j1_id, 0), theta_i, dynamics_model);
-  graph.addPrior(JointVelKey(j1_id, 0), dtheta_i, dynamics_model);
+  graph.addPrior(internal::JointAngleKey(j1_id, 0), theta_i, dynamics_model);
+  graph.addPrior(internal::JointVelKey(j1_id, 0), dtheta_i, dynamics_model);
 
   // Add state and min torque objectives to trajectory factor graph.
-  graph.addPrior(JointVelKey(j1_id, t_steps), dtheta_T, objectives_model);
-  graph.addPrior(JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model);
+  graph.addPrior(internal::JointVelKey(j1_id, t_steps), dtheta_T, objectives_model);
+  graph.addPrior(internal::JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model);
   bool apply_theta_objective_all_dt = false;
-  graph.addPrior(JointAngleKey(j1_id, t_steps), theta_T, objectives_model);
+  graph.addPrior(internal::JointAngleKey(j1_id, t_steps), theta_T, objectives_model);
   if (apply_theta_objective_all_dt) {
     for (int t = 0; t < t_steps; t++)
-      graph.addPrior(JointAngleKey(j1_id, t), theta_T, objectives_model);
+      graph.addPrior(internal::JointAngleKey(j1_id, t), theta_T, objectives_model);
   }
   for (int t = 0; t <= t_steps; t++)
-    graph.emplace_shared<MinTorqueFactor>(TorqueKey(j1_id, t), control_model);
+    graph.emplace_shared<MinTorqueFactor>(internal::TorqueKey(j1_id, t), control_model);
 
   // Initialize solution.
   auto init_vals = ZeroValuesTrajectory(ip, t_steps, 0, 0.0);
@@ -88,8 +87,8 @@ int main(int argc, char** argv) {
   double t_elapsed = 0;
   for (int t = 0; t <= t_steps; t++, t_elapsed += dt) {
     std::vector<gtsam::Key> keys = {
-        JointAngleKey(j1_id, t), JointVelKey(j1_id, t), JointAccelKey(j1_id, t),
-        TorqueKey(j1_id, t)};
+        internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t), internal::JointAccelKey(j1_id, t),
+        internal::TorqueKey(j1_id, t)};
     std::vector<std::string> vals = {std::to_string(t_elapsed)};
     for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k)));
     traj_file << boost::algorithm::join(vals, ",") << "\n";

examples/example_quadruped_mp/main.cpp

@@ -38,9 +38,9 @@ using gtsam::Pose3;
 using gtsam::Rot3;
 using gtsam::Vector3;
 
-typedef std::vector<gtsam::Vector> CoeffVector;
-typedef std::map<int, std::map<std::string, Pose3>> TargetFootholds;
-typedef std::map<std::string, Pose3> TargetPoses;
+using CoeffMatrix = gtsam::Matrix43;
+using TargetFootholds = std::map<int, std::map<std::string, Pose3>>;
+using TargetPoses = std::map<std::string, Pose3>;
 
 using namespace gtdynamics;
 
@@ -55,60 +55,67 @@ using namespace gtdynamics;
  * @param x_1_p tangent at the ending knot
  * @param horizon timestep between the knot at the start and the knot at the end
  *
- * @return 3*4 matrix of coefficients
- * 
- * Since uE[0,1] in the parametrization of the hermite spline equation, for
- * tE[0,horizon], u is set equal to t/horizon
+ * @return 4*3 matrix of coefficients.
+ *
+ * Since u ∈ [0,1] in the parametrization of the hermite spline equation, for
+ * t ∈ [0,horizon], u is set equal to `t/horizon`.
  *
  * Refer to this lecture for more info on the hermite parameterization
  * for cubic polynomials:
  * http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf
  */
-CoeffVector compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
+CoeffMatrix compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
                                         const Vector3 &x_0_p,
                                         const Vector3 &x_1_p,
                                         const double horizon) {
-
   // Extract position at the starting and ending knot.
   Vector3 x_0 = wTb_i.translation();
   Vector3 x_1 = wTb_f.translation();
 
-  // Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the 
+  // Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the
   // polynomial terms, and B and C are the basis matrix and control matrices
   // respectively.
-  // Vectors a_0 to a_3 constitute the matrix product A of the basis, B, and control,
-  // C, matrices leading to p(u)=U(u)*A where A=B*C.
-  Vector3 a_0 = x_0, a_1 = x_0_p;
-  // Since u=t/horizon, rearrange to integrate the horizon in the matrix product
-  // of B and C.
-  Vector3 a_2 = -std::pow(horizon, -2) *
-                (3 * (x_0 - x_1) + horizon * (2 * x_0_p + x_1_p));
-  Vector3 a_3 =
-      std::pow(horizon, -3) * (2 * (x_0 - x_1) + horizon * (x_0_p + x_1_p));
-
-  std::vector<gtsam::Vector> coeffs;
-  coeffs.push_back(a_0);
-  coeffs.push_back(a_1);
-  coeffs.push_back(a_2);
-  coeffs.push_back(a_3);
-
-  return coeffs;
+  gtsam::Matrix43 C;
+  C.row(0) = x_0;
+  C.row(1) = x_1;
+  C.row(2) = x_0_p;
+  C.row(3) = x_1_p;
+
+  gtsam::Matrix4 B;
+  B << 2, -2, 1, 1, -3, 3, -2, -1, 0, 0, 1, 0, 1, 0, 0, 0;
+  gtsam::Matrix43 A = B * C;
+
+  // Scale U by the horizon `t = u/horizon` so that we can directly use t.
+  A.row(0) /= (horizon * horizon * horizon);
+  A.row(1) /= (horizon * horizon);
+  A.row(2) /= horizon;
+
+  return A;
 }
 
 /**
- * Compute the robot base pose as defined by the hermite spline.
- * TODO(frank): document better
+ * Compute the robot base pose as defined by the calculated trajectory.
+ * The calculated trajectory is a piecewise polynomial consisting of
+ * cubic hermite splines; the base pose will move along this trajectory.
+ *
+ * @param coeffs 4*3 matrix of cubic polynomial coefficients
+ * @param x_0_p tangent at the starting knot
+ * @param t time at which pose will be evaluated
+ * @param wTb_i initial pose corresponding to the knot at the start
+ *
+ * @return rotation and position
  */
-Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p,
+Pose3 compute_hermite_pose(const CoeffMatrix &coeffs, const Vector3 &x_0_p,
                            const double t, const Pose3 &wTb_i) {
   // The position computed from the spline equation as a function of time,
-  // p(t)=U(t)*A where U(t)=[1,t,t^2,t^3].
-  Point3 p(coeffs[0] + coeffs[1] * t + coeffs[2] * std::pow(t, 2) +
-           coeffs[3] * std::pow(t, 3));
+  // p(t)=U(t)*A where U(t)=[t^3,t^2,t,1].
+  gtsam::Matrix14 t_vec(std::pow(t, 3), std::pow(t, 2), t, 1);
+  Point3 p(t_vec * coeffs);
 
   // Differentiate position with respect to t for velocity.
-  Point3 dpdt_v3 =
-      Point3(coeffs[1] + 2 * coeffs[2] * t + 3 * coeffs[3] * std::pow(t, 2));
+  gtsam::Matrix14 du(3 * t * t, 2 * t, 1, 0);
+  Point3 dpdt_v3 = Point3(du * coeffs);
+
   // Unit vector for velocity.
   dpdt_v3 = dpdt_v3 / dpdt_v3.norm();
   Point3 x_0_p_point(x_0_p);
@@ -123,11 +130,10 @@ Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p,
 }
 
 /** Compute the target footholds for each support phase. */
-TargetFootholds
-compute_target_footholds(const CoeffVector &coeffs, const Vector3 &x_0_p,
-                         const Pose3 &wTb_i, const double horizon,
-                         const double t_support,
-                         const std::map<std::string, Pose3> &bTfs) {
+TargetFootholds compute_target_footholds(
+    const CoeffMatrix &coeffs, const Vector3 &x_0_p, const Pose3 &wTb_i,
+    const double horizon, const double t_support,
+    const std::map<std::string, Pose3> &bTfs) {
   TargetFootholds target_footholds;
 
   double t_swing = t_support / 4.0; // Time for each swing foot trajectory.
@@ -153,7 +159,7 @@ TargetPoses compute_target_poses(const TargetFootholds &targ_footholds,
                                  const double horizon, const double t_support,
                                  const double t,
                                  const std::vector<std::string> &swing_sequence,
-                                 const CoeffVector &coeffs,
+                                 const CoeffMatrix &coeffs,
                                  const Vector3 &x_0_p, const Pose3 &wTb_i) {
   TargetPoses t_poses;
   // Compute the body pose.
@@ -221,7 +227,6 @@ struct CsvWriter {
   ~CsvWriter() { pose_file.close(); }
 
   void writeheader() {
-    pose_file.open("../traj.csv");
     pose_file << "bodyx,bodyy,bodyz";
     for (auto &&leg : swing_sequence)
       pose_file << "," << leg << "x"