diff --git a/bindings/python/IK/src/GravityTask.cpp b/bindings/python/IK/src/GravityTask.cpp index e0e39cd2cf..624e194d3c 100644 --- a/bindings/python/IK/src/GravityTask.cpp +++ b/bindings/python/IK/src/GravityTask.cpp @@ -29,12 +29,6 @@ void CreateGravityTask(pybind11::module& module) py::class_, IKLinearTask>(module, "GravityTask") .def(py::init()) - .def("set_desired_gravity_direction_in_target_frame", - &GravityTask::setDesiredGravityDirectionInTargetFrame, - py::arg("desired_gravity_direction")) - .def("set_feedforward_velocity_in_target_frame", - &GravityTask::setFeedForwardVelocityInTargetFrame, - py::arg("feedforward_velocity")) .def("set_set_point", &GravityTask::setSetPoint, py::arg("desired_gravity_direction"), diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index ed06bf4494..cf1dc894f7 100644 --- a/src/IK/include/BipedalLocomotion/IK/GravityTask.h +++ b/src/IK/include/BipedalLocomotion/IK/GravityTask.h @@ -127,6 +127,25 @@ class GravityTask : public IKLinearTask iDynTree::FrameIndex m_targetFrameIndex; /**< Index of the target frame. */ bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set-point has been set at least once */ + /** + * @brief Set the desired gravity direction expressed the target frame. + * @param desiredGravityDirection The desired gravity direction. + * @return True in case of success, false otherwise. + * + * The input is normalized, unless the norm is too small. + */ + bool setDesiredGravityDirectionInTargetFrame( + const Eigen::Ref desiredGravityDirection); + + /** + * @brief Set the feedforward angular velocity expressed in the target frame. + * @param feedforwardVelocity The desired feedforward velocity in rad/s. + * @return True in case of success, false otherwise. + * Only the first two components are used. + */ + bool setFeedForwardVelocityInTargetFrame( // + const Eigen::Ref feedforwardVelocity); + public: // clang-format off /** @@ -170,25 +189,6 @@ class GravityTask : public IKLinearTask */ bool update() override; - /** - * @brief Set the desired gravity direction expressed the target frame. - * @param desiredGravityDirection The desired gravity direction. - * @return True in case of success, false otherwise. - * - * The input is normalized, unless the norm is too small. - */ - bool setDesiredGravityDirectionInTargetFrame( - const Eigen::Ref desiredGravityDirection); - - /** - * @brief Set the feedforward angular velocity expressed in the target frame. - * @param feedforwardVelocity The desired feedforward velocity in rad/s. - * @return True in case of success, false otherwise. - * Only the first two components are used. - */ - bool - setFeedForwardVelocityInTargetFrame(const Eigen::Ref feedforwardVelocity); - /** * @brief Set the desired gravity direction expressed the target frame and the feedforward * velocity. diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index f2e8598600..e6e5370404 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -196,6 +196,7 @@ bool GravityTask::setFeedForwardVelocityInTargetFrame( const Eigen::Ref feedforwardVelocity) { m_feedForwardBody = feedforwardVelocity; + return true; } diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 17273df3d0..fba6fb56a2 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -708,8 +708,7 @@ TEST_CASE("QP-IK [With builder]") auto gravityTask = std::dynamic_pointer_cast(ik->getTask("GRAVITY_TASK").lock()); REQUIRE_FALSE(gravityTask == nullptr); - REQUIRE( - gravityTask->setDesiredGravityDirectionInTargetFrame(desiredSetPoints.desiredBodyGravity)); + REQUIRE(gravityTask->setSetPoint(desiredSetPoints.desiredBodyGravity)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -854,8 +853,7 @@ TEST_CASE("QP-IK [Distance and Gravity tasks]") REQUIRE(ikTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); - REQUIRE(ikTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( - desiredSetPoints.desiredBodyGravity)); + REQUIRE(ikTasks.gravityTask->setSetPoint(desiredSetPoints.desiredBodyGravity)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30; @@ -988,8 +986,7 @@ TEST_CASE("QP-IK [Distance and Gravity tasks Unconstrained]") REQUIRE(ikTasks.distanceTask->setDesiredDistance(desiredSetPoints.targetDistance)); - REQUIRE(ikTasks.gravityTask->setDesiredGravityDirectionInTargetFrame( - desiredSetPoints.desiredBodyGravity)); + REQUIRE(ikTasks.gravityTask->setSetPoint(desiredSetPoints.desiredBodyGravity)); // propagate the inverse kinematics for constexpr std::size_t iterations = 30;