Add isSetPointSetAtLeastOnce flag to IK and TSID task classes for set-point validation (#939)

Author: GiulioRomualdi

CHANGELOG.md

@@ -9,10 +9,12 @@ All notable changes to this project are documented in this file.
 - Add `getCurrentTime` to the `FixedFootDetector` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/928)
 - Add the possibility to easily disable/enable the rt logging in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/932)
 - Enable Programmatic Creation of `VariableRegularizationTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/934)
+- Check if the setpoint has been set at least once in TSID and IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/939)
 
 ### Changed
 - Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
 - Removed ROS1 device publisher and the corresponding example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
+- Remove the possibility of setting the desired gravity direction and desired forward velocity separately in the `IK::GravityTrask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/939)
 
 ### Fixed
 - Fix outputs of `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916)

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"),

bindings/python/IK/tests/test_QP_inverse_kinematics.py

@@ -256,9 +256,7 @@ def test_gravity_task():
     assert gravity_task.set_variables_handler(variables_handler=gravity_task_var_handler)
 
     # 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)])
+    assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)], feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)])
 
 def test_joint_velocity_limits_task():
 

src/IK/include/BipedalLocomotion/IK/AngularMomentumTask.h

@@ -53,7 +53,8 @@ class AngularMomentumTask : public IKLinearTask
 
     Eigen::MatrixXd m_centroidalMomentumMatrix;
     std::size_t m_angularMomentumTaskSize{m_angularVelocitySize}; /**< DoFs associated to the linear task */
-    std::array<bool, m_angularVelocitySize> m_mask{true, true, true};
+    std::array<bool, m_angularVelocitySize> m_mask{true, true, true}; /**< Mask representing the angular momentum coordinates controlled. */
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 public:
 
     /**

src/IK/include/BipedalLocomotion/IK/CoMTask.h

@@ -72,6 +72,8 @@ class CoMTask : public IKLinearTask
     std::size_t m_DoFs{m_linearVelocitySize}; /**< DoFs associated to the task */
 
     Eigen::MatrixXd m_jacobian; /**< CoM Jacobian matrix in MIXED representation */
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set-point has been set at least once */
+
 public:
     /**
      * Initialize the task.

src/IK/include/BipedalLocomotion/IK/DistanceTask.h

@@ -64,6 +64,7 @@ class DistanceTask : public IKLinearTask
     double m_computedDistance{0.0}; /**< Computed distance. */
     double m_distanceNumericThreshold{0.001}; /**< Numeric threshold when inverting the computed
                                                  distance in the Jacobian computation. */
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 
 public:
     // clang-format off

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

@@ -125,6 +125,7 @@ class GravityTask : public IKLinearTask
     Eigen::MatrixXd m_bm; /**< Matrix filtering acceleration. */
 
     iDynTree::FrameIndex m_targetFrameIndex; /**< Index of the target frame. */
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set-point has been set at least once */
 
 public:
     // clang-format off
@@ -169,25 +170,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.
@@ -205,6 +187,27 @@ class GravityTask : public IKLinearTask
                      const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity
                      = Eigen::Vector3d::Zero());
 
+   /**
+     * @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.
+     */
+    [[deprecated("Use setSetPoint instead")]]
+    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.
+     */
+    [[deprecated("Use setSetPoint instead")]]
+    bool setFeedForwardVelocityInTargetFrame( //
+        const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity);
+
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
      * @return the size of the task.

src/IK/include/BipedalLocomotion/IK/JointTrackingTask.h

@@ -48,6 +48,7 @@ class JointTrackingTask : public IKLinearTask
     std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
                                                                object */
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 public:
     /**
      * Initialize the task.

src/IK/include/BipedalLocomotion/IK/R3Task.h

@@ -74,6 +74,7 @@ class R3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskContr
     /** State of the proportional controller implemented in the task */
     Mode m_controllerMode{Mode::Enable};
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once */
 public:
     /**
      * Initialize the task.

src/IK/include/BipedalLocomotion/IK/SE3Task.h

@@ -88,6 +88,7 @@ class SE3Task : public IKLinearTask, public BipedalLocomotion::System::ITaskCont
     /** State of the proportional controller implemented in the task */
     Mode m_controllerMode{Mode::Enable};
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set-point has been set at least once */
 public:
     /**
      * Initialize the task.

src/IK/include/BipedalLocomotion/IK/SO3Task.h

@@ -59,8 +59,9 @@ class SO3Task : public IKLinearTask
     std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
                                                                object */
 
-    Eigen::MatrixXd m_jacobian;
+    Eigen::MatrixXd m_jacobian; /**< Internal buffer to store the jacobian. */
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 public:
 
     /**

src/IK/src/AngularMomentumTask.cpp

@@ -140,6 +140,19 @@ bool AngularMomentumTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[AngularMomentumTask::update] The task is not initialized. Please call "
+                     "initialize method.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[AngularMomentumTask::update] The set point has not been set at least once.");
+        return m_isValid;
+    }
+
     if (!m_kinDyn->getCentroidalTotalMomentumJacobian(m_centroidalMomentumMatrix))
     {
         log()->error("[AngularMomentumTask::update] Unable to get the centroidal momentum matrix.");
@@ -190,6 +203,8 @@ bool AngularMomentumTask::setSetPoint(Eigen::Ref<const Eigen::Vector3d> desiredA
             }
         }
     }
+
+    m_isSetPointSetAtLeastOnce = true;
     return true;
 }
 

src/IK/src/CoMTask.cpp

@@ -175,6 +175,19 @@ bool CoMTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[CoMTask::update] The task is not initialized. Please call initialize "
+                     "method.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[CoMTask::update] The set point has not been set at least once.");
+        return m_isValid;
+    }
+
     // set the state
     if (!m_useExogenousFeedback)
     {
@@ -228,6 +241,8 @@ bool CoMTask::setSetPoint(Eigen::Ref<const Eigen::Vector3d> position,
     ok = ok && m_R3Controller.setDesiredState(position);
     ok = ok && m_R3Controller.setFeedForward(velocity);
 
+    m_isSetPointSetAtLeastOnce = ok;
+
     return ok;
 }
 

src/IK/src/DistanceTask.cpp

@@ -182,6 +182,19 @@ bool DistanceTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[DistanceTask::update] The task is not initialized. Please call initialize "
+                     "method.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[DistanceTask::update] The set point has not been set at least once.");
+        return m_isValid;
+    }
+
     if (m_referenceName == "")
     {
         m_framePosition = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getPosition());
@@ -222,6 +235,7 @@ bool DistanceTask::update()
 bool DistanceTask::setDesiredDistance(double desiredDistance)
 {
     m_desiredDistance = std::abs(desiredDistance);
+    m_isSetPointSetAtLeastOnce = true;
     return true;
 }
 

src/IK/src/GravityTask.cpp

@@ -152,6 +152,19 @@ bool GravityTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[GravityTask::update] The task is not initialized. Please call initialize "
+                     "method.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[GravityTask::update] The set point has not been set at least once.");
+        return m_isValid;
+    }
+
     auto targetRotation = toEigen(m_kinDyn->getWorldTransform(m_targetFrameIndex).getRotation());
 
     Eigen::Vector3d desiredZDirectionAbsolute = targetRotation * m_desiredZDirectionBody;
@@ -176,21 +189,25 @@ bool GravityTask::setDesiredGravityDirectionInTargetFrame(
 {
     m_desiredZDirectionBody = desiredGravityDirection;
     m_desiredZDirectionBody.normalize();
+    m_isSetPointSetAtLeastOnce = true;
     return true;
 }
 
 bool GravityTask::setFeedForwardVelocityInTargetFrame(
     const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity)
 {
     m_feedForwardBody = feedforwardVelocity;
+    m_isSetPointSetAtLeastOnce = true;
     return true;
 }
 
 bool GravityTask::setSetPoint(const Eigen::Ref<const Eigen::Vector3d> desiredGravityDirection,
                               const Eigen::Ref<const Eigen::Vector3d> feedforwardVelocity)
 {
-    return setDesiredGravityDirectionInTargetFrame(desiredGravityDirection)
-           && setFeedForwardVelocityInTargetFrame(feedforwardVelocity);
+    m_isSetPointSetAtLeastOnce = setDesiredGravityDirectionInTargetFrame(desiredGravityDirection)
+                                 && setFeedForwardVelocityInTargetFrame(feedforwardVelocity);
+
+    return m_isSetPointSetAtLeastOnce;
 }
 
 std::size_t GravityTask::size() const

src/IK/src/JointTrackingTask.cpp

@@ -38,7 +38,6 @@ bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& vari
         return false;
     }
 
-
     if (!variablesHandler.getVariable(m_robotVelocityVariableName, robotVelocityVariable))
     {
         log()->error("{} Error while retrieving the robot velocity variable.", errorPrefix);
@@ -55,7 +54,6 @@ bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& vari
         return false;
     }
 
-
     // resize the matrices
     m_A.resize(m_kinDyn->getNrOfDegreesOfFreedom(), variablesHandler.getNumberOfVariables());
     m_A.setZero();
@@ -71,7 +69,8 @@ bool JointTrackingTask::setVariablesHandler(const System::VariablesHandler& vari
     return true;
 }
 
-bool JointTrackingTask::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
+bool JointTrackingTask::initialize(
+    std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler)
 {
     constexpr auto errorPrefix = "[JointTrackingTask::initialize] ";
 
@@ -131,6 +130,18 @@ bool JointTrackingTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("{} The task is not initialized. Please call initialize method.", errorPrefix);
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("{} The set point has not been set at least once.", errorPrefix);
+        return m_isValid;
+    }
+
     if (!m_kinDyn->getJointPos(m_jointPosition))
     {
         log()->error("{} Unable to get the joint position.", errorPrefix);
@@ -168,6 +179,8 @@ bool JointTrackingTask::setSetPoint(Eigen::Ref<const Eigen::VectorXd> jointPosit
     m_desiredJointPosition = jointPosition;
     m_desiredJointVelocity = jointVelocity;
 
+    m_isSetPointSetAtLeastOnce = true;
+
     return true;
 }