From 2296fcb3e7dde61f2c7919a1bc821a98b487e9cb Mon Sep 17 00:00:00 2001 From: Diane Bury Date: Wed, 22 Feb 2023 15:17:45 +0100 Subject: [PATCH 1/4] fix spline steering method when path length != 1 --- src/steering-method/spline.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/steering-method/spline.cc b/src/steering-method/spline.cc index 51f8a0be..2c51e1e2 100644 --- a/src/steering-method/spline.cc +++ b/src/steering-method/spline.cc @@ -107,22 +107,22 @@ PathPtr_t Spline<_PB, _SO>::impl_compute( // Compute the matrices // TODO calls to basisFunctionDerivative could be cached as they do not // depend on the inputs. - SplinePath::timeFreeBasisFunctionDerivative(0, 0, coeffs.row(0).transpose()); + p->basisFunctionDerivative(0, 0, coeffs.row(0).transpose()); pinocchio::difference(device_.lock(), q1, p->base(), rhs.row(0)); for (std::size_t i = 0; i < order1.size(); ++i) - SplinePath::timeFreeBasisFunctionDerivative(order1[i], 0, + p->basisFunctionDerivative(order1[i], 0, coeffs.row(i + 1).transpose()); rhs.middleRows(1, order1.size()).transpose() = derivatives1; size_type row = 1 + order1.size(); - SplinePath::timeFreeBasisFunctionDerivative(0, 1, + p->basisFunctionDerivative(0, 1, coeffs.row(row).transpose()); pinocchio::difference(device_.lock(), q2, p->base(), rhs.row(row)); ++row; for (std::size_t i = 0; i < order2.size(); ++i) - SplinePath::timeFreeBasisFunctionDerivative( + p->basisFunctionDerivative( order2[i], 1, coeffs.row(i + row).transpose()); rhs.middleRows(row, order2.size()).transpose() = derivatives2; From c7f5058b64b8cbbe6bafa5898a6f085265b6ca53 Mon Sep 17 00:00:00 2001 From: Diane Bury Date: Wed, 22 Feb 2023 15:18:04 +0100 Subject: [PATCH 2/4] test: add spline steering method test --- tests/spline-path.cc | 57 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/tests/spline-path.cc b/tests/spline-path.cc index a0060e3d..e46eb677 100644 --- a/tests/spline-path.cc +++ b/tests/spline-path.cc @@ -59,6 +59,12 @@ DevicePtr_t createRobot() { return robot; } +DevicePtr_t createRobotArm() { + DevicePtr_t robot = unittest::makeDevice(unittest::ManipulatorArm2); + robot->controlComputation((Computation_t)(JOINT_POSITION | JACOBIAN)); + return robot; +} + typedef std::pair Pair_t; std::ostream& operator<<(std::ostream& os, const Pair_t& p) { @@ -193,6 +199,51 @@ void check_velocity_bounds() { } } +template +void check_steering_method() { + typedef steeringMethod::Spline SM_t; + std::vector orders{1}; + if (order == 2) + orders.push_back(2); + + // Use the manipulator arm and not Romeo since steering method does not give + // correct values for vel/acc when the robot configuration contains a freeflyer + DevicePtr_t dev = createRobotArm(); + BOOST_REQUIRE(dev); + ProblemPtr_t problem = Problem::create(dev); + + // Create random configurations and velocities/accelerations + Configuration_t q1(::pinocchio::randomConfiguration(dev->model())); + Configuration_t q2(::pinocchio::randomConfiguration(dev->model())); + matrix_t deriv1(matrix_t::Random(dev->numberDof(), order)), + deriv2(matrix_t::Random(dev->numberDof(), order)); + double length = static_cast (rand()) / static_cast (RAND_MAX); + + // Create spline + typename SM_t::Ptr_t sm(SM_t::create(problem)); + PathPtr_t spline1 = sm->steer(q1, orders, deriv1, q2, orders, deriv2, length); + + // Check length + double spline_length = spline1->length(); + BOOST_CHECK_MESSAGE(abs(spline_length - length) < 0.0001, + "Path does not have desired length: " << spline_length << " instead of " << length); + + // Check configuration at start/end + Configuration_t spline_q1 = spline1->initial(); + Configuration_t spline_q2 = spline1->end(); + EIGEN_VECTOR_IS_APPROX(q1, spline_q1, 1e-6); + EIGEN_VECTOR_IS_APPROX(q2, spline_q2, 1e-6); + + // Check derivatives at start/end + for (int i=1; i <= order; i++) { + vector_t spline_v1(vector_t::Random(dev->numberDof())), spline_v2 = spline_v1; + spline1->derivative(spline_v1, 0, i); + spline1->derivative(spline_v2, spline_length, i); + EIGEN_VECTOR_IS_APPROX(deriv1.col(i-1), spline_v1, 1e-6); + EIGEN_VECTOR_IS_APPROX(deriv2.col(i-1), spline_v2, 1e-6); + } +} + BOOST_AUTO_TEST_CASE(spline_bernstein) { compare_to_straight_path(); } @@ -200,3 +251,9 @@ BOOST_AUTO_TEST_CASE(spline_bernstein) { BOOST_AUTO_TEST_CASE(spline_bernstein_velocity) { check_velocity_bounds(); } + +BOOST_AUTO_TEST_CASE(steering_method) { + check_steering_method(); + check_steering_method(); + check_steering_method(); +} From cb545deda309ae46fb19739b86469de8c6d368c2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Feb 2023 15:11:35 +0000 Subject: [PATCH 3/4] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/steering-method/spline.cc | 9 +++------ tests/spline-path.cc | 22 ++++++++++++---------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/steering-method/spline.cc b/src/steering-method/spline.cc index 2c51e1e2..aefc71f1 100644 --- a/src/steering-method/spline.cc +++ b/src/steering-method/spline.cc @@ -111,19 +111,16 @@ PathPtr_t Spline<_PB, _SO>::impl_compute( pinocchio::difference(device_.lock(), q1, p->base(), rhs.row(0)); for (std::size_t i = 0; i < order1.size(); ++i) - p->basisFunctionDerivative(order1[i], 0, - coeffs.row(i + 1).transpose()); + p->basisFunctionDerivative(order1[i], 0, coeffs.row(i + 1).transpose()); rhs.middleRows(1, order1.size()).transpose() = derivatives1; size_type row = 1 + order1.size(); - p->basisFunctionDerivative(0, 1, - coeffs.row(row).transpose()); + p->basisFunctionDerivative(0, 1, coeffs.row(row).transpose()); pinocchio::difference(device_.lock(), q2, p->base(), rhs.row(row)); ++row; for (std::size_t i = 0; i < order2.size(); ++i) - p->basisFunctionDerivative( - order2[i], 1, coeffs.row(i + row).transpose()); + p->basisFunctionDerivative(order2[i], 1, coeffs.row(i + row).transpose()); rhs.middleRows(row, order2.size()).transpose() = derivatives2; // Solve the problem diff --git a/tests/spline-path.cc b/tests/spline-path.cc index e46eb677..b84816b3 100644 --- a/tests/spline-path.cc +++ b/tests/spline-path.cc @@ -203,11 +203,11 @@ template void check_steering_method() { typedef steeringMethod::Spline SM_t; std::vector orders{1}; - if (order == 2) - orders.push_back(2); + if (order == 2) orders.push_back(2); // Use the manipulator arm and not Romeo since steering method does not give - // correct values for vel/acc when the robot configuration contains a freeflyer + // correct values for vel/acc when the robot configuration contains a + // freeflyer DevicePtr_t dev = createRobotArm(); BOOST_REQUIRE(dev); ProblemPtr_t problem = Problem::create(dev); @@ -216,8 +216,8 @@ void check_steering_method() { Configuration_t q1(::pinocchio::randomConfiguration(dev->model())); Configuration_t q2(::pinocchio::randomConfiguration(dev->model())); matrix_t deriv1(matrix_t::Random(dev->numberDof(), order)), - deriv2(matrix_t::Random(dev->numberDof(), order)); - double length = static_cast (rand()) / static_cast (RAND_MAX); + deriv2(matrix_t::Random(dev->numberDof(), order)); + double length = static_cast(rand()) / static_cast(RAND_MAX); // Create spline typename SM_t::Ptr_t sm(SM_t::create(problem)); @@ -226,7 +226,8 @@ void check_steering_method() { // Check length double spline_length = spline1->length(); BOOST_CHECK_MESSAGE(abs(spline_length - length) < 0.0001, - "Path does not have desired length: " << spline_length << " instead of " << length); + "Path does not have desired length: " + << spline_length << " instead of " << length); // Check configuration at start/end Configuration_t spline_q1 = spline1->initial(); @@ -235,12 +236,13 @@ void check_steering_method() { EIGEN_VECTOR_IS_APPROX(q2, spline_q2, 1e-6); // Check derivatives at start/end - for (int i=1; i <= order; i++) { - vector_t spline_v1(vector_t::Random(dev->numberDof())), spline_v2 = spline_v1; + for (int i = 1; i <= order; i++) { + vector_t spline_v1(vector_t::Random(dev->numberDof())), + spline_v2 = spline_v1; spline1->derivative(spline_v1, 0, i); spline1->derivative(spline_v2, spline_length, i); - EIGEN_VECTOR_IS_APPROX(deriv1.col(i-1), spline_v1, 1e-6); - EIGEN_VECTOR_IS_APPROX(deriv2.col(i-1), spline_v2, 1e-6); + EIGEN_VECTOR_IS_APPROX(deriv1.col(i - 1), spline_v1, 1e-6); + EIGEN_VECTOR_IS_APPROX(deriv2.col(i - 1), spline_v2, 1e-6); } } From 739a093121e23f31ac229d1617ddcaf2478eaf90 Mon Sep 17 00:00:00 2001 From: Diane Bury Date: Tue, 28 Feb 2023 11:17:50 +0100 Subject: [PATCH 4/4] doc: add warning that splines do not correctly handle non-vector space --- include/hpp/core/path/spline.hh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/hpp/core/path/spline.hh b/include/hpp/core/path/spline.hh index 3e79ea42..df89ce6b 100644 --- a/include/hpp/core/path/spline.hh +++ b/include/hpp/core/path/spline.hh @@ -84,6 +84,9 @@ struct sbf_traits { /// /// The dimension of control points, corresponding to the robot number of /// degrees of freedom can be retrieved by getter Spline::parameterSize. +/// +/// \warning Velocities for robots with non-vector configuration space are +/// not correctly handled. template class HPP_CORE_DLLAPI Spline : public Path { public: