Working tests

Author: edantec

bindings/module.cpp

@@ -17,7 +17,8 @@ namespace simple_mpc::python
   void exposeMPC();
   void exposeIDSolver();
   void exposeIKIDSolver();
-  void exposeInterpolator();
+  void exposeStateInterpolator();
+  void exposeLinearInterpolator();
 
   /* PYTHON MODULE */
   BOOST_PYTHON_MODULE(simple_mpc_pywrap)
@@ -36,7 +37,8 @@ namespace simple_mpc::python
     exposeMPC();
     exposeIDSolver();
     exposeIKIDSolver();
-    exposeInterpolator();
+    exposeStateInterpolator();
+    exposeLinearInterpolator();
   }
 
 } // namespace simple_mpc::python

examples/go2_fulldynamics.py

@@ -268,7 +268,8 @@
 
     for j in range(N_simu):
         # time.sleep(0.01)
-        delay = j / float(N_simu)
+        delay = j / float(N_simu) * dt
+
         x_interp = state_interpolator.interpolate(delay, dt, xss)
         u_interp = u_interpolator.interpolate(delay, dt, uss)
         acc_interp = acc_interpolator.interpolate(delay, dt, ddqs)

src/fulldynamics.cpp

@@ -195,7 +195,7 @@ namespace simple_mpc
             space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name),
             pinocchio::LOCAL_WORLD_ALIGNED);
           FunctionSliceXpr vel_slice = FunctionSliceXpr(velocity_residual, vel_id);
-          // stm.addConstraint(vel_slice, EqualityConstraint());
+          stm.addConstraint(vel_slice, EqualityConstraint());
 
           std::vector<int> frame_id = {2};
 
@@ -205,7 +205,7 @@ namespace simple_mpc
 
           FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id);
 
-          // stm.addConstraint(frame_slice, EqualityConstraint());
+          stm.addConstraint(frame_slice, EqualityConstraint());
         }
       }
     }

src/interpolator.cpp

@@ -28,7 +28,7 @@ namespace simple_mpc
 
     // Interpolate state and command trajectories
     if (step_nb >= xs.size() - 1)
-      x_interp = xs[step_nb];
+      x_interp = xs.back();
     else
     {
       // Compute the differential between configuration
@@ -59,7 +59,7 @@ namespace simple_mpc
 
     // Interpolate state and command trajectories
     if (step_nb >= vecs.size() - 1)
-      vec_interp = vecs[step_nb];
+      vec_interp = vecs.back();
     else
     {
       vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress);

tests/interpolator.cpp

@@ -37,6 +37,20 @@ BOOST_AUTO_TEST_CASE(interpolate)
   interpolator.interpolate(delay, timestep, xs, x_interp);
 
   BOOST_CHECK(xs[2].isApprox(x_interp));
+
+  delay = 0.5;
+  interpolator.interpolate(delay, timestep, xs, x_interp);
+  BOOST_CHECK(xs.back().isApprox(x_interp));
+
+  delay = 0.005;
+  interpolator.interpolate(delay, timestep, xs, x_interp);
+  Eigen::VectorXd x_interp2(model.nq + model.nv);
+  Eigen::VectorXd dq(model.nv);
+  pinocchio::difference(model, xs[0].head(model.nq), xs[1].head(model.nq), dq);
+  pinocchio::integrate(model, xs[0].head(model.nq), dq * 0.5, x_interp2.head(model.nq));
+  x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5;
+
+  BOOST_CHECK(x_interp2.isApprox(x_interp));
 }
 
 BOOST_AUTO_TEST_CASE(linear_interpolate)
@@ -62,8 +76,18 @@ BOOST_AUTO_TEST_CASE(linear_interpolate)
 
   Eigen::VectorXd v_interp(model.nv);
   interpolator.interpolate(delay, timestep, vs, v_interp);
-
   BOOST_CHECK(vs[2].isApprox(v_interp));
+
+  delay = 0.5;
+  interpolator.interpolate(delay, timestep, vs, v_interp);
+  BOOST_CHECK(vs.back().isApprox(v_interp));
+
+  delay = 0.005;
+  interpolator.interpolate(delay, timestep, vs, v_interp);
+  Eigen::VectorXd v_interp2(model.nv);
+  v_interp2 = (vs[0] + vs[1]) * 0.5;
+
+  BOOST_CHECK(v_interp2.isApprox(v_interp));
 }
 
 BOOST_AUTO_TEST_SUITE_END()