Check that the size the gains and weights vectors are correct #797

CHANGELOG.md

@@ -7,6 +7,8 @@ All notable changes to this project are documented in this file.
 ### Changed
 - 🤖 [`ergoCubSN001`] Update `YarpRobotLoggerDevice` configuration file to exclude the head-imu and include the arms FTs  (https://github.com/ami-iit/bipedal-locomotion-framework/pull/798)
 - Avoid to call `BufferedPort::prepare` every time `VectorsCollectionServer::populateData` is called (https://github.com/ami-iit/bipedal-locomotion-framework/pull/790)
+- Check that the size the gains vectors in the `IK::JointTrackingTask`, `IK::JointLimitsTask` and `TSID::JointLimitsTask` are correct (https://github.com/ami-iit/bipedal-locomotion-framework/pull/797)
+- Check that the size of the weights in MultiStateWeightProvider are all the same (https://github.com/ami-iit/bipedal-locomotion-framework/pull/797)
 
 ### Fixed
 

src/ContinuousDynamicalSystem/src/MultiStateWeightProvider.cpp

@@ -72,6 +72,28 @@ bool MultiStateWeightProvider::initialize(
         }
     }
 
+    // check that all the states have the same size
+    const std::size_t size = m_states.begin()->second.size();
+    for (const auto& state : m_states)
+    {
+        if (state.second.size() != size)
+        {
+            log()->error("{} The size of the weight associated to the state named '{}' does not "
+                         "match with the size of the weight associated to the state named '{}'. "
+                         "Size of the of the weight named '{}': {}.Size of the weight named '{}': "
+                         "{}.",
+                         logPrefix,
+                         state.first,
+                         m_states.begin()->first,
+                         state.first,
+                         state.second.size(),
+                         m_states.begin()->first,
+                         size);
+            return false;
+        }
+    }
+
+    // initialize the smoother
     std::string firstState;
     ptr->getGroup(states[0]).lock()->getParameter("name", firstState);
     return this->reset(firstState);

src/IK/src/JointLimitsTask.cpp

@@ -155,6 +155,16 @@ bool JointLimitsTask::initialize(
         return false;
     }
 
+    if (m_klim.size() != m_kinDyn->getNrOfDegreesOfFreedom())
+    {
+        log()->error("{} The size of the 'klim' parameter does not match with the number of "
+                     "degrees of freedom of the robot. Expected: {}. Given: {}",
+                     errorPrefix,
+                     m_kinDyn->getNrOfDegreesOfFreedom(),
+                     m_klim.size());
+        return false;
+    }
+
     if (!ptr->getParameter("sampling_time", m_samplingTime)
         || m_samplingTime <= std::chrono::nanoseconds::zero())
     {

src/IK/src/JointTrackingTask.cpp

@@ -102,6 +102,16 @@ bool JointTrackingTask::initialize(std::weak_ptr<const ParametersHandler::IParam
         return false;
     }
 
+    if (m_kp.size() != m_kinDyn->getNrOfDegreesOfFreedom())
+    {
+        log()->error("{} The size of the proportional gain does not match with the one stored in "
+                     "kinDynComputations object. Expected: {}. Given: {}",
+                     errorPrefix,
+                     m_kinDyn->getNrOfDegreesOfFreedom(),
+                     m_kp.size());
+        return false;
+    }
+
     // set the description
     m_description = "Joint tracking task";
 

src/TSID/src/JointTrackingTask.cpp

@@ -107,6 +107,26 @@ bool JointTrackingTask::initialize(std::weak_ptr<const ParametersHandler::IParam
         m_kd = 2 * m_kp.cwiseSqrt();
     }
 
+    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. "
+                     "Expected: {}. Given: {}",
+                     errorPrefix,
+                     m_kinDyn->getNrOfDegreesOfFreedom(),
+                     m_kp.size());
+        return false;
+    }
+
+    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. "
+                     "Expected: {}. Given: {}",
+                     errorPrefix,
+                     m_kinDyn->getNrOfDegreesOfFreedom(),
+                     m_kd.size());
+        return false;
+    }
+
     // set the description
     m_description = "Joint tracking task";