Add interpolation class to simple_mpc

bindings/CMakeLists.txt

@@ -9,7 +9,8 @@ set(
   expose-robot-handler.cpp
   expose-problem.cpp
   expose-mpc.cpp
-  expose-lowlevel.cpp
+  expose-qp.cpp
+  expose-interpolate.cpp
   expose-centroidal.cpp
   expose-fulldynamics.cpp
   expose-kinodynamics.cpp

bindings/expose-interpolate.cpp

@@ -0,0 +1,51 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 2-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include <eigenpy/deprecation-policy.hpp>
+#include <eigenpy/std-vector.hpp>
+
+#include "simple-mpc/interpolator.hpp"
+
+namespace simple_mpc
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    Eigen::VectorXd stateInterpolateProxy(
+      StateInterpolator & 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;
+    }
+
+    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
+} // namespace simple_mpc

bindings/expose-lowlevel.cpp → bindings/expose-qp.cpp

@@ -10,7 +10,7 @@
 #include <eigenpy/deprecation-policy.hpp>
 #include <eigenpy/std-vector.hpp>
 
-#include "simple-mpc/lowlevel-control.hpp"
+#include "simple-mpc/qp-solvers.hpp"
 #include <proxsuite/proxqp/dense/dense.hpp>
 #include <proxsuite/proxqp/dense/wrapper.hpp>
 

bindings/module.cpp

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

examples/go2_fulldynamics.py

@@ -1,6 +1,15 @@
 import numpy as np
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation
+from simple_mpc import (
+    RobotModelHandler,
+    RobotDataHandler,
+    FullDynamicsOCP,
+    MPC,
+    IDSolver,
+    StateInterpolator,
+    LinearInterpolator,
+    FrictionCompensation
+)
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -26,6 +35,8 @@
 nu = nv - 6
 force_size = 3
 nk = len(model_handler.getFeetNames())
+nf = force_size
+
 gravity = np.array([0, 0, -9.81])
 fref = np.zeros(force_size)
 fref[2] = -model_handler.getMass() / nk * gravity[2]
@@ -144,6 +155,11 @@
 
 """ 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)
 
 """ Initialize simulation"""
 device = BulletRobot(
@@ -155,8 +171,6 @@
     model_handler.getReferenceState()[:3],
 )
 
-nq = mpc.getModelHandler().getModel().nq
-nv = mpc.getModelHandler().getModel().nv
 device.initializeJoints(model_handler.getReferenceState()[:nq])
 
 for i in range(40):
@@ -237,6 +251,11 @@
     force_RL.append(RL_f)
     force_RR.append(RR_f)
 
+    forces = [total_forces, total_forces]
+    ddqs = [a0, a1]
+    xss = [mpc.xs[0], mpc.xs[1]]
+    uss = [mpc.us[0], mpc.us[1]]
+
     FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
     FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation)
     RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation)
@@ -251,10 +270,12 @@
 
     for j in range(N_simu):
         # time.sleep(0.01)
-        u_interp = (N_simu - j) / N_simu * mpc.us[0] + j / N_simu * mpc.us[1]
-        a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1
-        x_interp = (N_simu - j) / N_simu * mpc.xs[0] + j / N_simu * mpc.xs[1]
-        K_interp = (N_simu - j) / N_simu * mpc.Ks[0] + j / N_simu * mpc.Ks[1]
+        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_meas, v_meas = device.measureState()
         x_measured = np.concatenate([q_meas, v_meas])
@@ -269,9 +290,9 @@
             mpc.getDataHandler().getData(),
             contact_states,
             x_measured[nq:],
-            a0,
+            acc_interp,
             current_torque,
-            total_forces,
+            force_interp,
             mpc.getDataHandler().getData().M,
         )
 

examples/go2_kinodynamics.py

@@ -1,6 +1,6 @@
 import numpy as np
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver
+from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver, Interpolator
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -21,6 +21,10 @@
 model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
 
+nq = model_handler.getModel().nq
+nv = model_handler.getModel().nv
+nu = nv - 6
+nf = 12
 force_size = 3
 nk = len(model_handler.getFeetNames())
 gravity = np.array([0, 0, -9.81])
@@ -144,6 +148,9 @@
 
 qp = IDSolver(id_conf, model_handler.getModel())
 
+""" Interpolation """
+interpolator = Interpolator(nq + nv, nv, nu, nf, 0.01)
+
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -155,10 +162,7 @@
 )
 
 device.initializeJoints(model_handler.getReferenceState()[:model_handler.getModel().nq])
-
 device.changeCamera(1.0, 60, -15, [0.6, -0.2, 0.5])
-nq = mpc.getModelHandler().getModel().nq
-nv = mpc.getModelHandler().getModel().nv
 
 q_meas, v_meas = device.measureState()
 x_measured  = np.concatenate([q_meas, v_meas])
@@ -234,6 +238,11 @@
     forces1 = mpc.us[1][: nk * force_size]
     contact_states = mpc.ocp_handler.getContactState(0)
 
+    forces = [forces0, forces1]
+    ddqs = [a0, a1]
+    xss = [mpc.xs[0], mpc.xs[1]]
+    uss = [mpc.us[0], mpc.us[1]]
+
     device.moveQuadrupedFeet(
         mpc.getReferencePose(0, "FL_foot").translation,
         mpc.getReferencePose(0, "FR_foot").translation,
@@ -243,21 +252,20 @@
 
     for j in range(N_simu):
         # time.sleep(0.01)
+        interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces)
+
         q_meas, v_meas = device.measureState()
         x_measured  = np.concatenate([q_meas, v_meas])
 
         mpc.getDataHandler().updateInternalData(x_measured, True)
 
-        a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1
-        f_interp = (N_simu - j) / N_simu * forces0 + j / N_simu * forces1
-
         qp.solveQP(
             mpc.getDataHandler().getData(),
             contact_states,
             x_measured[nq:],
-            a_interp,
+            interpolator.a_interpolated,
             np.zeros(12),
-            f_interp,
+            interpolator.forces_interpolated,
             mpc.getDataHandler().getData().M,
         )
 

include/simple-mpc/interpolator.hpp

@@ -0,0 +1,58 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+/**
+ * @file interpolator.hpp
+ * @brief Interpolation class for practical control of the robot
+ */
+
+#ifndef SIMPLE_MPC_INTERPOLATOR_HPP_
+#define SIMPLE_MPC_INTERPOLATOR_HPP_
+
+#include "simple-mpc/fwd.hpp"
+#include "simple-mpc/model-utils.hpp"
+
+namespace simple_mpc
+{
+  class StateInterpolator
+  {
+  public:
+    explicit StateInterpolator(const Model & model);
+
+    void interpolate(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> xs,
+      Eigen::Ref<Eigen::VectorXd> x_interp);
+
+    // Intermediate differential configuration
+    Eigen::VectorXd diff_q_;
+
+    // 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 -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+
+#endif // SIMPLE_MPC_INTERPOLATOR_HPP_

include/simple-mpc/lowlevel-control.hpp → include/simple-mpc/qp-solvers.hpp

@@ -6,13 +6,12 @@
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 /**
- * @file lowlevel-control.hpp
- * @brief Build a low-level control for kinodynamics
- * and centroidal MPC schemes
+ * @file qp-solvers.hpp
+ * @brief QP inverse dynamics for MPC schemes
  */
 
-#ifndef SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
-#define SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
+#ifndef SIMPLE_MPC_QP_SOLVERS_HPP_
+#define SIMPLE_MPC_QP_SOLVERS_HPP_
 
 #include "simple-mpc/deprecated.hpp"
 #include "simple-mpc/fwd.hpp"
@@ -241,4 +240,4 @@ namespace simple_mpc
 /* --- Details -------------------------------------------------------------- */
 /* --- Details -------------------------------------------------------------- */
 
-#endif // SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
+#endif // SIMPLE_MPC_QP_SOLVERS_HPP_