Add discrete interpollator and fix tests

.gersemirc

@@ -1,4 +1,4 @@
-definitions: [./CMakeLists.txt, ./benchmark, ./bindings, ./cmake, ./tests]
+definitions: [./CMakeLists.txt, ./benchmark, ./bindings, ./tests]
 line_length: 80
 indent: 2
 warn_about_unknown_commands: false

.pre-commit-config.yaml

@@ -30,3 +30,4 @@ repos:
     rev: 0.19.3
     hooks:
       - id: gersemi
+        args: ["--print-config=verbose"]

CMakeLists.txt

@@ -96,7 +96,7 @@ include(dependencies.cmake)
 
 # Main Library
 file(GLOB mpc_SOURCE CONFIGURE_DEPENDS src/*.cpp)
-file(GLOB mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp)
+file(GLOB mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp include/${PROJECT_NAME}/*.hxx)
 
 add_library(${PROJECT_NAME} SHARED ${mpc_HEADER} ${mpc_SOURCE})
 target_include_directories(

bindings/expose-interpolate.cpp

@@ -9,7 +9,7 @@
 #include <eigenpy/deprecation-policy.hpp>
 #include <eigenpy/std-vector.hpp>
 
-#include "simple-mpc/interpolator.hpp"
+#include "simple-mpc/interpolator.hxx"
 
 namespace simple_mpc
 {

include/simple-mpc/interpolator.hxx

@@ -0,0 +1,98 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 3-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+/**
+ * @file interpolator.hxx
+ * @brief Interpolation class for practical control of the robot
+ */
+
+#pragma once
+
+#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)
+    : model_(model) {};
+
+    template<typename T>
+    void interpolateConfiguration(const double delay, const double timestep, const std::vector<T> & qs, T & 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);
+      }
+    }
+
+    template<typename T>
+    void interpolateState(const double delay, const double timestep, const std::vector<T> & xs, T & 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);
+      }
+    }
+
+    template<typename T>
+    void interpolateLinear(const double delay, const double timestep, const std::vector<T> & vs, T & 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);
+      }
+    }
+
+    template<typename T>
+    void interpolateDiscrete(const double delay, const double timestep, const std::vector<T> & vs, T & v_interp)
+    {
+      // Compute the time knot corresponding to the current delay
+      size_t step_nb = static_cast<size_t>(delay / timestep);
+      step_nb = std::clamp(step_nb, 0UL, vs.size() - 1);
+
+      // Set the output arg
+      v_interp = vs[step_nb];
+    }
+
+    // Pinocchio model
+    Model model_;
+  };
+
+} // namespace simple_mpc