Add isSetPointSetAtLeastOnce flag to task classes for set-point valid…

…ation
ami-iit · Feb 26, 2025 · 4f6d4af · 4f6d4af
1 parent 0af2b0d
commit 4f6d4af
Show file tree

Hide file tree

Showing 14 changed files with 120 additions and 22 deletions.
diff --git a/src/TSID/include/BipedalLocomotion/TSID/AngularMomentumTask.h b/src/TSID/include/BipedalLocomotion/TSID/AngularMomentumTask.h
@@ -60,6 +60,7 @@ class AngularMomentumTask : public TSIDLinearTask
                                                                object */
 
     LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< Controller in R3 */
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 
 public:
     /**

diff --git a/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h b/src/TSID/include/BipedalLocomotion/TSID/CoMTask.h
@@ -61,6 +61,7 @@ class CoMTask : public TSIDLinearTask
     std::array<bool, m_linearVelocitySize> m_mask{true, true, true};
     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:
 
     /**

diff --git a/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h b/src/TSID/include/BipedalLocomotion/TSID/JointTrackingTask.h
@@ -50,6 +50,7 @@ class JointTrackingTask : public TSIDLinearTask
     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 planner.

diff --git a/src/TSID/include/BipedalLocomotion/TSID/R3Task.h b/src/TSID/include/BipedalLocomotion/TSID/R3Task.h
@@ -77,6 +77,8 @@ class R3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCon
     /** State of the proportional derivative 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 planner.

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SE3Task.h
@@ -84,6 +84,7 @@ class SE3Task : public TSIDLinearTask, public BipedalLocomotion::System::ITaskCo
     /** State of the proportional derivative 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 planner.

diff --git a/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h b/src/TSID/include/BipedalLocomotion/TSID/SO3Task.h
@@ -60,6 +60,7 @@ class SO3Task : public TSIDLinearTask
 
     Eigen::MatrixXd m_jacobian; /**< Jacobian of the frame expressed in mixed representation */
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
 public:
     /**
      * Initialize the SO3Task.

diff --git a/src/TSID/include/BipedalLocomotion/TSID/VariableRegularizationTask.h b/src/TSID/include/BipedalLocomotion/TSID/VariableRegularizationTask.h
@@ -32,6 +32,8 @@ class VariableRegularizationTask : public TSIDLinearTask
     bool m_isValid{false}; /**< True if the task is valid. */
     std::size_t m_variableSize{0}; /**< Size of the regularized variable. */
 
+    bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
+
 public:
     /**
      * Initialize the planner.
@@ -63,6 +65,12 @@ class VariableRegularizationTask : public TSIDLinearTask
      */
     bool setSetPoint(Eigen::Ref<const Eigen::VectorXd> setPoint);
 
+    /**
+     * Update the content of the task.
+     * @return True in case of success, false otherwise.
+     */
+    bool update() override;
+
     /**
      * Get the size of the task. (I.e the number of rows of the vector b)
      * @return the size of the task.

diff --git a/src/TSID/src/AngularMomentumTask.cpp b/src/TSID/src/AngularMomentumTask.cpp
@@ -162,6 +162,18 @@ bool AngularMomentumTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[AngularMomentumTask::update] The task is not initialized.");
+        return m_isValid;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[AngularMomentumTask::update] The set-point has not been set at least once.");
+        return m_isValid;
+    }
+
     // update the control law
     m_R3Controller.setState(iDyn::toEigen(m_kinDyn->getCentroidalTotalMomentum().getAngularVec3()));
     m_R3Controller.computeControlLaw();
@@ -185,6 +197,8 @@ bool AngularMomentumTask::setSetPoint(Eigen::Ref<const Eigen::Vector3d> angularM
 {
     bool ok = m_R3Controller.setFeedForward(angularMomentumDerivative);
     ok = ok && m_R3Controller.setDesiredState(angularMomentum);
+
+    m_isSetPointSetAtLeastOnce = ok;
     return ok;
 }
 

diff --git a/src/TSID/src/CoMTask.cpp b/src/TSID/src/CoMTask.cpp
@@ -172,6 +172,18 @@ bool CoMTask::update()
 
     m_isValid = false;
 
+    if (!m_isInitialized)
+    {
+        log()->error("[CoMTask::update] The task is not initialized.");
+        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
     m_R3Controller.setState(toEigen(m_kinDyn->getCenterOfMassPosition()),
                             toEigen(m_kinDyn->getCenterOfMassVelocity()));
@@ -226,6 +238,7 @@ bool CoMTask::setSetPoint(Eigen::Ref<const Eigen::Vector3d> position,
     bool ok = true;
     ok = ok && m_R3Controller.setDesiredState(position, velocity);
     ok = ok && m_R3Controller.setFeedForward(acceleration);
+    m_isSetPointSetAtLeastOnce = ok;
     return ok;
 }
 

diff --git a/src/TSID/src/JointTrackingTask.cpp b/src/TSID/src/JointTrackingTask.cpp
@@ -69,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]";
 
@@ -109,7 +110,8 @@ bool JointTrackingTask::initialize(std::weak_ptr<const ParametersHandler::IParam
 
     if (m_kp.size() != m_kinDyn->getNrOfDegreesOfFreedom())
     {
-        log()->error("{} The size of the kp gain does not match with the one stored in kinDynComputations object. "
+        log()->error("{} The size of the kp gain does not match with the one stored in "
+                     "kinDynComputations object. "
                      "Expected: {}. Given: {}",
                      errorPrefix,
                      m_kinDyn->getNrOfDegreesOfFreedom(),
@@ -119,7 +121,8 @@ bool JointTrackingTask::initialize(std::weak_ptr<const ParametersHandler::IParam
 
     if (m_kd.size() != m_kinDyn->getNrOfDegreesOfFreedom())
     {
-        log()->error("{} The size of the kd gain does not match with the one stored in kinDynComputations object. "
+        log()->error("{} The size of the kd gain does not match with the one stored in "
+                     "kinDynComputations object. "
                      "Expected: {}. Given: {}",
                      errorPrefix,
                      m_kinDyn->getNrOfDegreesOfFreedom(),
@@ -149,13 +152,19 @@ bool JointTrackingTask::update()
 
     if (!m_kinDyn->getJointPos(m_jointPosition))
     {
-        std::cerr << errorPrefix << " Unable to get the joint position" << std::endl;
+        log()->error("{} Unable to get the joint position", errorPrefix);
         return false;
     }
 
     if (!m_kinDyn->getJointVel(m_jointVelocity))
     {
-        std::cerr << errorPrefix << " Unable to get the joint velocity" << std::endl;
+        log()->error("{} Unable to get the joint velocity", errorPrefix);
+        return false;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("{} The set-point has not been set at least once.", errorPrefix);
         return false;
     }
 
@@ -174,33 +183,38 @@ bool JointTrackingTask::setSetPoint(Eigen::Ref<const Eigen::VectorXd> jointPosit
 }
 
 bool JointTrackingTask::setSetPoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
-                                     Eigen::Ref<const Eigen::VectorXd> jointVelocity)
+                                    Eigen::Ref<const Eigen::VectorXd> jointVelocity)
 {
     return this->setSetPoint(jointPosition, jointVelocity, m_zero);
 }
 
 bool JointTrackingTask::setSetPoint(Eigen::Ref<const Eigen::VectorXd> jointPosition,
-                                     Eigen::Ref<const Eigen::VectorXd> jointVelocity,
-                                     Eigen::Ref<const Eigen::VectorXd> jointAcceleration)
+                                    Eigen::Ref<const Eigen::VectorXd> jointVelocity,
+                                    Eigen::Ref<const Eigen::VectorXd> jointAcceleration)
 {
     constexpr std::string_view errorPrefix = "[JointTrackingTask::setSetpoint] ";
 
     if (jointPosition.size() != m_kinDyn->getNrOfDegreesOfFreedom()
         || jointVelocity.size() != m_kinDyn->getNrOfDegreesOfFreedom()
         || jointAcceleration.size() != m_kinDyn->getNrOfDegreesOfFreedom())
     {
-        std::cerr << errorPrefix << "Wrong size of the desired reference trajectory:" << std::endl
-                  << "Expected size: " << m_kinDyn->getNrOfDegreesOfFreedom() << std::endl
-                  << "Joint position size: " << jointPosition.size() << std::endl
-                  << "Joint velocity size: " << jointVelocity.size() << std::endl
-                  << "Joint acceleration size: " << jointAcceleration.size() << std::endl;
+        log()->error("{} Wrong size of the desired reference trajectory. Expected size: {}. "
+                     "Joint position size: {}. Joint velocity size: {}. Joint acceleration size: "
+                     "{}.",
+                     errorPrefix,
+                     m_kinDyn->getNrOfDegreesOfFreedom(),
+                     jointPosition.size(),
+                     jointVelocity.size(),
+                     jointAcceleration.size());
         return false;
     }
 
     m_desiredJointPosition = jointPosition;
     m_desiredJointVelocity = jointVelocity;
     m_desiredJointAcceleration = jointAcceleration;
 
+    m_isSetPointSetAtLeastOnce = true;
+
     return true;
 }
 

diff --git a/src/TSID/src/R3Task.cpp b/src/TSID/src/R3Task.cpp
@@ -203,6 +203,12 @@ bool R3Task::update()
         return m_isValid;
     }
 
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[R3Task::update] The set-point has not been set at least once.");
+        return m_isValid;
+    }
+
     auto getGenericControllerOutput = [&](const auto& controller) {
         if (m_controllerMode == Mode::Enable)
             return controller.getControl().coeffs();
@@ -253,13 +259,15 @@ bool R3Task::update()
     return m_isValid;
 }
 
-bool R3Task::setSetPoint(Eigen::Ref<const Eigen::Vector3d> I_p_F,
-                         Eigen::Ref<const Eigen::Vector3d> velocity, /** = Eigen::Vector3d::Zero() */
-                         Eigen::Ref<const Eigen::Vector3d> acceleration /** = Eigen::Vector3d::Zero()*/)
+bool R3Task::setSetPoint(
+    Eigen::Ref<const Eigen::Vector3d> I_p_F,
+    Eigen::Ref<const Eigen::Vector3d> velocity, /** = Eigen::Vector3d::Zero() */
+    Eigen::Ref<const Eigen::Vector3d> acceleration /** = Eigen::Vector3d::Zero()*/)
 {
     bool ok = true;
     ok = ok && m_R3Controller.setDesiredState(I_p_F, velocity);
     ok = ok && m_R3Controller.setFeedForward(acceleration);
+    m_isSetPointSetAtLeastOnce = ok;
 
     return ok;
 }

diff --git a/src/TSID/src/SE3Task.cpp b/src/TSID/src/SE3Task.cpp
@@ -238,6 +238,12 @@ bool SE3Task::update()
         return m_isValid;
     }
 
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[SE3Task::update] The set-point has not been set at least once.");
+        return m_isValid;
+    }
+
     auto getGenericControllerOutput = [&](const auto& controller) {
         if (m_controllerMode == Mode::Enable)
             return controller.getControl().coeffs();
@@ -317,6 +323,8 @@ bool SE3Task::setSetPoint(const manif::SE3d& I_H_F,
     ok = ok && m_SO3Controller.setDesiredState(I_H_F.quat(), mixedVelocity.ang());
     ok = ok && m_SO3Controller.setFeedForward(mixedAcceleration.ang());
 
+    m_isSetPointSetAtLeastOnce = ok;
+
     return ok;
 }
 

diff --git a/src/TSID/src/SO3Task.cpp b/src/TSID/src/SO3Task.cpp
@@ -132,8 +132,7 @@ bool SO3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
     if (ptr->getParameter("kp_angular", scalarBuffer))
     {
         kpAngular.setConstant(scalarBuffer);
-    }
-    else if(!ptr->getParameter("kp_angular", kpAngular))
+    } else if (!ptr->getParameter("kp_angular", kpAngular))
     {
         log()->error("{}, [{} {}] Unable to get the proportional angular gain.",
                      errorPrefix,
@@ -145,8 +144,7 @@ bool SO3Task::initialize(std::weak_ptr<const ParametersHandler::IParametersHandl
     if (ptr->getParameter("kd_angular", scalarBuffer))
     {
         kdAngular.setConstant(scalarBuffer);
-    }
-    else if(!ptr->getParameter("kd_angular", kdAngular))
+    } else if (!ptr->getParameter("kd_angular", kdAngular))
     {
         log()->error("{}, [{} {}] Unable to get the derivative angular gain.",
                      errorPrefix,
@@ -175,14 +173,19 @@ bool SO3Task::update()
         return m_isValid;
     }
 
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[SO3Task::update] The set-point has not been set at least once.");
+        return m_isValid;
+    }
+
     m_SO3Controller.setState(BipedalLocomotion::Conversions::toManifRot(
                                  m_kinDyn->getWorldTransform(m_frameIndex).getRotation()),
                              iDynTree::toEigen(
                                  m_kinDyn->getFrameVel(m_frameIndex).getAngularVec3()));
 
     m_SO3Controller.computeControlLaw();
 
-
     m_b = -iDynTree::toEigen(m_kinDyn->getFrameBiasAcc(m_frameIndex)).tail<3>();
     m_b += m_SO3Controller.getControl().coeffs();
 
@@ -207,6 +210,8 @@ bool SO3Task::setSetPoint(const manif::SO3d& I_R_F,
     ok = ok && m_SO3Controller.setDesiredState(I_R_F, angularVelocity);
     ok = ok && m_SO3Controller.setFeedForward(angularAcceleration);
 
+    m_isSetPointSetAtLeastOnce = ok;
+
     return ok;
 }
 

diff --git a/src/TSID/src/VariableRegularizationTask.cpp b/src/TSID/src/VariableRegularizationTask.cpp
@@ -88,6 +88,24 @@ bool VariableRegularizationTask::setVariablesHandler(const VariablesHandler& var
     return true;
 }
 
+bool VariableRegularizationTask::update()
+{
+    if (!m_isInitialized)
+    {
+        log()->error("[VariableRegularizationTask::update] Please call initialize() before update().");
+        return false;
+    }
+
+    if (!m_isSetPointSetAtLeastOnce)
+    {
+        log()->error("[VariableRegularizationTask::update] The set-point has not been set at least once.");
+        return false;
+    }
+
+    // no-op this is just for check
+    return true;
+}
+
 bool VariableRegularizationTask::initialize(std::weak_ptr<const IParametersHandler> paramHandler)
 {
     constexpr auto errorPrefix = "[VariableRegularizationTask::initialize]";
@@ -159,7 +177,10 @@ bool VariableRegularizationTask::setSetPoint(Eigen::Ref<const Eigen::VectorXd> s
     }
     m_isValid = true;
     m_b = setPoint;
-    return true;
+
+    m_isSetPointSetAtLeastOnce = m_isValid;
+
+    return m_isValid;
 }
 
 std::size_t VariableRegularizationTask::size() const
-Original file line number
+Diff line change
@@ Expand Up / @@ -60,6 +60,7 @@ class AngularMomentumTask : public TSIDLinearTask @@
                                                                    object */
         LieGroupControllers::ProportionalControllerR3d m_R3Controller; /**< Controller in R3 */
+        bool m_isSetPointSetAtLeastOnce{false}; /**< True if the set point has been set at least once. */
     public:
         /**
@@ Expand Down @@