Fix human model being stuck in case of bad calibration

CHANGELOG.md

@@ -6,6 +6,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Fixed
+- Fixed human model calibration bug in which sometimes it gets stuck (https://github.com/robotology/human-dynamics-estimation/pull/388).
+
 ## [3.0.0] - 2023-11-09
 
 ### Changed

HumanDynamicsEstimationLibrary/algorithms/include/hde/algorithms/DynamicalInverseKinematics.hpp

@@ -201,7 +201,7 @@ class hde::algorithms::DynamicalInverseKinematics
 
     void clearProblem();
 
-    bool solve(const double dt);
+    bool solve(const double dt, bool &reset);
 
     double getTargetsMeanOrientationErrorNorm() const;
     double getTargetsMeanPositionErrorNorm() const;

HumanDynamicsEstimationLibrary/algorithms/src/DynamicalInverseKinematics.cpp

@@ -91,7 +91,7 @@ class DynamicalInverseKinematics::impl
 
     TargetsMap::iterator getTargetRefIfItExists(const std::string& targetFrameName);
 
-    bool solveProblem(const double dt);
+    bool solveProblem(const double dt, bool &reset);
 
     bool computeDesiredLinkVelocities();
 
@@ -905,7 +905,7 @@ bool DynamicalInverseKinematics::impl::addTarget(const InverseKinematicsTarget&
     return result.second;
 }
 
-bool DynamicalInverseKinematics::impl::solveProblem(const double dt)
+bool DynamicalInverseKinematics::impl::solveProblem(const double dt, bool &reset)
 {
     if (!m_isInverseKinematicsInitializd) {
         if (!initialize())
@@ -929,7 +929,8 @@ bool DynamicalInverseKinematics::impl::solveProblem(const double dt)
     m_stateIntegrator.integrate(m_state.dot_s,
                               m_state.dot_W_p_B,
                               m_state.omega_B,
-                              dt);
+                              dt,
+                              reset);
     m_stateIntegrator.getJointConfiguration(m_state.s);
     m_stateIntegrator.getBasePose(m_state.W_p_B, m_state.W_R_B);
 
@@ -1657,9 +1658,9 @@ bool DynamicalInverseKinematics::getBaseVelocitySolution(iDynTree::Vector3& line
     return true;
 }
 
-bool DynamicalInverseKinematics::solve(const double dt)
+bool DynamicalInverseKinematics::solve(const double dt, bool &reset)
 {
-    return pImpl->solveProblem(dt);
+    return pImpl->solveProblem(dt, reset);
 }
 
 void DynamicalInverseKinematics::clearProblem()

HumanDynamicsEstimationLibrary/utils/include/hde/utils/iDynTreeUtils.hpp

@@ -133,7 +133,8 @@ class hde::utils::idyntree::state::Integrator
     void integrate(const iDynTree::VectorDynSize& dot_s,
                    const iDynTree::Vector3& dot_W_p_B,
                    const iDynTree::Vector3& omega_B,
-                   const double dt);
+                   const double dt,
+                   bool &resetFlags);
 
 private:
     unsigned int nJoints;

HumanDynamicsEstimationLibrary/utils/src/iDynTreeUtils.cpp

@@ -391,8 +391,14 @@ void state::Integrator::integrate(const iDynTree::VectorDynSize& new_dot_s, cons
 void state::Integrator::integrate(const iDynTree::VectorDynSize& new_dot_s,
                                   const iDynTree::Vector3& new_dot_W_p_B,
                                   const iDynTree::Vector3& new_omega_B,
-                                  const double dt)
+                                  const double dt,
+                                  bool &resetFlag)
 {
+    if(resetFlag) {
+        resetFlag = false;
+        oldState.zero();
+    }
+
     // integrate joints configuration
     integrate(new_dot_s, dt);
 

devices/HumanStateProvider/HumanStateProvider.h

@@ -4,11 +4,11 @@
 #ifndef HDE_DEVICES_HUMANSTATEPROVIDER
 #define HDE_DEVICES_HUMANSTATEPROVIDER
 
+#include <hde/interfaces/IHumanState.h>
+#include <hde/interfaces/IWearableTargets.h>
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IMultipleWrapper.h>
 #include <yarp/os/PeriodicThread.h>
-#include <hde/interfaces/IHumanState.h>
-#include <hde/interfaces/IWearableTargets.h>
 
 #include <memory>