Fix human model being stuck in case of bad calibration (#388)

* Add integretor reset on calibration * formatting HumanStateProvider.cpp and HumanStateProvider.h * add flag and mutex to reset integrator in HumanStateProvider::impl * Set jointPos and state at integrator to zero during calibration * Add flag check to reset previous state in the integrator * Update changelog * Fixed changelog * fix #388 (comment) * Changed integrator reset argument to be passed by reference instead of address * Removed debug print * Changed reset integrator flag logic to be passed by value --------- Co-authored-by: Davide <[email protected]>
robotology · May 13, 2024 · 943483e · 943483e
1 parent 48d0147
commit 943483e
Show file tree

Hide file tree

Showing 7 changed files with 878 additions and 694 deletions.
diff --git a/CHANGELOG.md b/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

diff --git a/...ynamicsEstimationLibrary/algorithms/include/hde/algorithms/DynamicalInverseKinematics.hpp b/...ynamicsEstimationLibrary/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, const bool reset = false);
 
     double getTargetsMeanOrientationErrorNorm() const;
     double getTargetsMeanPositionErrorNorm() const;

diff --git a/HumanDynamicsEstimationLibrary/algorithms/src/DynamicalInverseKinematics.cpp b/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, const bool reset = false);
 
     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, const 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, const bool reset)
 {
-    return pImpl->solveProblem(dt);
+    return pImpl->solveProblem(dt, reset);
 }
 
 void DynamicalInverseKinematics::clearProblem()

diff --git a/HumanDynamicsEstimationLibrary/utils/include/hde/utils/iDynTreeUtils.hpp b/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,
+                   const bool resetFlags = false);
 
 private:
     unsigned int nJoints;

diff --git a/HumanDynamicsEstimationLibrary/utils/src/iDynTreeUtils.cpp b/HumanDynamicsEstimationLibrary/utils/src/iDynTreeUtils.cpp
@@ -391,8 +391,13 @@ 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,
+                                  const bool resetFlag)
 {
+    if(resetFlag) {
+        oldState.zero();
+    }
+
     // integrate joints configuration
     integrate(new_dot_s, dt);
 

diff --git a/devices/HumanStateProvider/HumanStateProvider.cpp b/devices/HumanStateProvider/HumanStateProvider.cpp
diff --git a/devices/HumanStateProvider/HumanStateProvider.h b/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>