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,45 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 interpolateConfigurationProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & qs)
+    {
+      Eigen::VectorXd q_interp(qs[0].size());
+      self.interpolateConfiguration(delay, timestep, qs, q_interp);
+
+      return q_interp;
+    }
+
+    Eigen::VectorXd interpolateLinearProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & vs)
+    {
+      Eigen::VectorXd v_interp(vs[0].size());
+      self.interpolateLinear(delay, timestep, vs, v_interp);
+
+      return v_interp;
+    }
+
+    void exposeInterpolator()
+    {
+      bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
+        .def("interpolateConfiguration", &interpolateConfigurationProxy)
+        .def("interpolateLinear", &interpolateLinearProxy);
+    }
+  } // 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,7 @@ namespace simple_mpc::python
   void exposeMPC();
   void exposeIDSolver();
   void exposeIKIDSolver();
+  void exposeInterpolator();
   void exposeFrictionCompensation();
 
   /* PYTHON MODULE */
@@ -36,6 +37,7 @@ namespace simple_mpc::python
     exposeMPC();
     exposeIDSolver();
     exposeIKIDSolver();
+    exposeInterpolator();
     exposeFrictionCompensation();
   }
 

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, FrictionCompensation
+from simple_mpc import (
+    RobotModelHandler,
+    RobotDataHandler,
+    FullDynamicsOCP,
+    MPC,
+    IDSolver,
+    Interpolator,
+    FrictionCompensation
+)
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -26,6 +34,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 +154,8 @@
 
 """ Friction """
 fcompensation = FrictionCompensation(model_handler.getModel(), True)
+""" Interpolation """
+interpolator = Interpolator(model_handler.getModel())
 
 """ Initialize simulation"""
 device = BulletRobot(
@@ -155,8 +167,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 +247,12 @@
     force_RL.append(RL_f)
     force_RR.append(RR_f)
 
+    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:]]
+    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 +267,15 @@
 
     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
+
+        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])
@@ -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,50 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 <pinocchio/algorithm/joint-configuration.hpp>
+
+#include "simple-mpc/fwd.hpp"
+#include "simple-mpc/model-utils.hpp"
+
+namespace simple_mpc
+{
+  class Interpolator
+  {
+  public:
+    explicit Interpolator(const Model & model);
+
+    void interpolateConfiguration(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> & qs,
+      Eigen::Ref<Eigen::VectorXd> q_interp);
+
+    void interpolateLinear(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> & vs,
+      Eigen::Ref<Eigen::VectorXd> v_interp);
+
+    // Pinocchio model
+    Model model_;
+  };
+
+} // 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_

src/interpolator.cpp

@@ -0,0 +1,58 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 2-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+#include "simple-mpc/interpolator.hpp"
+
+namespace simple_mpc
+{
+
+  Interpolator::Interpolator(const Model & model)
+  {
+    model_ = model;
+  }
+
+  void Interpolator::interpolateConfiguration(
+    const double delay,
+    const double timestep,
+    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 configuration trajectory
+    if (step_nb >= qs.size() - 1)
+      q_interp = qs.back();
+    else
+    {
+      q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress);
+    }
+  }
+
+  void Interpolator::interpolateLinear(
+    const double delay,
+    const double timestep,
+    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 configuration trajectory
+    if (step_nb >= vs.size() - 1)
+      v_interp = vs.back();
+    else
+    {
+      v_interp = vs[step_nb + 1] * step_progress + vs[step_nb] * (1. - step_progress);
+    }
+  }
+
+} // namespace simple_mpc

src/lowlevel-control.cpp → src/qp-solvers.cpp

@@ -5,7 +5,7 @@
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
-#include "simple-mpc/lowlevel-control.hpp"
+#include "simple-mpc/qp-solvers.hpp"
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp>
 #include <proxsuite/proxqp/settings.hpp>
@@ -75,7 +75,7 @@ namespace simple_mpc
     }
 
     // Set the block matrix for torque limits
-    C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6);
+    // C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6);
 
     // Set size of solutions
     solved_forces_.resize(force_dim_);