Added setSetPoint method to GravityTask

bindings/python/IK/src/GravityTask.cpp

@@ -34,7 +34,11 @@ void CreateGravityTask(pybind11::module& module)
  py::arg("desired_gravity_direction"))
  .def("set_feedforward_velocity_in_target_frame",
  &GravityTask::setFeedForwardVelocityInTargetFrame,
- py::arg("feedforward_velocity"));
+ py::arg("feedforward_velocity"))
+ .def("set_set_point",
+ &GravityTask::setSetPoint,
+ py::arg("desired_gravity_direction"),
+ py::arg("feedforward_velocity") = Eigen::Vector3d::Zero());
 }
 
 } // namespace IK

bindings/python/IK/tests/test_QP_inverse_kinematics.py

@@ -258,6 +258,7 @@ def test_gravity_task():
  # Set desired distance
  assert gravity_task.set_desired_gravity_direction_in_target_frame(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)])
  assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)])
+ assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)])
 
 def test_integration_based_ik_state():
 

src/IK/include/BipedalLocomotion/IK/GravityTask.h

@@ -172,6 +172,7 @@ class GravityTask : public IKLinearTask
  /**
  * @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.
  */
@@ -181,12 +182,29 @@ class GravityTask : public IKLinearTask
  /**
  * @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.
+ * @param desiredGravityDirection The desired gravity direction in target frame.
+ * @param feedforwardVelocity The desired feedforward velocity in rad/s expressed in the target
+ * frame.
+ * @return True in case of success, false otherwise.
+ *
+ * The desiredGravityDirection is normalized, unless the norm is too small.
+ * Only the first two components of the feedforwardVelocity are used.
+ * This is equivalent of using setDesiredGravityDirectionInTargetFrame and
+ * setFeedForwardVelocityInTargetFrame
+ */
+ bool setSetPoint(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection,
+ const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity
+ = Eigen::Vector3d::Zero());
+
  /**
  * Get the size of the task. (I.e the number of rows of the vector b)
  * @return the size of the task.

src/IK/src/GravityTask.cpp

@@ -185,6 +185,13 @@ bool GravityTask::setFeedForwardVelocityInTargetFrame(
  return true;
 }
 
+bool GravityTask::setSetPoint(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection,
+ const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity)
+{
+ return setDesiredGravityDirectionInTargetFrame(desiredGravityDirection)
+ && setFeedForwardVelocityInTargetFrame(feedforwardVelocity);
+}
+
 std::size_t GravityTask::size() const
 {
  return m_DoFs;

src/IK/tests/GravityTaskTest.cpp

@@ -86,9 +86,9 @@ TEST_CASE("Distance task")
 
  Eigen::Vector3d desiredDirection({1.0, 2.0, 3.0});
  desiredDirection.normalize();
- REQUIRE(task.setDesiredGravityDirectionInTargetFrame(desiredDirection));
  Eigen::Vector3d feedforward({0.1, 0.2, 0.3});
- REQUIRE(task.setFeedForwardVelocityInTargetFrame(feedforward));
+
+ REQUIRE(task.setSetPoint(desiredDirection, feedforward));
 
  REQUIRE(task.update());
  REQUIRE(task.isValid());