diff --git a/src/pyhpp/core/path.cc b/src/pyhpp/core/path.cc index b2e4969..fcf0e04 100644 --- a/src/pyhpp/core/path.cc +++ b/src/pyhpp/core/path.cc @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -223,6 +224,25 @@ void exposePath() { static_cast(&StraightPath::create))); + + class_, InterpolatedPathPtr_t, + boost::noncopyable>("InterpolatedPath", no_init) + .def("__init__", + make_constructor( + static_cast(&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 diff --git a/tests/unit/test_interpolated_path.py b/tests/unit/test_interpolated_path.py new file mode 100644 index 0000000..c1923bd --- /dev/null +++ b/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()