Add support for nonConsideredAxesPositions parameter (#188)

CMakeLists.txt

@@ -32,7 +32,7 @@ set(YARP_REQUIRED_VERSION 3.0.1)
 
 find_package(YARP ${YARP_REQUIRED_VERSION} REQUIRED COMPONENTS robotinterface idl_tools eigen os sig)
 find_package(Eigen3 REQUIRED)
-find_package(iDynTree 10.0.0 REQUIRED)
+find_package(iDynTree 12.2.0 REQUIRED)
 find_package(ICUB REQUIRED)
 
 yarp_configure_plugins_installation(${PROJECT_NAME})

devices/wholeBodyDynamics/README.md

@@ -14,6 +14,7 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
 |:--------------:|:--------------:|:-----------------:|:-----:|:-------------:|:--------:|:-----------------------------------------------------------------:|:-----:|
 | axesNames      |      -         | vector of strings |   -   |   -           | Yes      | Ordered list of the axes that are part of the remapped device.    |       |
 | additionalConsideredFixedJoints      |      -         | vector of strings |   -   |   -           | No      | Optional list of fixed joints being omitted by the URDF model parser but are needed to be considered in the joint list.    |       |
+| nonConsideredAxesPositions      |      -         | complex structure, like ((nameOfJoint, 10.0), (nameOfotherJoint, 12.0)) |   -   |   -           | No      | Specify values for joints that are not in axesNames. The positions values specified in radians for revolute joints, and meters for prismatic joints. |       |
 | modelFile      |      -         | path to file      |   -   | model.urdf    | No       | Path to the URDF file used for the kinematic and dynamic model.   |       |
 | assume_fixed    |                | frame name        |   -   |     -         | No       | If it is present, the initial kinematic source used for estimation will be that specified frame is fixed, and its gravity is specified by fixedFrameGravity. The IMU related parameters from the configuration are not used if this parameter exists. Otherwise, the default IMU will be used. | |
 | fixedFrameGravity  |      -     | vector of doubles | m/s^2 | -             | Yes      | Gravity of the frame that is assumed to be fixed, if the kinematic source used is the fixed frame. | |

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

@@ -14,8 +14,11 @@
 #include <iDynTree/YARPConversions.h>
 #include <iDynTree/Utils.h>
 #include <iDynTree/EigenHelpers.h>
+#include <iDynTree/ModelLoader.h>
+
 #include <cassert>
 #include <cmath>
+#include <unordered_map>
 
 namespace yarp
 {
@@ -218,6 +221,48 @@ bool getAdditionalConsideredFixedJointsList(os::Searchable& config, std::vector<
     return getConfigParamsAsList(config,"additionalConsideredFixedJoints",additionalFixedJoints, notRequired);
 }
 
+bool getNonConsideredAxesPositions(os::Searchable& config, std::unordered_map<std::string, double>& removedJointPositions)
+{
+    bool notRequired{false};
+    std::string propertyName = "nonConsideredAxesPositions";
+    yarp::os::Property prop;
+    prop.fromString(config.toString().c_str());
+    yarp::os::Bottle *propNames=prop.find(propertyName).asList();
+    if(propNames==nullptr)
+    {
+        return false;
+    }
+
+    for(auto elem=0u; elem < propNames->size(); elem++)
+    {
+        const auto value = propNames->get(elem);
+        if (!value.isList())
+        {
+            yError() << "WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \". Expected vector of (name,scalarValue) pairs.\n";
+            return false;
+        }
+        yarp::os::Bottle *nonConsideredValuePair = value.asList();
+        if (nonConsideredValuePair->size() != 2)
+        {
+            yError() << "WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \". Expected vector of (name,scalarValue) pairs.\n";
+            return false;
+        }
+
+        if (!nonConsideredValuePair->get(0).isString() || !nonConsideredValuePair->get(1).isFloat64())
+        {
+            yError() << "WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \". Expected vector of (name,scalarValue) pairs.\n";
+            return false;
+        }
+
+        std::string jointName = nonConsideredValuePair->get(0).asString();
+        double jointPosition = nonConsideredValuePair->get(1).asFloat64();
+        removedJointPositions[jointName] = jointPosition;
+
+    }
+
+    return true;
+}
+
 bool getGravityCompensationDOFsList(os::Searchable& config, std::vector<std::string> & gravityCompensationDOFs)
 {
     bool required{true};
@@ -332,7 +377,15 @@ bool WholeBodyDynamicsDevice::openEstimator(os::Searchable& config)
         estimationJointNames.insert(estimationJointNames.end(), additionalConsideredJoints.begin(), additionalConsideredJoints.end());
     }
 
-    ok = estimator.loadModelAndSensorsFromFileWithSpecifiedDOFs(modelFileFullPath,estimationJointNames);
+    std::unordered_map<std::string, double> removedJointPositions;
+
+    // Return value is ignored as the parameter is not required
+    getNonConsideredAxesPositions(config, removedJointPositions);
+
+    iDynTree::ModelLoader mdlLoader;
+    mdlLoader.loadReducedModelFromFile(modelFileFullPath, estimationJointNames, removedJointPositions);
+
+    ok = estimator.setModel(mdlLoader.model());
     if( !ok )
     {
         yInfo() << "wholeBodyDynamics : impossible to create ExtWrenchesAndJointTorquesEstimator from file "

devices/wholeBodyDynamics/test/WholeBodyDynamicsUnitTests.cpp

@@ -54,7 +54,7 @@ TEST_CASE("WholeBodyDynamicsSmokeTest")
     REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup));
     REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseRun));
 
-    // If the robotinterface started, we are good to go, we can close it 
+    // If the robotinterface started, we are good to go, we can close it
     REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1));
     REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt2));
     REQUIRE(xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt3));

devices/wholeBodyDynamics/test/ergocub_simulated_wholebodydynamics.xml

@@ -4,6 +4,7 @@
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
   <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+  <param name="nonConsideredAxesPositions">((camera_tilt,0.17), (l_pinkie_prox,-0.17), (r_pinkie_prox,-0.17))</param>
   <param name="modelFile">model.urdf</param>
   <param name="fixedFrameGravity">(0,0,-9.81)</param>
   <param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>