Devel

src/pyhpp/core/path.cc

@@ -30,6 +30,7 @@
 
 #include <boost/python.hpp>
 #include <eigenpy/eigenpy.hpp>
+#include <hpp/core/interpolated-path.hh>
 #include <hpp/core/path.hh>
 #include <hpp/core/straight-path.hh>
 #include <hpp/python/config.hh>
@@ -223,6 +224,25 @@ void exposePath() {
                static_cast<StraightPathPtr_t (*)(
                    const DevicePtr_t&, ConfigurationIn_t, ConfigurationIn_t,
                    interval_t, ConstraintSetPtr_t)>(&StraightPath::create)));
+
+  class_<InterpolatedPath, bases<Path>, InterpolatedPathPtr_t,
+         boost::noncopyable>("InterpolatedPath", no_init)
+      .def("__init__",
+           make_constructor(
+               static_cast<InterpolatedPathPtr_t (*)(
+                   const DevicePtr_t&, ConfigurationIn_t, ConfigurationIn_t,
+                   interval_t)>(&InterpolatedPath::create)))
+      .def("insert", &InterpolatedPath::insert,
+           "Insert a configuration at the given path parameter.")
+      .def(
+          "interpolationPoints",
+          +[](const InterpolatedPath& path) {
+            list points;
+            for (const auto& point : path.interpolationPoints())
+              points.append(make_tuple(point.first, point.second));
+            return points;
+          },
+          "Return ordered (parameter, configuration) interpolation points.");
 }
 }  // namespace core
 }  // namespace pyhpp

tests/unit/test_interpolated_path.py

@@ -0,0 +1,35 @@
+"""Interpolation points remain available when paths are copied or extracted."""
+
+import unittest
+
+import numpy as np
+from pyhpp.core import InterpolatedPath, interval
+from pyhpp.core.path import Vector
+from unit.conftest import create_ur5_problem
+
+
+class TestInterpolatedPath(unittest.TestCase):
+    def test_preserves_points_through_vector(self):
+        _, robot = create_ur5_problem()
+        q0 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0])
+        q1 = q0.copy()
+        q1[0] = 0.5
+        path = InterpolatedPath(robot, q0, q0, interval(0.0, 2.0))
+        path.insert(1.0, q1)
+        vector = Vector(robot.configSize(), robot.numberDof())
+        vector.appendPath(path)
+
+        retrieved = vector.pathAtRank(0)
+        self.assertIsInstance(retrieved, InterpolatedPath)
+        points = retrieved.interpolationPoints()
+        self.assertEqual([t for t, _ in points], [0.0, 1.0, 2.0])
+        np.testing.assert_allclose(points[1][1], q1)
+        for start, end in ((0.0, 1.0), (1.0, 2.0)):
+            piece = retrieved.extract(start, end)
+            self.assertEqual(len(piece.interpolationPoints()), 2)
+        points[1][1][0] = 99.0
+        np.testing.assert_allclose(retrieved(1.0)[0], q1)
+
+
+if __name__ == "__main__":
+    unittest.main()