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"]

bindings/expose-interpolate.cpp

@@ -44,12 +44,22 @@ namespace simple_mpc
       return v_interp;
     }
 
+    std::vector<bool> interpolateContactsProxy(
+      Interpolator & self, const double delay, const double timestep, const std::vector<std::vector<bool>> & cs)
+    {
+      std::vector<bool> c_interp(cs[0].size());
+      self.interpolateContacts(delay, timestep, cs, c_interp);
+
+      return c_interp;
+    }
+
     void exposeInterpolator()
     {
       bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
         .def("interpolateConfiguration", &interpolateConfigurationProxy)
         .def("interpolateState", &interpolateStateProxy)
-        .def("interpolateLinear", &interpolateLinearProxy);
+        .def("interpolateLinear", &interpolateLinearProxy)
+        .def("interpolateContacts", &interpolateContactsProxy);
     }
   } // namespace python
 } // namespace simple_mpc

include/simple-mpc/interpolator.hpp

@@ -10,8 +10,7 @@
  * @brief Interpolation class for practical control of the robot
  */
 
-#ifndef SIMPLE_MPC_INTERPOLATOR_HPP_
-#define SIMPLE_MPC_INTERPOLATOR_HPP_
+#pragma once
 
 #include <pinocchio/algorithm/joint-configuration.hpp>
 
@@ -23,7 +22,8 @@ namespace simple_mpc
   class Interpolator
   {
   public:
-    explicit Interpolator(const Model & model);
+    explicit Interpolator(const Model & model)
+    : model_(model) {};
 
     void interpolateConfiguration(
       const double delay,
@@ -43,14 +43,14 @@ namespace simple_mpc
       const std::vector<Eigen::VectorXd> & vs,
       Eigen::Ref<Eigen::VectorXd> v_interp);
 
+    void interpolateContacts(
+      const double delay,
+      const double timestep,
+      const std::vector<std::vector<bool>> & cs,
+      std::vector<bool> & c_interp);
+
     // Pinocchio model
     Model model_;
   };
 
 } // namespace simple_mpc
-
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-/* --- Details -------------------------------------------------------------- */
-
-#endif // SIMPLE_MPC_INTERPOLATOR_HPP_

src/interpolator.cpp

@@ -1,20 +1,7 @@
-///////////////////////////////////////////////////////////////////////////////
-// 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,
@@ -79,4 +66,14 @@ namespace simple_mpc
     }
   }
 
+  void Interpolator::interpolateContacts(
+    const double delay, const double timestep, const std::vector<std::vector<bool>> & cs, std::vector<bool> & c_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, cs.size() - 1);
+
+    // Set the output arg
+    c_interp = cs[step_nb];
+  }
 } // namespace simple_mpc

tests/interpolator.cpp

@@ -20,117 +20,166 @@ BOOST_AUTO_TEST_CASE(interpolate)
   Interpolator interpolator = Interpolator(model);
 
   // Test configuration interpolation method
-  std::vector<Eigen::VectorXd> qs;
-  for (std::size_t i = 0; i < 4; i++)
   {
-    Eigen::VectorXd q0(model.nq);
-    Eigen::VectorXd q1(model.nq);
-    q0 = pinocchio::neutral(model);
+    std::vector<Eigen::VectorXd> qs;
+    for (std::size_t i = 0; i < 4; i++)
+    {
+      Eigen::VectorXd q0(model.nq);
+      Eigen::VectorXd q1(model.nq);
+      q0 = pinocchio::neutral(model);
+      Eigen::VectorXd dq(model.nv);
+      dq.setRandom();
+      pinocchio::integrate(model, q0, dq, q1);
+      qs.push_back(q1);
+    }
+    Eigen::VectorXd q_interp(model.nq);
+
+    double delay = 0.02;
+    interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
+
+    BOOST_CHECK(qs[2].isApprox(q_interp));
+
+    delay = 0.5;
+    interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
+    BOOST_CHECK(qs.back().isApprox(q_interp));
+
+    delay = 0.005;
+    interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
+    Eigen::VectorXd q_interp2(model.nq);
     Eigen::VectorXd dq(model.nv);
-    dq.setRandom();
-    pinocchio::integrate(model, q0, dq, q1);
-
-    qs.push_back(q1);
-  }
-  double delay = 0.02;
-
-  Eigen::VectorXd q_interp(model.nq);
-  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
-
-  BOOST_CHECK(qs[2].isApprox(q_interp));
-
-  delay = 0.5;
-  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
-  BOOST_CHECK(qs.back().isApprox(q_interp));
-
-  delay = 0.005;
-  interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
-  Eigen::VectorXd q_interp2(model.nq);
-  Eigen::VectorXd dq(model.nv);
-  pinocchio::difference(model, qs[0], qs[1], dq);
-  pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2);
-
-  BOOST_CHECK(q_interp2.isApprox(q_interp));
-
-  std::vector<Eigen::VectorXd> qs2;
-  for (std::size_t i = 0; i < 2; i++)
-  {
-    qs2.push_back(qs[0]);
+    pinocchio::difference(model, qs[0], qs[1], dq);
+    pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2);
+
+    BOOST_CHECK(q_interp2.isApprox(q_interp));
+
+    std::vector<Eigen::VectorXd> qs2;
+    for (std::size_t i = 0; i < 2; i++)
+    {
+      qs2.push_back(qs[0]);
+    }
+    interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
+    BOOST_CHECK(qs2[0].isApprox(q_interp));
   }
-  interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
-  BOOST_CHECK(qs2[0].isApprox(q_interp));
 
   // Test state interpolation method
-  std::vector<Eigen::VectorXd> xs;
-  for (std::size_t i = 0; i < 4; i++)
   {
-    Eigen::VectorXd x0(model.nq + model.nv);
-    x0.head(model.nq) = pinocchio::neutral(model);
+    std::vector<Eigen::VectorXd> xs;
+    for (std::size_t i = 0; i < 4; i++)
+    {
+      Eigen::VectorXd x0(model.nq + model.nv);
+      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));
+      x0.tail(model.nv).setRandom();
+      xs.push_back(x0);
+    }
+    Eigen::VectorXd x_interp(model.nq + model.nv);
     Eigen::VectorXd dq(model.nv);
-    dq.setRandom();
-    pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq));
-    x0.tail(model.nv).setRandom();
 
-    xs.push_back(x0);
+    double delay = 0.02;
+    interpolator.interpolateState(delay, timestep, xs, x_interp);
+    BOOST_CHECK(xs[2].isApprox(x_interp));
+
+    delay = 0.5;
+    interpolator.interpolateState(delay, timestep, xs, x_interp);
+    BOOST_CHECK(xs.back().isApprox(x_interp));
+
+    delay = 0.005;
+    interpolator.interpolateState(delay, timestep, xs, x_interp);
+    Eigen::VectorXd x_interp2(model.nq + 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;
+    BOOST_CHECK(x_interp2.isApprox(x_interp));
+
+    std::vector<Eigen::VectorXd> xs2;
+    for (std::size_t i = 0; i < 2; i++)
+    {
+      xs2.push_back(xs[0]);
+    }
+    interpolator.interpolateState(delay, timestep, xs2, x_interp);
+    BOOST_CHECK(xs2[0].isApprox(x_interp));
   }
-  delay = 0.02;
-
-  Eigen::VectorXd x_interp(model.nq + model.nv);
-  interpolator.interpolateState(delay, timestep, xs, x_interp);
-  BOOST_CHECK(xs[2].isApprox(x_interp));
-
-  delay = 0.5;
-  interpolator.interpolateState(delay, timestep, xs, x_interp);
-  BOOST_CHECK(xs.back().isApprox(x_interp));
-
-  delay = 0.005;
-  interpolator.interpolateState(delay, timestep, xs, x_interp);
-  Eigen::VectorXd x_interp2(model.nq + 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;
-  BOOST_CHECK(x_interp2.isApprox(x_interp));
-
-  std::vector<Eigen::VectorXd> xs2;
-  for (std::size_t i = 0; i < 2; i++)
+
+  // Test linear interpolation method
   {
-    xs2.push_back(xs[0]);
+    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);
+    }
+    Eigen::VectorXd v_interp(model.nv);
+
+    double delay = 0.02;
+    interpolator.interpolateLinear(delay, timestep, vs, v_interp);
+    BOOST_CHECK(vs[2].isApprox(v_interp));
+
+    delay = 0.5;
+    interpolator.interpolateLinear(delay, timestep, vs, v_interp);
+    BOOST_CHECK(vs.back().isApprox(v_interp));
+
+    delay = 0.005;
+    interpolator.interpolateLinear(delay, timestep, vs, v_interp);
+    Eigen::VectorXd v_interp2(model.nv);
+    v_interp2 = (vs[0] + vs[1]) * 0.5;
+
+    BOOST_CHECK(v_interp2.isApprox(v_interp));
+
+    std::vector<Eigen::VectorXd> vs2;
+    for (std::size_t i = 0; i < 2; i++)
+    {
+      vs2.push_back(vs[0]);
+    }
+    interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
+    BOOST_CHECK(vs2[0].isApprox(v_interp));
   }
-  interpolator.interpolateState(delay, timestep, xs2, x_interp);
-  BOOST_CHECK(xs2[0].isApprox(x_interp));
 
-  // Test linear interpolation method
-  std::vector<Eigen::VectorXd> vs;
-  for (std::size_t i = 0; i < 4; i++)
+  // Test contacts interpolation method
   {
-    Eigen::VectorXd v0(model.nv);
-    v0.setRandom();
+    const size_t n_contacts = 4;
 
-    vs.push_back(v0);
-  }
-  delay = 0.02;
-  Eigen::VectorXd v_interp(model.nv);
-  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
-  BOOST_CHECK(vs[2].isApprox(v_interp));
+    std::vector<std::vector<bool>> cs;
+    for (std::size_t i = 0; i < n_contacts; i++)
+    {
+      std::vector<bool> c(n_contacts, false);
+      c[i] = true;
+      cs.push_back(c);
+    }
+    std::vector<bool> c_interp(n_contacts);
 
-  delay = 0.5;
-  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
-  BOOST_CHECK(vs.back().isApprox(v_interp));
+    // First element
+    interpolator.interpolateContacts(-0.001, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
 
-  delay = 0.005;
-  interpolator.interpolateLinear(delay, timestep, vs, v_interp);
-  Eigen::VectorXd v_interp2(model.nv);
-  v_interp2 = (vs[0] + vs[1]) * 0.5;
+    interpolator.interpolateContacts(0, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
 
-  BOOST_CHECK(v_interp2.isApprox(v_interp));
+    interpolator.interpolateContacts(0.001, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
 
-  std::vector<Eigen::VectorXd> vs2;
-  for (std::size_t i = 0; i < 2; i++)
-  {
-    vs2.push_back(vs[0]);
+    // Middle element
+    interpolator.interpolateContacts(0.1, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
+
+    interpolator.interpolateContacts(0.15, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
+
+    interpolator.interpolateContacts(0.2, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[2].begin()));
+
+    // Last element
+    interpolator.interpolateContacts(0.39, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
+
+    interpolator.interpolateContacts(0.4, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
+
+    interpolator.interpolateContacts(0.5, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
   }
-  interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
-  BOOST_CHECK(vs2[0].isApprox(v_interp));
 }
 
 BOOST_AUTO_TEST_SUITE_END()