[WBD] Add fixed joints l_foot_ft_sensor and r_foot_ft_sensor explicit…

…ly (#109) * [WBD] Add fixed joints l_foot_ft_sensor and r_foot_ft_sensor * update comment * add config param additionalConsideredFixedJoints * update README and CHANGELOG
robotology · Apr 13, 2021 · 353985f · 353985f
1 parent 9457244
commit 353985f
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 4 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
 ## Unreleased
+- Added an optional config parameter list of fixed joints `additionalConsideredFixedJoints` being omitted by the URDF model parser but are needed to be considered in the joint list. (See [!109](https://github.com/robotology/whole-body-estimators/pull/109)).
 - Added some debug prints to detect where the wholeBodyDynamics device hangs at startup. (See [!106](https://github.com/robotology/whole-body-estimators/pull/106)).
 - Fixed the configuration files to run wholeBodyDynamics with iCubGazeboV3. (See [!107](https://github.com/robotology/whole-body-estimators/pull/107)).
 - Avoid to use getOutputCount before broadcasting data. (See [!108](https://github.com/robotology/whole-body-estimators/pull/108)).

diff --git a/devices/wholeBodyDynamics/README.md b/devices/wholeBodyDynamics/README.md
@@ -13,6 +13,7 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
 | Parameter name | SubParameter   | Type              | Units | Default Value | Required |   Description                                                     | Notes |
 |:--------------:|:--------------:|:-----------------:|:-----:|:-------------:|:--------:|:-----------------------------------------------------------------:|:-----:|
 | 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.    |       |
 | 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. | |

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -148,14 +148,17 @@ void addVectorOfStringToProperty(yarp::os::Property& prop, std::string key, std:
     return;
 }
 
-bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std::vector<std::string> & list)
+bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std::vector<std::string> & list, bool isRequired)
 {
     yarp::os::Property prop;
     prop.fromString(config.toString().c_str());
     yarp::os::Bottle *propNames=prop.find(propertyName).asList();
     if(propNames==nullptr)
     {
-        yError() <<"WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \" should be followed by a list\n";
+        if(isRequired)
+        {
+            yError() <<"WholeBodyDynamicsDevice: Error parsing parameters: \" "<<propertyName<<" \" should be followed by a list\n";
+        }
         return false;
     }
 
@@ -170,12 +173,20 @@ bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std
 
 bool getUsedDOFsList(os::Searchable& config, std::vector<std::string> & usedDOFs)
 {
-    return getConfigParamsAsList(config,"axesNames",usedDOFs);
+    bool required{true};
+    return getConfigParamsAsList(config,"axesNames",usedDOFs, required);
+}
+
+bool getAdditionalConsideredFixedJointsList(os::Searchable& config, std::vector<std::string> & additionalFixedJoints)
+{
+    bool notRequired{false};
+    return getConfigParamsAsList(config,"additionalConsideredFixedJoints",additionalFixedJoints, notRequired);
 }
 
 bool getGravityCompensationDOFsList(os::Searchable& config, std::vector<std::string> & gravityCompensationDOFs)
 {
-    return getConfigParamsAsList(config,"gravityCompensationAxesNames",gravityCompensationDOFs);
+    bool required{true};
+    return getConfigParamsAsList(config,"gravityCompensationAxesNames",gravityCompensationDOFs, required);
 }
 
 
@@ -277,6 +288,16 @@ bool WholeBodyDynamicsDevice::openEstimator(os::Searchable& config)
 
     yInfo() << "wholeBodyDynamics : Loading model from " << modelFileFullPath;
 
+    // Add additional fixed joints listed with the parameter `additionalConsideredFixedJoints`
+    std::vector<std::string> additionalConsideredJoints;
+    bool additionalFixedJointsExist = getAdditionalConsideredFixedJointsList(config,additionalConsideredJoints);
+    if(additionalFixedJointsExist)
+    {
+        yInfo() << "wholeBodyDynamics: Loading additional fixed joints from the config file: " << additionalConsideredJoints;
+        // Append additionalConsideredJoints to estimationJointNames
+        estimationJointNames.insert(estimationJointNames.end(), additionalConsideredJoints.begin(), additionalConsideredJoints.end());
+    }
+
     ok = estimator.loadModelAndSensorsFromFileWithSpecifiedDOFs(modelFileFullPath,estimationJointNames);
     if( !ok )
     {