Add interpolate state function

bindings/expose-interpolate.cpp

@@ -26,6 +26,15 @@ namespace simple_mpc
       return q_interp;
     }
 
+    Eigen::VectorXd interpolateStateProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & xs)
+    {
+      Eigen::VectorXd x_interp(xs[0].size());
+      self.interpolateState(delay, timestep, xs, x_interp);
+
+      return x_interp;
+    }
+
     Eigen::VectorXd interpolateLinearProxy(
       Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & vs)
     {
@@ -39,6 +48,7 @@ namespace simple_mpc
     {
       bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
         .def("interpolateConfiguration", &interpolateConfigurationProxy)
+        .def("interpolateState", &interpolateStateProxy)
         .def("interpolateLinear", &interpolateLinearProxy);
     }
   } // namespace python

examples/go2_fulldynamics.py

@@ -249,8 +249,7 @@
 
     forces = [total_forces, total_forces]
     ddqs = [a0, a1]
-    qss = [mpc.xs[0][:model_handler.getModel().nq], mpc.xs[1][:model_handler.getModel().nq]]
-    vss = [mpc.xs[0][model_handler.getModel().nq:], mpc.xs[1][model_handler.getModel().nq:]]
+    xss = [mpc.xs[0], mpc.xs[1]]
     uss = [mpc.us[0], mpc.us[1]]
 
     FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
@@ -269,14 +268,11 @@
         # time.sleep(0.01)
         delay = j / float(N_simu) * dt
 
-        q_interp = interpolator.interpolateConfiguration(delay, dt, qss)
-        v_interp = interpolator.interpolateLinear(delay, dt, vss)
+        x_interp = interpolator.interpolateState(delay, dt, xss)
         u_interp = interpolator.interpolateLinear(delay, dt, uss)
         acc_interp = interpolator.interpolateLinear(delay, dt, ddqs)
         force_interp = interpolator.interpolateLinear(delay, dt, forces)
 
-        x_interp = np.concatenate((q_interp, v_interp))
-
         q_meas, v_meas = device.measureState()
         x_measured = np.concatenate([q_meas, v_meas])
 

include/simple-mpc/interpolator.hpp

@@ -31,6 +31,12 @@ namespace simple_mpc
       const std::vector<Eigen::VectorXd> & qs,
       Eigen::Ref<Eigen::VectorXd> q_interp);
 
+    void interpolateState(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> & xs,
+      Eigen::Ref<Eigen::VectorXd> x_interp);
+
     void interpolateLinear(
       const double delay,
       const double timestep,

src/interpolator.cpp

@@ -36,6 +36,30 @@ namespace simple_mpc
     }
   }
 
+  void Interpolator::interpolateState(
+    const double delay,
+    const double timestep,
+    const std::vector<Eigen::VectorXd> & xs,
+    Eigen::Ref<Eigen::VectorXd> x_interp)
+  {
+    assert(("State is not of the right size", xs[0].size() == model_.nq + model_.nv));
+
+    // Compute the time knot corresponding to the current delay
+    size_t step_nb = static_cast<size_t>(delay / timestep);
+    double step_progress = (delay - (double)step_nb * timestep) / timestep;
+
+    // Interpolate state trajectory
+    if (step_nb >= xs.size() - 1)
+      x_interp = xs.back();
+    else
+    {
+      x_interp.head(model_.nq) =
+        pinocchio::interpolate(model_, xs[step_nb].head(model_.nq), xs[step_nb + 1].head(model_.nq), step_progress);
+      x_interp.tail(model_.nv) =
+        xs[step_nb + 1].tail(model_.nv) * step_progress + xs[step_nb].tail(model_.nv) * (1. - step_progress);
+    }
+  }
+
   void Interpolator::interpolateLinear(
     const double delay,
     const double timestep,

tests/interpolator.cpp

@@ -60,6 +60,45 @@ BOOST_AUTO_TEST_CASE(interpolate)
   interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
   BOOST_CHECK(qs2[0].isApprox(q_interp));
 
+  // Test state interpolation method
+  std::vector<Eigen::VectorXd> xs;
+  for (std::size_t i = 0; i < 4; i++)
+  {
+    Eigen::VectorXd x0(model.nq + model.nv);
+    x0.head(model.nq) = pinocchio::neutral(model);
+    Eigen::VectorXd dq(model.nv);
+    dq.setRandom();
+    pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq));
+    x0.tail(model.nv).setRandom();
+
+    xs.push_back(x0);
+  }
+  delay = 0.02;
+
+  Eigen::VectorXd x_interp(model.nq + model.nv);
+  interpolator.interpolateState(delay, timestep, xs, x_interp);
+  BOOST_CHECK(xs[2].isApprox(x_interp));
+
+  delay = 0.5;
+  interpolator.interpolateState(delay, timestep, xs, x_interp);
+  BOOST_CHECK(xs.back().isApprox(x_interp));
+
+  delay = 0.005;
+  interpolator.interpolateState(delay, timestep, xs, x_interp);
+  Eigen::VectorXd x_interp2(model.nq + 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));
+
+  std::vector<Eigen::VectorXd> xs2;
+  for (std::size_t i = 0; i < 2; i++)
+  {
+    xs2.push_back(xs[0]);
+  }
+  interpolator.interpolateState(delay, timestep, xs2, x_interp);
+  BOOST_CHECK(xs2[0].isApprox(x_interp));
+
   // Test linear interpolation method
   std::vector<Eigen::VectorXd> vs;
   for (std::size_t i = 0; i < 4; i++)