Set the calibrationMeasurementToLink matrix to the initial when reset…

…All is called (#421)

* set the calibrationMeasurementToLink matrix to the initial when resetAll command is run

* Update CHANGELOG.md

CHANGELOG.md

@@ -2,6 +2,11 @@
 
 This file documents notable changes to this project done before November 2023. For changes after that date, please refer to the release notes of each release at https://github.com/robotology/human-dynamics-estimation/releases.
 
+## [Unreleased]
+
+### Fixed
+- Set the calibrationMeasurementToLink matrix to the initial when `resetAll` rpc command is called (https://github.com/robotology/human-dynamics-estimation/pull/421).
+
 ## [3.0.0] - 2023-11-09
 
 ### Changed

devices/HumanStateProvider/HumanStateProvider.cpp

@@ -787,6 +787,8 @@ bool HumanStateProvider::open(yarp::os::Searchable& config)
             fixedRightRotation);
         pImpl->wearableTargets[targetName].get()->calibrationMeasurementToLink.setPosition(
             fixedRightPositionOffset);
+        pImpl->wearableTargets[targetName].get()->calibrationMeasurementToLinkInitial =
+            pImpl->wearableTargets[targetName].get()->calibrationMeasurementToLink;
         yInfo()
             << LogPrefix << "Adding Fixed Rotation for " << targetName << "==>"
             << pImpl->wearableTargets[targetName].get()->calibrationMeasurementToLink.toString();

interfaces/IWearableTargets/hde/interfaces/IWearableTargets.h

@@ -46,6 +46,7 @@ namespace hde {
 
         iDynTree::Transform calibrationWorldToMeasurementWorld;
         iDynTree::Transform calibrationMeasurementToLink;
+        iDynTree::Transform calibrationMeasurementToLinkInitial;
         iDynTree::Vector3 positionScaleFactor;
 
         // buffer variables
@@ -89,7 +90,7 @@ namespace hde {
         {
             std::lock_guard<std::mutex> lock(mutex);
             calibrationWorldToMeasurementWorld = iDynTree::Transform::Identity();
-            calibrationMeasurementToLink = iDynTree::Transform::Identity();
+            calibrationMeasurementToLink = calibrationMeasurementToLinkInitial;
         };
 
         void clearWorldCalibrationMatrix()