Update bindings of QuinticSpline:evaluatePoint

ami-iit · Nov 11, 2020 · 69fa094 · 69fa094
1 parent 5b00071
commit 69fa094
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 16 deletions.
diff --git a/bindings/python/QuinticSpline.cpp b/bindings/python/QuinticSpline.cpp
@@ -22,10 +22,20 @@ void BipedalLocomotion::bindings::CreateQuinticSpline(pybind11::module& module)
         .def("set_knots", &QuinticSpline::setKnots)
         .def("set_initial_conditions", &QuinticSpline::setInitialConditions)
         .def("set_final_conditions", &QuinticSpline::setFinalConditions)
-        .def("evaluate_point",
-             py::overload_cast<const double&,
-                               Eigen::Ref<Eigen::VectorXd>,
-                               Eigen::Ref<Eigen::VectorXd>,
-                               Eigen::Ref<Eigen::VectorXd>>(
-                 &QuinticSpline::evaluatePoint));
+        .def(
+            "evaluate_point",
+            [](QuinticSpline& qs,
+               const double time) -> std::tuple<Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd> {
+                Eigen::VectorXd pos(4);
+                Eigen::VectorXd vel(4);
+                Eigen::VectorXd acc(4);
+
+                if (!qs.evaluatePoint(time, pos, vel, acc))
+                {
+                    throw std::runtime_error("Failed to evaluate point");
+                }
+
+                return {pos, vel, acc};
+            },
+            py::arg("time"));
 }
diff --git a/bindings/python/tests/test_quintic_spline.py b/bindings/python/tests/test_quintic_spline.py
@@ -60,16 +60,13 @@ def test_quintic_spline():
 
     dT_check_points = (final_time - init_time) / points_to_check_number
 
-    position = np.zeros(4)
-    velocity = np.zeros(4)
-    acceleration = np.zeros(4)
-
     for i in range(int(points_to_check_number)):
 
         t = dT_check_points * i + init_time
 
-        assert spline.evaluate_point(t, position, velocity, acceleration)
+        position, velocity, acceleration = spline.evaluate_point(time=t)
 
+        # Check position
         expected = \
             coefficients[0] \
             + coefficients[1] * t \
@@ -78,23 +75,23 @@ def test_quintic_spline():
             + coefficients[4] * (t ** 4) \
             + coefficients[5] * (t ** 5)
 
-        assert np.allclose(expected.transpose()[0], position, atol=1e-5)
+        assert expected.transpose()[0] == pytest.approx(position, abs=1e-5)
 
-        # check velocity
+        # Check velocity
         expected = \
             coefficients[1] \
             + 2 * coefficients[2] * t \
             + 3 * coefficients[3] * (t ** 2) \
             + 4 * coefficients[4] * (t ** 3) \
             + 5 * coefficients[5] * (t ** 4)
 
-        assert np.allclose(expected.transpose()[0], velocity, atol=1e-5)
+        assert expected.transpose()[0] == pytest.approx(velocity, abs=1e-5)
 
-        # check acceleration
+        # Check acceleration
         expected = \
             2 * coefficients[2] \
             + 3 * 2 * coefficients[3] * t \
             + 4 * 3 * coefficients[4] * (t ** 2) \
             + 5 * 4 * coefficients[5] * (t ** 3)
 
-        assert np.allclose(expected.transpose()[0], acceleration, atol=1e-5)
+        assert expected.transpose()[0] == pytest.approx(acceleration, abs=1e-5)