Add integretor reset on calibration

dariosortino · Apr 30, 2024 · 44ae3a3 · 44ae3a3
1 parent 48d0147
commit 44ae3a3
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 3 deletions.
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, bool* reset = nullptr);
 
     double getTargetsMeanOrientationErrorNorm() const;
     double getTargetsMeanPositionErrorNorm() const;

diff --git a/HumanDynamicsEstimationLibrary/algorithms/src/DynamicalInverseKinematics.cpp b/HumanDynamicsEstimationLibrary/algorithms/src/DynamicalInverseKinematics.cpp
@@ -1657,7 +1657,7 @@ 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);
 }

diff --git a/devices/HumanStateProvider/HumanStateProvider.cpp b/devices/HumanStateProvider/HumanStateProvider.cpp
@@ -1557,14 +1557,20 @@ void HumanStateProvider::impl::computeSecondaryCalibrationRotationsForChain(cons
 {
     // initialize vectors
     iDynTree::VectorDynSize jointPos(jointConfigurationSolution);
+    jointPos.zero(); 
     iDynTree::VectorDynSize jointVel(jointVelocitiesSolution);
     jointVel.zero();
     iDynTree::Twist baseVel;
     baseVel.zero();
 
     for (auto const& jointZeroIdx: jointZeroIndices) {
         jointPos.setVal(jointZeroIdx, jointCalibrationSolution.getVal(jointZeroIdx));
+        std::cout << "jointPos: " << jointPos.getVal(jointZeroIdx) << std::endl;
     }
+
+    std::lock_guard<std::mutex> lock(mutexFlagIntegrator);
+    resetIntegrator = true;
+
     // TODO check which value to give to the base (before we were using the base target measurement)
     kinDynComputations->setRobotState(iDynTree::Transform::Identity(), jointPos, baseVel, jointVel, worldGravity);
 
@@ -2334,7 +2340,8 @@ bool HumanStateProvider::impl::solveDynamicalInverseKinematics()
         }
     }
 
-    if (!dynamicalInverseKinematics.solve(dt))
+    std::lock_guard<std::mutex> lock(mutexFlagIntegrator);
+    if (!dynamicalInverseKinematics.solve(dt, &resetIntegrator))
         return false;
 
     dynamicalInverseKinematics.getConfigurationSolution(baseTransformSolution, jointConfigurationSolution);

diff --git a/devices/HumanStateProvider/HumanStateProvider.h b/devices/HumanStateProvider/HumanStateProvider.h
@@ -28,6 +28,8 @@ class hde::devices::HumanStateProvider final
 private:
     class impl;
     std::unique_ptr<impl> pImpl;
+    std::mutex mutexFlagIntegrator;
+    bool resetIntegrator = false;
 
 public:
     HumanStateProvider();