[trajoptlib] Use direct collocation for diff drive dynamics constraints

Author: calcmogul

trajoptlib/src/DifferentialTrajectoryGenerator.cpp

@@ -203,18 +203,21 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
                                 {θ.at(index + 1)},
                                 {vl.at(index + 1)},
                                 {vr.at(index + 1)}};
+      slp::VariableMatrix u_k_1{{Fl.at(index + 1)}, {Fr.at(index + 1)}};
 
-      // Dynamics constraints - RK4
-      slp::VariableMatrix k1 = f(x_k, u_k);
-      slp::VariableMatrix k2 = f(x_k + dt * 0.5 * k1, u_k);
-      slp::VariableMatrix k3 = f(x_k + dt * 0.5 * k2, u_k);
-      slp::VariableMatrix k4 = f(x_k + dt * k3, u_k);
-      problem.SubjectTo(x_k_1 ==
-                        x_k + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4));
-
-      auto dx_dt = k1;
-      problem.SubjectTo(al.at(index) == dx_dt(3));
-      problem.SubjectTo(ar.at(index) == dx_dt(4));
+      // Dynamics constraints - direct collocation
+      // (https://mec560sbu.github.io/2016/09/30/direct_collocation/)
+      auto xdot_k = f(x_k, u_k);
+      auto xdot_k_1 = f(x_k_1, u_k_1);
+      auto xdot_c = -3 / (2 * dt) * (x_k - x_k_1) - 0.25 * (xdot_k + xdot_k_1);
+
+      auto x_c = 0.5 * (x_k + x_k_1) + dt / 8 * (xdot_k - xdot_k_1);
+      auto u_c = 0.5 * (u_k + u_k_1);
+
+      problem.SubjectTo(xdot_c == f(x_c, u_c));
+
+      problem.SubjectTo(al.at(index) == xdot_k(3));
+      problem.SubjectTo(ar.at(index) == xdot_k(4));
     }
   }