Add non-collocated wrench estimation in HumanWrenchProvider

conf/ros/rviz/HDERviz.rviz

@@ -383,7 +383,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 239; 41; 41
-      Max Intensity: 20
+      Max Intensity: 30
       Min Color: 138; 226; 52
       Min Intensity: 0
       Name: jL5S1Effort
@@ -533,7 +533,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 239; 41; 41
-      Max Intensity: 40
+      Max Intensity: 30
       Min Color: 138; 226; 52
       Min Intensity: 0
       Name: jLeftShoulderEffort
@@ -563,7 +563,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 239; 41; 41
-      Max Intensity: 15
+      Max Intensity: 6
       Min Color: 138; 226; 52
       Min Intensity: 0
       Name: jLeftWristEffort
@@ -713,7 +713,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 239; 41; 41
-      Max Intensity: 40
+      Max Intensity: 30
       Min Color: 138; 226; 52
       Min Intensity: 0
       Name: jRightShoulderEffort
@@ -743,7 +743,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 239; 41; 41
-      Max Intensity: 15
+      Max Intensity: 6
       Min Color: 138; 226; 52
       Min Intensity: 0
       Name: jRightWristEffort
@@ -790,27 +790,27 @@ Visualization Manager:
       Arrow Width: 0.30000001192092896
       Class: rviz/WrenchStamped
       Enabled: true
-      Force Arrow Scale: 0.02500000037252903
-      Force Color: 0; 255; 200
-      Hide Small Values: true
+      Force Arrow Scale: 0.019999999552965164
+      Force Color: 255; 239; 6
+      Hide Small Values: false
       History Length: 1
       Name: LeftHand_ExtWrenches
-      Topic: /HDE/WrenchStamped/LeftHand
-      Torque Arrow Scale: 0
+      Topic: /HDE/WrenchStamped/LeftHandCOM
+      Torque Arrow Scale: 0.5
       Torque Color: 37; 95; 255
       Unreliable: false
       Value: true
     - Alpha: 1
       Arrow Width: 0.30000001192092896
       Class: rviz/WrenchStamped
       Enabled: true
-      Force Arrow Scale: 0.02500000037252903
-      Force Color: 0; 255; 200
-      Hide Small Values: true
+      Force Arrow Scale: 0.019999999552965164
+      Force Color: 255; 239; 6
+      Hide Small Values: false
       History Length: 1
       Name: RightHand_ExtWrenches
-      Topic: /HDE/WrenchStamped/RightHand
-      Torque Arrow Scale: 0
+      Topic: /HDE/WrenchStamped/RightHandCOM
+      Torque Arrow Scale: 0.5
       Torque Color: 37; 95; 255
       Unreliable: false
       Value: true

conf/xml/Human.xml

@@ -128,7 +128,6 @@
         <param name="period">0.100</param>
         <param name="human_urdf">humanSubject03_66dof.urdf</param>
         <param name="pHRIScenario">false</param>
-        <param name="number_of_sources">2</param>
         <param name="sources">(FTShoeLeft FTShoeRight)</param>
         <group name="FTShoeLeft">
             <param name="sensorName">FTShoeLeftFTSensors</param>
@@ -149,18 +148,36 @@
             <param name="position">(-0.0026 0.0 -0.1208)</param>
         </group>
 	<!--Example of dummy wrench data sources-->
-	<!--group name="LeftHand">
+	<!--group name="LeftHandCOM">
             <param name="sensorName">none</param>
-            <param name="outputFrame">LeftHand</param>
+            <param name="outputFrame">LeftHandCOM</param>
             <param name="type">dummy</param>
             <param name="value">(0.0 0.0 0.0 0.0 0.0 0.0)</param>
         </group>
-        <group name="RightHand">
+        <group name="RightHandCOM">
             <param name="sensorName">none</param>
-            <param name="outputFrame">RightHand</param>
+            <param name="outputFrame">RightHandCOM</param>
             <param name="type">dummy</param>
             <param name="value">(0.0 0.0 0.0 0.0 0.0 0.0)</param>
         </group-->
+        <!-- MAPEstPArams -->
+        <!-- Use this group to use a Maximum-A-Posteriori estimator for the
+            the transformed wrenches
+            In order to use it the useMAPEst parameter must be set to true -->
+        <group name="MAPEstParams">
+            <param extern-name="useMAPEst" name="useMAPEst">false</param>
+            <group name="cov_measurements_NET_EXT_WRENCH_SENSOR">
+                <param name="value">1e-09</param>
+                <param name="specificElements">(LeftHandCOM)</param>
+                <param name="LeftHandCOM">(1e06 1e06 1e06 1e06 1e06 1e06)</param>
+                <param name="RightHandCOM">(1e-6 1e-6 1e-6 1e-6 1e-6 1e-6)</param>
+                <param name="LeftFoot">(1e-6 1e-6 1e-6 1e-6 1e-6 1e-6)</param>
+                <param name="RightFoot">(1e-6 1e-6 1e-6 1e-6 1e-6 1e-6)</param>
+            </group>
+            <param name="cov_measurements_RCM_SENSOR">(1e-6 1e-6 1e-6 1e-6 1e-6 1e-6)</param>
+            <param name="mu_dyn_variables">1e-9</param>
+            <param name="cov_dyn_variables">1e04</param>
+        </group>
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
                  <elem name="HumanWrenchProviderLabelFTShoeLeft">FTSourcesIWearRemapper</elem>

conf/xml/hands-pHRI.xml

@@ -123,7 +123,6 @@
         <param name="pHRIScenario">true</param>
         <param name="robot_urdf">model.urdf</param>
         <param name="number_of_source_devices">3</param>
-        <param name="number_of_sources">2</param>
         <param name="sources">(RobotLeftHandFT RobotRightHandFT)</param>
         <group name="RobotLeftHandFT">
             <param name="sensorName">ICub::ft6D::leftWBDFTSensor</param>

conf/xml/pHRI.xml

@@ -124,7 +124,6 @@
         <param name="pHRIScenario">true</param>
         <param name="robot_urdf">model.urdf</param>
         <param name="number_of_source_devices">3</param>
-        <param name="number_of_sources">4</param>
         <param name="sources">(FTShoeLeft FTShoeRight RobotLeftHandFT RobotRightHandFT)</param>
         <group name="FTShoeLeft">
             <param name="sensorName">FTShoeLeftFTSensors</param>

devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp

@@ -43,6 +43,9 @@ const std::string DeviceName = "HumanDynamicsEstimator";
 const std::string LogPrefix = DeviceName + " :";
 constexpr double DefaultPeriod = 0.01;
 
+// Timeout for checking that the iHumanState interface is providing consistent data
+constexpr static double INTERFACE_CHECK_TIMEOUT_S = 10;
+
 using namespace hde::devices;
 
 static bool parseYarpValueToStdVector(const yarp::os::Value& option, std::vector<double>& output)
@@ -1087,6 +1090,7 @@ void HumanDynamicsEstimator::run()
     // Get state data from the attached IHumanState interface
     std::vector<double> jointsPosition    = pImpl->iHumanState->getJointPositions();
     std::vector<double> jointsVelocity    = pImpl->iHumanState->getJointVelocities();
+    std::vector<std::string> jointNames   = pImpl->iHumanState->getJointNames();
 
     std::array<double, 3> basePosition    = pImpl->iHumanState->getBasePosition();
     std::array<double, 4> baseOrientation = pImpl->iHumanState->getBaseOrientation();
@@ -1098,14 +1102,17 @@ void HumanDynamicsEstimator::run()
     pImpl->berdyData.state.baseAngularVelocity.setVal(2, baseVelocity.at(5));
 
     // Set the received state data to berdy state variables
-    pImpl->berdyData.state.jointsPosition.resize(jointsPosition.size());
-    for (size_t i = 0; i < jointsPosition.size(); i++) {
-        pImpl->berdyData.state.jointsPosition.setVal(i, jointsPosition.at(i));
-    }
-
-    pImpl->berdyData.state.jointsVelocity.resize(jointsVelocity.size());
-    for (size_t i = 0; i < jointsVelocity.size(); ++i) {
-        pImpl->berdyData.state.jointsVelocity.setVal(i, jointsVelocity.at(i));
+    for(int i=0; i<jointNames.size(); i++)
+    {
+        iDynTree::JointIndex jointIndex = pImpl->humanModel.getJointIndex(jointNames[i]);
+        if(jointIndex==iDynTree::JOINT_INVALID_INDEX)
+        {
+            yError() << LogPrefix << "Joint"<<jointNames[i]<<"from iHumanState not found in the loaded model!";
+            askToStop();
+            return;
+        }
+        pImpl->berdyData.state.jointsPosition.setVal(jointIndex, jointsPosition[i]);
+        pImpl->berdyData.state.jointsVelocity.setVal(jointIndex, jointsVelocity[i]);
     }
 
     // Fill in the y vector with sensor measurements for the FT sensors
@@ -1231,40 +1238,46 @@ bool HumanDynamicsEstimator::attach(yarp::dev::PolyDriver* poly)
             return false;
         }
 
-        // Check the interface
-        if (pImpl->iHumanState->getNumberOfJoints() == 0
+        // Check the iHumanState interface
+        double interfaceCheckStart = yarp::os::Time::now();
+        int count = 1000000;
+        while (pImpl->iHumanState->getNumberOfJoints() == 0
                 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) {
-            yError() << "The IHumanState interface might not be ready";
-            return false;
+
+            if(yarp::os::Time::now()-interfaceCheckStart>INTERFACE_CHECK_TIMEOUT_S)
+            {
+                yError() << LogPrefix<<"The iHumanState interface has been providing inconsistent data for"<<INTERFACE_CHECK_TIMEOUT_S<<"seconds!";
+                return false;
+            }
         }
 
         yInfo() << LogPrefix << deviceName << "attach() successful";
     }
 
     if (deviceName == "human_wrench_provider") {
-        // Attach IAnalogServer interfaces coming from HumanWrenchProvider
-        if (pImpl->iAnalogSensor || !poly->view(pImpl->iAnalogSensor) || !pImpl->iAnalogSensor) {
-            yError() << LogPrefix << "Failed to view IAnalogSensor interface from the polydriver";
+        // Attach IHumanWrench interfaces coming from HumanWrenchProvider
+        if (pImpl->iHumanWrench || !poly->view(pImpl->iHumanWrench) || !pImpl->iHumanWrench) {
+            yError() << LogPrefix << "Failed to view iHumanWrench interface from the polydriver";
             return false;
         }
 
         // Check the interface
-        auto numberOfSensors = stoi(poly->getValue("number_of_sources").asString());
-        if (pImpl->iAnalogSensor->getChannels() != 6 * numberOfSensors) {
-            yError() << LogPrefix << "The IAnalogSensor interface might not be ready";
+        auto numberOfWrenchSources = pImpl->iHumanWrench->getNumberOfWrenchSources();
+        if ( numberOfWrenchSources == 0 ||
+             numberOfWrenchSources != pImpl->iHumanWrench->getWrenchSourceNames().size()) {
+            yError() << "The IHumanWrench interface might not be ready";
             return false;
         }
 
-        // Attach IHumanWrench interfaces coming from HumanWrenchProvider
-        if (pImpl->iHumanWrench || !poly->view(pImpl->iHumanWrench) || !pImpl->iHumanWrench) {
-            yError() << LogPrefix << "Failed to view iHumanWrench interface from the polydriver";
+        // Attach IAnalogServer interfaces coming from HumanWrenchProvider
+        if (pImpl->iAnalogSensor || !poly->view(pImpl->iAnalogSensor) || !pImpl->iAnalogSensor) {
+            yError() << LogPrefix << "Failed to view IAnalogSensor interface from the polydriver";
             return false;
         }
 
         // Check the interface
-        if (pImpl->iHumanWrench->getNumberOfWrenchSources() == 0
-                || pImpl->iHumanWrench->getNumberOfWrenchSources() != pImpl->iHumanWrench->getWrenchSourceNames().size()) {
-            yError() << "The IHumanWrench interface might not be ready";
+        if (pImpl->iAnalogSensor->getChannels() != 6 * numberOfWrenchSources) {
+            yError() << LogPrefix << "The IAnalogSensor interface might not be ready";
             return false;
         }
 

devices/HumanWrenchProvider/CMakeLists.txt

@@ -33,6 +33,7 @@ target_link_libraries(HumanWrenchProvider PUBLIC
     IWear::IWear
     iDynTree::idyntree-core
     iDynTree::idyntree-model
+    iDynTree::idyntree-estimation
     iDynTree::idyntree-high-level)
 
 yarp_install(