diff --git a/CHANGELOG.md b/CHANGELOG.md index de26ce8..2b08bb0 100644 --- 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 index 1cac198..e079aad 100644 --- 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 index e3a17a7..ca099e8 100644 --- 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 & list) +bool getConfigParamsAsList(os::Searchable& config,std::string propertyName , std::vector & 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: \" "< & usedDOFs) { - return getConfigParamsAsList(config,"axesNames",usedDOFs); + bool required{true}; + return getConfigParamsAsList(config,"axesNames",usedDOFs, required); +} + +bool getAdditionalConsideredFixedJointsList(os::Searchable& config, std::vector & additionalFixedJoints) +{ + bool notRequired{false}; + return getConfigParamsAsList(config,"additionalConsideredFixedJoints",additionalFixedJoints, notRequired); } bool getGravityCompensationDOFsList(os::Searchable& config, std::vector & 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 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 ) {