State and linear interpolators

Author: edantec

bindings/expose-interpolate.cpp

@@ -17,19 +17,34 @@ namespace simple_mpc
   {
     namespace bp = boost::python;
 
-    void exposeInterpolator()
+    Eigen::VectorXd stateInterpolateProxy(
+      StateInterpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> xs)
     {
-      bp::class_<Interpolator>(
-        "Interpolator", bp::init<const long, const long, const long, const long, const double>(
-                          bp::args("self", "nx", "nv", "nu", "nu", "MPC_timestep")))
-        .def("interpolate", &Interpolator::interpolate)
-        .add_property("MPC_timestep", &Interpolator::MPC_timestep_)
-        .add_property("x_interpolated", &Interpolator::x_interpolated_)
-        .add_property("u_interpolated", &Interpolator::u_interpolated_)
-        .add_property("a_interpolated", &Interpolator::a_interpolated_)
-        .add_property("forces_interpolated", &Interpolator::forces_interpolated_)
-        .add_property("step_nb", &Interpolator::step_nb_)
-        .add_property("step_progress", &Interpolator::step_progress_);
+      Eigen::VectorXd x_interp(xs[0].size());
+      self.interpolate(delay, timestep, xs, x_interp);
+
+      return x_interp;
+    }
+
+    Eigen::VectorXd linearInterpolateProxy(
+      LinearInterpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> xs)
+    {
+      Eigen::VectorXd x_interp(xs[0].size());
+      self.interpolate(delay, timestep, xs, x_interp);
+
+      return x_interp;
+    }
+
+    void exposeStateInterpolator()
+    {
+      bp::class_<StateInterpolator>("StateInterpolator", bp::init<const Model &>(bp::args("self", "model")))
+        .def("interpolate", &stateInterpolateProxy);
+    }
+
+    void exposeLinearInterpolator()
+    {
+      bp::class_<LinearInterpolator>("LinearInterpolator", bp::init<const size_t>(bp::args("self", "vec_size")))
+        .def("interpolate", &linearInterpolateProxy);
     }
 
   } // namespace python

examples/go2_fulldynamics.py

@@ -1,6 +1,14 @@
 import numpy as np
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, Interpolator
+from simple_mpc import (
+    RobotModelHandler,
+    RobotDataHandler,
+    FullDynamicsOCP,
+    MPC,
+    IDSolver,
+    StateInterpolator,
+    LinearInterpolator
+)
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -146,7 +154,10 @@
 qp = IDSolver(id_conf, model_handler.getModel())
 
 """ Interpolation """
-interpolator = Interpolator(nq + nv, nv, nu, nf, dt)
+state_interpolator = StateInterpolator(model_handler.getModel())
+acc_interpolator = LinearInterpolator(model_handler.getModel().nv)
+u_interpolator = LinearInterpolator(nu)
+force_interpolator = LinearInterpolator(nf)
 
 """ Initialize simulation"""
 device = BulletRobot(
@@ -257,24 +268,28 @@
 
     for j in range(N_simu):
         # time.sleep(0.01)
-        interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces)
+        delay = j / float(N_simu)
+        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_meas, v_meas = device.measureState()
         x_measured = np.concatenate([q_meas, v_meas])
 
         mpc.getDataHandler().updateInternalData(x_measured, True)
 
-        current_torque = interpolator.u_interpolated - 1. * mpc.Ks[0] @ model_handler.difference(
-            x_measured, interpolator.x_interpolated
+        current_torque = u_interp - 1. * mpc.Ks[0] @ model_handler.difference(
+            x_measured, x_interp
         )
 
         qp.solveQP(
             mpc.getDataHandler().getData(),
             contact_states,
             x_measured[nq:],
-            interpolator.a_interpolated,
+            acc_interp,
             current_torque,
-            interpolator.forces_interpolated,
+            force_interp,
             mpc.getDataHandler().getData().M,
         )
 

include/simple-mpc/interpolator.hpp

@@ -14,35 +14,40 @@
 #define SIMPLE_MPC_INTERPOLATOR_HPP_
 
 #include "simple-mpc/fwd.hpp"
+#include "simple-mpc/model-utils.hpp"
 
 namespace simple_mpc
 {
-  class Interpolator
+  class StateInterpolator
   {
   public:
-    explicit Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep);
+    explicit StateInterpolator(const Model & model);
 
     void interpolate(
       const double delay,
-      std::vector<Eigen::VectorXd> xs,
-      std::vector<Eigen::VectorXd> us,
-      std::vector<Eigen::VectorXd> ddqs,
-      std::vector<Eigen::VectorXd> forces);
+      const double timestep,
+      const std::vector<Eigen::VectorXd> xs,
+      Eigen::Ref<Eigen::VectorXd> x_interp);
 
-    // Interpolated trajectories
-    Eigen::VectorXd x_interpolated_;
-    Eigen::VectorXd u_interpolated_;
-    Eigen::VectorXd a_interpolated_;
-    Eigen::VectorXd forces_interpolated_;
+    // Intermediate differential configuration
+    Eigen::VectorXd diff_q_;
 
-    // Timestep of trajectories
-    double MPC_timestep_;
+    // Pinocchio model
+    Model model_;
+  };
+
+  class LinearInterpolator
+  {
+  public:
+    explicit LinearInterpolator(const size_t vec_size);
 
-    // Time knot associated with current delay
-    size_t step_nb_;
+    void interpolate(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> vecs,
+      Eigen::Ref<Eigen::VectorXd> vec_interp);
 
-    // Interpolation time between two knots
-    double step_progress_;
+    size_t vec_size_;
   };
 } // namespace simple_mpc
 

src/interpolator.cpp

@@ -10,42 +10,59 @@
 namespace simple_mpc
 {
 
-  Interpolator::Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep)
-  : MPC_timestep_(MPC_timestep)
+  StateInterpolator::StateInterpolator(const Model & model)
   {
-    x_interpolated_.resize(nx);
-    u_interpolated_.resize(nu);
-    a_interpolated_.resize(nv);
-    forces_interpolated_.resize(nf);
+    model_ = model;
+    diff_q_.resize(model_.nv);
   }
 
-  void Interpolator::interpolate(
+  void StateInterpolator::interpolate(
     const double delay,
-    std::vector<Eigen::VectorXd> xs,
-    std::vector<Eigen::VectorXd> us,
-    std::vector<Eigen::VectorXd> ddqs,
-    std::vector<Eigen::VectorXd> forces)
+    const double timestep,
+    const std::vector<Eigen::VectorXd> xs,
+    Eigen::Ref<Eigen::VectorXd> x_interp)
   {
     // Compute the time knot corresponding to the current delay
-    step_nb_ = static_cast<size_t>(delay / MPC_timestep_);
-    step_progress_ = (delay - (double)step_nb_ * MPC_timestep_) / MPC_timestep_;
+    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)
+    if (step_nb >= xs.size() - 1)
+      x_interp = xs[step_nb];
+    else
     {
-      step_nb_ = xs.size() - 1;
-      step_progress_ = 0.0;
-      x_interpolated_ = xs[step_nb_];
-      u_interpolated_ = us[step_nb_];
-      a_interpolated_ = ddqs[step_nb_];
-      forces_interpolated_ = forces[step_nb_];
+      // 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);
     }
+  }
+
+  LinearInterpolator::LinearInterpolator(const size_t vec_size)
+  {
+    vec_size_ = vec_size;
+  }
+
+  void LinearInterpolator::interpolate(
+    const double delay,
+    const double timestep,
+    const std::vector<Eigen::VectorXd> vecs,
+    Eigen::Ref<Eigen::VectorXd> vec_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[step_nb];
     else
     {
-      x_interpolated_ = xs[step_nb_ + 1] * step_progress_ + xs[step_nb_] * (1. - step_progress_);
-      u_interpolated_ = us[step_nb_ + 1] * step_progress_ + us[step_nb_] * (1. - step_progress_);
-      a_interpolated_ = ddqs[step_nb_ + 1] * step_progress_ + ddqs[step_nb_] * (1. - step_progress_);
-      forces_interpolated_ = forces[step_nb_ + 1] * step_progress_ + forces[step_nb_] * (1. - step_progress_);
+      vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress);
     }
   }
 

tests/interpolator.cpp

@@ -10,34 +10,60 @@ using namespace simple_mpc;
 
 BOOST_AUTO_TEST_CASE(interpolate)
 {
-  long nx = 27;
-  long nu = 12;
-  long nv = 18;
-  long nf = 12;
-  double MPC_timestep = 0.01;
-  Interpolator interpolator = Interpolator(nx, nv, nu, nf, MPC_timestep);
+  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;
+  StateInterpolator interpolator = StateInterpolator(model);
 
   std::vector<Eigen::VectorXd> xs;
-  std::vector<Eigen::VectorXd> us;
-  std::vector<Eigen::VectorXd> ddqs;
-  std::vector<Eigen::VectorXd> forces;
   for (std::size_t i = 0; i < 4; i++)
   {
-    xs.push_back(Eigen::Vector<double, 27>::Random());
-    us.push_back(Eigen::Vector<double, 12>::Random());
-    ddqs.push_back(Eigen::Vector<double, 18>::Random());
-    forces.push_back(Eigen::Vector<double, 12>::Random());
+    Eigen::VectorXd x0(model.nq + model.nv);
+    x0.tail(model.nv).setRandom();
+    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));
+
+    xs.push_back(x0);
+  }
+  double delay = 0.02;
+
+  Eigen::VectorXd x_interp(model.nq + model.nv);
+  interpolator.interpolate(delay, timestep, xs, x_interp);
+
+  BOOST_CHECK(xs[2].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);
+
+  std::vector<Eigen::VectorXd> vs;
+  for (std::size_t i = 0; i < 4; i++)
+  {
+    Eigen::VectorXd v0(model.nv);
+    v0.setRandom();
+
+    vs.push_back(v0);
   }
   double delay = 0.02;
 
-  interpolator.interpolate(delay, xs, us, ddqs, forces);
+  Eigen::VectorXd v_interp(model.nv);
+  interpolator.interpolate(delay, timestep, vs, v_interp);
 
-  BOOST_CHECK(xs[2].isApprox(interpolator.x_interpolated_));
-  BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_));
-  BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_));
-  BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_));
-  BOOST_CHECK(interpolator.step_nb_ == 2);
-  BOOST_CHECK(interpolator.step_progress_ == 0.);
+  BOOST_CHECK(vs[2].isApprox(v_interp));
 }
 
 BOOST_AUTO_TEST_SUITE_END()