Refactor GravityTask: remove redundant methods and update tests to us…

…e setSetPoint

bindings/python/IK/src/GravityTask.cpp

@@ -29,12 +29,6 @@ void CreateGravityTask(pybind11::module& module)
 
     py::class_<GravityTask, std::shared_ptr<GravityTask>, 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"),

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<const Eigen::Vector3d> 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<const Eigen::Vector3d> 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<const Eigen::Vector3d> 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<const Eigen::Vector3d> feedforwardVelocity);
-
     /**
      * @brief Set the desired gravity direction expressed the target frame and the feedforward
      * velocity.

src/IK/src/GravityTask.cpp

@@ -196,6 +196,7 @@ bool GravityTask::setFeedForwardVelocityInTargetFrame(
     const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity)
 {
     m_feedForwardBody = feedforwardVelocity;
+
     return true;
 }
 

src/IK/tests/QPInverseKinematicsTest.cpp

@@ -708,8 +708,7 @@ TEST_CASE("QP-IK [With builder]")
 
     auto gravityTask = std::dynamic_pointer_cast<GravityTask>(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;