Add isSetPointSetAtLeastOnce flag to task classes and update validati…

…on logic in the IK

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

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;
@@ -189,8 +202,10 @@ bool GravityTask::setFeedForwardVelocityInTargetFrame(
 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;
 }
 

src/IK/src/R3Task.cpp

@@ -38,8 +38,7 @@ bool R3Task::setVariablesHandler(const System::VariablesHandler& variablesHandle
     }
 
     // get the variable
-    if (!variablesHandler.getVariable(m_robotVelocityVariable.name,
-                                      m_robotVelocityVariable))
+    if (!variablesHandler.getVariable(m_robotVelocityVariable.name, m_robotVelocityVariable))
     {
         log()->error("[R3Task::setVariablesHandler] Unable to get the variable named {}.",
                      m_robotVelocityVariable.name);
@@ -74,12 +73,11 @@ bool R3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandle
 
     std::string maskDescription = "";
     auto boolToString = [](bool b) { return b ? " true" : " false"; };
-    for(const auto flag : m_mask)
+    for (const auto flag : m_mask)
     {
         maskDescription += boolToString(flag);
     }
 
-
     if (m_kinDyn == nullptr || !m_kinDyn->isValid())
     {
         log()->error("{} [{} {}] KinDynComputations object is not valid.",
@@ -137,8 +135,7 @@ bool R3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandle
     if (ptr->getParameter("kp_linear", scalarBuffer))
     {
         kpLinear.setConstant(scalarBuffer);
-    }
-    else if(!ptr->getParameter("kp_linear", kpLinear))
+    } else if (!ptr->getParameter("kp_linear", kpLinear))
     {
         log()->error("{} [{} {}] Unable to get the proportional linear gain.",
                      errorPrefix,
@@ -156,8 +153,7 @@ bool R3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandle
                     descriptionPrefix,
                     frameName,
                     maskDescription);
-    }
-    else
+    } else
     {
         // covert an std::vector in a std::array
         std::copy(mask.begin(), mask.end(), m_mask.begin());
@@ -166,7 +162,7 @@ bool R3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandle
 
         // Update the mask description
         maskDescription.clear();
-        for(const auto flag : m_mask)
+        for (const auto flag : m_mask)
         {
             maskDescription += boolToString(flag);
         }
@@ -186,6 +182,19 @@ bool R3Task::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[R3Task::update] The task is not initialized. Please call initialize "
+                     "method.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[R3Task::update] The set-point has not been set at least once.");
+        return m_isValid;
+    }
+
     auto getControllerState = [&](const auto& controller) {
         if (m_controllerMode == Mode::Enable)
             return controller.getControl().coeffs();
@@ -241,6 +250,8 @@ bool R3Task::setSetPoint(Eigen::Ref<const Eigen::Vector3d> I_p_F,
     ok = ok && m_R3Controller.setDesiredState(I_p_F);
     ok = ok && m_R3Controller.setFeedForward(velocity);
 
+    m_isSetPointSetAtLeastOnce = ok;
+
     return ok;
 }