Revert templatisation of interpolator

Author: earlaud

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.hxx

@@ -25,8 +25,11 @@ namespace simple_mpc
     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)
+    void 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));
 
@@ -43,8 +46,11 @@ namespace simple_mpc
       }
     }
 
-    template<typename T>
-    void interpolateState(const double delay, const double timestep, const std::vector<T> & xs, T & x_interp)
+    void interpolateState(
+      const double delay,
+      const double timestep,
+      const std::vector<Eigen::VectorXd> & xs,
+      Eigen::Ref<Eigen::VectorXd> x_interp)
     {
       assert(("State is not of the right size", xs[0].size() == model_.nq + model_.nv));
 
@@ -64,8 +70,11 @@ namespace simple_mpc
       }
     }
 
-    template<typename T>
-    void interpolateLinear(const double delay, const double timestep, const std::vector<T> & vs, T & v_interp)
+    void 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);
@@ -80,15 +89,18 @@ namespace simple_mpc
       }
     }
 
-    template<typename T>
-    void interpolateDiscrete(const double delay, const double timestep, const std::vector<T> & vs, T & v_interp)
+    void 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, vs.size() - 1);
+      step_nb = std::clamp(step_nb, 0UL, cs.size() - 1);
 
       // Set the output arg
-      v_interp = vs[step_nb];
+      c_interp = cs[step_nb];
     }
 
     // Pinocchio model

tests/interpolator.cpp

@@ -1,5 +1,6 @@
 
 #include <boost/test/unit_test.hpp>
+#include <vector>
 
 #include "simple-mpc/interpolator.hxx"
 #include "test_utils.cpp"
@@ -137,46 +138,48 @@ BOOST_AUTO_TEST_CASE(interpolate)
     BOOST_CHECK(vs2[0].isApprox(v_interp));
   }
 
-  // Test discrete interpolation method
+  // Test contacts interpolation method
   {
-    std::vector<Eigen::VectorXd> vs;
-    for (std::size_t i = 0; i < 4; i++)
+    const size_t n_contacts = 4;
+
+    std::vector<std::vector<bool>> cs;
+    for (std::size_t i = 0; i < n_contacts; i++)
     {
-      Eigen::VectorXd v0(model.nv);
-      v0.setRandom();
-      vs.push_back(v0);
+      std::vector<bool> c(n_contacts, false);
+      c[i] = true;
+      cs.push_back(c);
     }
-    Eigen::VectorXd v_interp(model.nv);
+    std::vector<bool> c_interp(n_contacts);
 
     // First element
-    interpolator.interpolateDiscrete(-0.001, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[0].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()));
 
-    interpolator.interpolateDiscrete(0, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[0].isApprox(v_interp));
+    interpolator.interpolateContacts(0, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
 
-    interpolator.interpolateDiscrete(0.001, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[0].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()));
 
     // Middle element
-    interpolator.interpolateDiscrete(0.1, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[1].isApprox(v_interp));
+    interpolator.interpolateContacts(0.1, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
 
-    interpolator.interpolateDiscrete(0.15, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[1].isApprox(v_interp));
+    interpolator.interpolateContacts(0.15, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
 
-    interpolator.interpolateDiscrete(0.2, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[2].isApprox(v_interp));
+    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.interpolateDiscrete(0.39, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[3].isApprox(v_interp));
+    interpolator.interpolateContacts(0.39, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
 
-    interpolator.interpolateDiscrete(0.4, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[3].isApprox(v_interp));
+    interpolator.interpolateContacts(0.4, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
 
-    interpolator.interpolateDiscrete(0.5, 0.1, vs, v_interp);
-    BOOST_CHECK(vs[3].isApprox(v_interp));
+    interpolator.interpolateContacts(0.5, 0.1, cs, c_interp);
+    BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
   }
 }