One class for all interpolate methods

Author: edantec

bindings/expose-interpolate.cpp

@@ -17,35 +17,29 @@ namespace simple_mpc
   {
     namespace bp = boost::python;
 
-    Eigen::VectorXd stateInterpolateProxy(
-      StateInterpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> xs)
+    Eigen::VectorXd interpolateConfigurationProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & qs)
     {
-      Eigen::VectorXd x_interp(xs[0].size());
-      self.interpolate(delay, timestep, xs, x_interp);
+      Eigen::VectorXd q_interp(qs[0].size());
+      self.interpolateConfiguration(delay, timestep, qs, q_interp);
 
-      return x_interp;
+      return q_interp;
     }
 
-    Eigen::VectorXd linearInterpolateProxy(
-      LinearInterpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> xs)
+    Eigen::VectorXd interpolateLinearProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & vs)
     {
-      Eigen::VectorXd x_interp(xs[0].size());
-      self.interpolate(delay, timestep, xs, x_interp);
+      Eigen::VectorXd v_interp(vs[0].size());
+      self.interpolateLinear(delay, timestep, vs, v_interp);
 
-      return x_interp;
+      return v_interp;
     }
 
-    void exposeStateInterpolator()
+    void exposeInterpolator()
     {
-      bp::class_<StateInterpolator>("StateInterpolator", bp::init<const Model &>(bp::args("self", "model")))
-        .def("interpolate", &stateInterpolateProxy);
+      bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
+        .def("interpolateConfiguration", &interpolateConfigurationProxy)
+        .def("interpolateLinear", &interpolateLinearProxy);
     }
-
-    void exposeLinearInterpolator()
-    {
-      bp::class_<LinearInterpolator>("LinearInterpolator", bp::init<const size_t>(bp::args("self", "vec_size")))
-        .def("interpolate", &linearInterpolateProxy);
-    }
-
   } // namespace python
 } // namespace simple_mpc

bindings/module.cpp

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

examples/go2_fulldynamics.py

@@ -6,8 +6,7 @@
     FullDynamicsOCP,
     MPC,
     IDSolver,
-    StateInterpolator,
-    LinearInterpolator,
+    Interpolator,
     FrictionCompensation
 )
 import example_robot_data as erd
@@ -156,10 +155,7 @@
 """ Friction """
 fcompensation = FrictionCompensation(model_handler.getModel(), True)
 """ Interpolation """
-state_interpolator = StateInterpolator(model_handler.getModel())
-acc_interpolator = LinearInterpolator(model_handler.getModel().nv)
-u_interpolator = LinearInterpolator(nu)
-force_interpolator = LinearInterpolator(nf)
+interpolator = Interpolator(model_handler.getModel())
 
 """ Initialize simulation"""
 device = BulletRobot(
@@ -253,7 +249,8 @@
 
     forces = [total_forces, total_forces]
     ddqs = [a0, a1]
-    xss = [mpc.xs[0], mpc.xs[1]]
+    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:]]
     uss = [mpc.us[0], mpc.us[1]]
 
     FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
@@ -272,10 +269,13 @@
         # time.sleep(0.01)
         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)
-        force_interp = force_interpolator.interpolate(delay, dt, forces)
+        q_interp = interpolator.interpolateConfiguration(delay, dt, qss)
+        v_interp = interpolator.interpolateLinear(delay, dt, vss)
+        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

@@ -13,42 +13,34 @@
 #ifndef SIMPLE_MPC_INTERPOLATOR_HPP_
 #define SIMPLE_MPC_INTERPOLATOR_HPP_
 
+#include <pinocchio/algorithm/joint-configuration.hpp>
+
 #include "simple-mpc/fwd.hpp"
 #include "simple-mpc/model-utils.hpp"
 
 namespace simple_mpc
 {
-  class StateInterpolator
+  class Interpolator
   {
   public:
-    explicit StateInterpolator(const Model & model);
+    explicit Interpolator(const Model & model);
 
-    void interpolate(
+    void interpolateConfiguration(
       const double delay,
       const double timestep,
-      const std::vector<Eigen::VectorXd> xs,
-      Eigen::Ref<Eigen::VectorXd> x_interp);
+      const std::vector<Eigen::VectorXd> & qs,
+      Eigen::Ref<Eigen::VectorXd> q_interp);
 
-    // Intermediate differential configuration
-    Eigen::VectorXd diff_q_;
+    void interpolateLinear(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> & vs,
+      Eigen::Ref<Eigen::VectorXd> v_interp);
 
     // Pinocchio model
     Model model_;
   };
 
-  class LinearInterpolator
-  {
-  public:
-    explicit LinearInterpolator(const size_t vec_size);
-
-    void interpolate(
-      const double delay,
-      const double timestep,
-      const std::vector<Eigen::VectorXd> vecs,
-      Eigen::Ref<Eigen::VectorXd> vec_interp);
-
-    size_t vec_size_;
-  };
 } // namespace simple_mpc
 
 /* --- Details -------------------------------------------------------------- */

src/interpolator.cpp

@@ -6,64 +6,52 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 #include "simple-mpc/interpolator.hpp"
-#include <pinocchio/algorithm/joint-configuration.hpp>
 
 namespace simple_mpc
 {
 
-  StateInterpolator::StateInterpolator(const Model & model)
+  Interpolator::Interpolator(const Model & model)
   {
     model_ = model;
-    diff_q_.resize(model_.nv);
   }
 
-  void StateInterpolator::interpolate(
+  void Interpolator::interpolateConfiguration(
     const double delay,
     const double timestep,
-    const std::vector<Eigen::VectorXd> xs,
-    Eigen::Ref<Eigen::VectorXd> x_interp)
+    const std::vector<Eigen::VectorXd> & qs,
+    Eigen::Ref<Eigen::VectorXd> q_interp)
   {
+    assert(("Configuration is not of the right size", qs[0].size() == model_.nq));
+
     // 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 and command trajectories
-    if (step_nb >= xs.size() - 1)
-      x_interp = xs.back();
+    // Interpolate configuration trajectory
+    if (step_nb >= qs.size() - 1)
+      q_interp = qs.back();
     else
     {
-      // Compute the differential between configuration
-      diff_q_ = pinocchio::difference(model_, xs[step_nb].head(model_.nq), xs[step_nb + 1].head(model_.nq));
-
-      pinocchio::integrate(model_, xs[step_nb].head(model_.nq), diff_q_ * step_progress, x_interp.head(model_.nq));
-
-      // Compute velocity interpolation
-      x_interp.tail(model_.nv) =
-        xs[step_nb + 1].tail(model_.nv) * step_progress + xs[step_nb].tail(model_.nv) * (1. - step_progress);
+      q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress);
     }
   }
 
-  LinearInterpolator::LinearInterpolator(const size_t vec_size)
-  {
-    vec_size_ = vec_size;
-  }
-
-  void LinearInterpolator::interpolate(
+  void Interpolator::interpolateLinear(
     const double delay,
     const double timestep,
-    const std::vector<Eigen::VectorXd> vecs,
-    Eigen::Ref<Eigen::VectorXd> vec_interp)
+    const std::vector<Eigen::VectorXd> & vs,
+    Eigen::Ref<Eigen::VectorXd> v_interp)
   {
     // 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 and command trajectories
-    if (step_nb >= vecs.size() - 1)
-      vec_interp = vecs.back();
+    // Interpolate configuration trajectory
+    if (step_nb >= vs.size() - 1)
+      v_interp = vs.back();
     else
     {
-      vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress);
+      v_interp = vs[step_nb + 1] * step_progress + vs[step_nb] * (1. - step_progress);
     }
   }
 

tests/interpolator.cpp

@@ -17,61 +17,50 @@ BOOST_AUTO_TEST_CASE(interpolate)
 
   pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model);
   double timestep = 0.01;
-  StateInterpolator interpolator = StateInterpolator(model);
+  Interpolator interpolator = Interpolator(model);
 
-  std::vector<Eigen::VectorXd> xs;
+  // Test configuration interpolation method
+  std::vector<Eigen::VectorXd> qs;
   for (std::size_t i = 0; i < 4; i++)
   {
-    Eigen::VectorXd x0(model.nq + model.nv);
-    x0.tail(model.nv).setRandom();
-    x0.head(model.nq) = pinocchio::neutral(model);
+    Eigen::VectorXd q0(model.nq);
+    Eigen::VectorXd q1(model.nq);
+    q0 = pinocchio::neutral(model);
     Eigen::VectorXd dq(model.nv);
     dq.setRandom();
-    pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq));
+    pinocchio::integrate(model, q0, dq, q1);
 
-    xs.push_back(x0);
+    qs.push_back(q1);
   }
   double delay = 0.02;
 
-  Eigen::VectorXd x_interp(model.nq + model.nv);
-  interpolator.interpolate(delay, timestep, xs, x_interp);
+  Eigen::VectorXd q_interp(model.nq);
+  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
 
-  BOOST_CHECK(xs[2].isApprox(x_interp));
+  BOOST_CHECK(qs[2].isApprox(q_interp));
 
   delay = 0.5;
-  interpolator.interpolate(delay, timestep, xs, x_interp);
-  BOOST_CHECK(xs.back().isApprox(x_interp));
+  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
+  BOOST_CHECK(qs.back().isApprox(q_interp));
 
   delay = 0.005;
-  interpolator.interpolate(delay, timestep, xs, x_interp);
-  Eigen::VectorXd x_interp2(model.nq + model.nv);
+  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
+  Eigen::VectorXd q_interp2(model.nq);
   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;
+  pinocchio::difference(model, qs[0], qs[1], dq);
+  pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2);
 
-  BOOST_CHECK(x_interp2.isApprox(x_interp));
+  BOOST_CHECK(q_interp2.isApprox(q_interp));
 
-  std::vector<Eigen::VectorXd> xs2;
+  std::vector<Eigen::VectorXd> qs2;
   for (std::size_t i = 0; i < 2; i++)
   {
-    xs2.push_back(xs[0]);
+    qs2.push_back(qs[0]);
   }
-  interpolator.interpolate(delay, timestep, xs2, x_interp);
-  BOOST_CHECK(xs2[0].isApprox(x_interp));
-}
-
-BOOST_AUTO_TEST_CASE(linear_interpolate)
-{
-  Model model;
-  // Load pinocchio model from example robot data
-  const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf";
-  const std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/srdf/solo.srdf";
-
-  pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model);
-  double timestep = 0.01;
-  LinearInterpolator interpolator = LinearInterpolator((size_t)model.nv);
+  interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
+  BOOST_CHECK(qs2[0].isApprox(q_interp));
 
+  // Test linear interpolation method
   std::vector<Eigen::VectorXd> vs;
   for (std::size_t i = 0; i < 4; i++)
   {
@@ -80,18 +69,17 @@ BOOST_AUTO_TEST_CASE(linear_interpolate)
 
     vs.push_back(v0);
   }
-  double delay = 0.02;
-
+  delay = 0.02;
   Eigen::VectorXd v_interp(model.nv);
-  interpolator.interpolate(delay, timestep, vs, v_interp);
+  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
   BOOST_CHECK(vs[2].isApprox(v_interp));
 
   delay = 0.5;
-  interpolator.interpolate(delay, timestep, vs, v_interp);
+  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
   BOOST_CHECK(vs.back().isApprox(v_interp));
 
   delay = 0.005;
-  interpolator.interpolate(delay, timestep, vs, v_interp);
+  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
   Eigen::VectorXd v_interp2(model.nv);
   v_interp2 = (vs[0] + vs[1]) * 0.5;
 
@@ -102,7 +90,7 @@ BOOST_AUTO_TEST_CASE(linear_interpolate)
   {
     vs2.push_back(vs[0]);
   }
-  interpolator.interpolate(delay, timestep, vs2, v_interp);
+  interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
   BOOST_CHECK(vs2[0].isApprox(v_interp));
 }