fix bug when erappling secondary calibration

robotology · Nov 26, 2019 · 1bef99e · 1bef99e
1 parent b40af88
commit 1bef99e
Showing 1 changed file with 32 additions and 15 deletions.
diff --git a/devices/HumanStateProvider/HumanStateProvider.cpp b/devices/HumanStateProvider/HumanStateProvider.cpp
@@ -164,6 +164,7 @@ class HumanStateProvider::impl
     // Buffers
     std::unordered_map<std::string, iDynTree::Rotation> linkRotationMatrices;
     std::unordered_map<std::string, iDynTree::Transform> linkTransformMatrices;
+    std::unordered_map<std::string, iDynTree::Transform> linkTransformMatricesRaw;
     std::unordered_map<std::string, iDynTree::Rotation> linkOrientationMatrices;
     std::unordered_map<std::string, iDynTree::Twist> linkVelocities;
     iDynTree::VectorDynSize jointConfigurationSolution;
@@ -241,6 +242,9 @@ class HumanStateProvider::impl
     bool getLinkTransformFromInputData(std::unordered_map<std::string, iDynTree::Transform>& t);
     bool getLinkVelocityFromInputData(std::unordered_map<std::string, iDynTree::Twist>& t);
 
+    // calibrate data
+    bool applySecondaryCalibration(const std::unordered_map<std::string, iDynTree::Transform>& t_in, std::unordered_map<std::string, iDynTree::Transform>& t_out);
+
     // solver initialization and update
     bool createLinkPairs();
     bool initializePairwisedInverseKinematicsSolver();
@@ -1053,12 +1057,19 @@ bool HumanStateProvider::close()
 void HumanStateProvider::run()
 {
     // Get the link transformations from input data
-    if (!pImpl->getLinkTransformFromInputData(pImpl->linkTransformMatrices)) {
+    if (!pImpl->getLinkTransformFromInputData(pImpl->linkTransformMatricesRaw)) {
         yError() << LogPrefix << "Failed to get link transforms from input data";
         askToStop();
         return;
     }
 
+    // Apply the secondart calibration to input data
+    if (!pImpl->applySecondaryCalibration(pImpl->linkTransformMatricesRaw, pImpl->linkTransformMatrices)) {
+        yError() << LogPrefix << "Failed to apply secondary calibration to input data";
+        askToStop();
+        return;
+    }
+
     // Get the link velocity from input data
     if (!pImpl->getLinkVelocityFromInputData(pImpl->linkVelocities)) {
         yError() << LogPrefix << "Failed to get link velocity from input data";
@@ -1280,7 +1291,7 @@ bool HumanStateProvider::impl::applyRpcCommand()
                 }
                 // computing new calibration
                 iDynTree::Rotation linkRotationZero = kinDynComputations->getWorldTransform(kinDynComputations->model().getLinkIndex(linkToCalibrateName)).getRotation();
-                iDynTree::Rotation secondaryCalibrationRotation = linkTransformMatrices.at(linkToCalibrateName).getRotation().inverse() * linkRotationZero;
+                iDynTree::Rotation secondaryCalibrationRotation = linkTransformMatricesRaw.at(linkToCalibrateName).getRotation().inverse() * linkRotationZero;
                 // add new calibration
                 secondaryCalibrationRotations.emplace(linkToCalibrateName,secondaryCalibrationRotation);
                 yInfo() << LogPrefix << "secondary calibration for " << linkToCalibrateName << " is set";
@@ -1338,19 +1349,6 @@ bool HumanStateProvider::impl::getLinkTransformFromInputData(
         iDynTree::Rotation rotation;
         rotation.fromQuaternion({orientation.data(), 4});
 
-        // Apply secondary calibration to the rotation
-        if (!(secondaryCalibrationRotations.find(modelLinkName)
-              == secondaryCalibrationRotations.end())) {
-            yInfo() << "secondary calibration matrix is: ";
-            yInfo() << secondaryCalibrationRotations.at(modelLinkName).toString();
-            yInfo() << "applying secondary calibration for " << modelLinkName;
-            yInfo() << "before";
-            yInfo() << rotation.toString();
-            rotation = rotation * secondaryCalibrationRotations.at(modelLinkName);
-            yInfo() << "after";
-            yInfo() << rotation.toString();
-        }
-
         iDynTree::Transform transform(rotation, pos);
 
         // Note that this map is used during the IK step for setting a target transform to a
@@ -1361,6 +1359,25 @@ bool HumanStateProvider::impl::getLinkTransformFromInputData(
     return true;
 }
 
+bool HumanStateProvider::impl::applySecondaryCalibration(
+        const std::unordered_map<std::string, iDynTree::Transform> &transforms_in, std::unordered_map<std::string, iDynTree::Transform> &transforms_out)
+{
+    transforms_out = transforms_in;
+
+    for (const auto& linkMapEntry : wearableStorage.modelToWearable_LinkName) {
+        const ModelLinkName& modelLinkName = linkMapEntry.first;
+        if (!(secondaryCalibrationRotations.find(modelLinkName)
+              == secondaryCalibrationRotations.end())) {
+            iDynTree::Rotation rotation = transforms_out[modelLinkName].getRotation();
+            rotation = rotation * secondaryCalibrationRotations.at(modelLinkName);
+
+            transforms_out[modelLinkName].setRotation(rotation);
+        }
+    }
+
+    return true;
+}
+
 bool HumanStateProvider::impl::getLinkVelocityFromInputData(
     std::unordered_map<std::string, iDynTree::Twist>& velocities)
 {