search

robotology / whole-body-estimators

YARP devices that implement estimators for humanoid robots.
26 stars 12 forks source link

Wholebodydynamics not running with iCubGazebov2_5 and iCubGazeboV3 models after update to YARP 3.8 #169

Closed gabrielenava closed 1 year ago

gabrielenava commented 1 year ago

I have tried to use iCubGazeboV2_5 and iCubGazeboV3 models in Gazebo and I wanted to also run wholebodydynamics.

I have loaded the model in Gazebo and run:

YARP_ROBOT_NAME=iCubGazeboV3 yarprobotinterface --config launch-wholebodydynamics.xml

(same for V2_5 but with a different YARP_ROBOT_NAME)

and I have got the following error messages:

``` [ERROR] GenericSensorClient::open() error could not connect to /icubSim/inertial [ERROR] |yarp.dev.PolyDriver|inertial| Driver was found but could not open [WARNING] Cannot open device inertial [WARNING] Cannot open device inertial [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/left_arm/analog:o [ERROR] |yarp.dev.PolyDriver|left_upper_arm_strain| Driver was found but could not open [WARNING] Cannot open device left_upper_arm_strain [WARNING] Cannot open device left_upper_arm_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/right_arm/analog:o [ERROR] |yarp.dev.PolyDriver|right_upper_arm_strain| Driver was found but could not open [WARNING] Cannot open device right_upper_arm_strain [WARNING] Cannot open device right_upper_arm_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/left_foot_front/analog:o [ERROR] |yarp.dev.PolyDriver|left_lower_leg_front_strain| Driver was found but could not open [WARNING] Cannot open device left_lower_leg_front_strain [WARNING] Cannot open device left_lower_leg_front_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/left_foot_rear/analog:o [ERROR] |yarp.dev.PolyDriver|left_lower_leg_rear_strain| Driver was found but could not open [WARNING] Cannot open device left_lower_leg_rear_strain [WARNING] Cannot open device left_lower_leg_rear_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/left_leg/analog:o [ERROR] |yarp.dev.PolyDriver|left_upper_leg_strain| Driver was found but could not open [WARNING] Cannot open device left_upper_leg_strain [WARNING] Cannot open device left_upper_leg_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/right_foot_front/analog:o [ERROR] |yarp.dev.PolyDriver|right_lower_leg_front_strain| Driver was found but could not open [WARNING] Cannot open device right_lower_leg_front_strain [WARNING] Cannot open device right_lower_leg_front_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/right_foot_rear/analog:o [ERROR] |yarp.dev.PolyDriver|right_lower_leg_rear_strain| Driver was found but could not open [WARNING] Cannot open device right_lower_leg_rear_strain [WARNING] Cannot open device right_lower_leg_rear_strain [WARNING] |yarp.device.analogsensorclient| The 'inertial' device is deprecated in favour of 'multipleanalogsensorsclient' [WARNING] |yarp.device.analogsensorclient| The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4. [WARNING] |yarp.device.analogsensorclient| Please update your scripts. [ERROR] |yarp.device.analogsensorclient| AnalogSensorClient::open() error could not connect to /icubSim/right_leg/analog:o [ERROR] |yarp.dev.PolyDriver|right_upper_leg_strain| Driver was found but could not open [WARNING] Cannot open device right_upper_leg_strain [WARNING] Cannot open device right_upper_leg_strain [WARNING] subModel no 4 has the maximum number of variables (6). The frame l_lower_leg will not be added to it. [WARNING] subModel no 1 has the maximum number of variables (6). The frame r_lower_leg will not be added to it. [WARNING] There was some problem opening one or more devices. Please check the log and your configuration [ERROR] One or more devices failed opening... see previous log messages for more info [ERROR] Error in startup phase... see previous messages for more info [WARNING] Interrupt # 1 # received. [ERROR] wholebodydynamics is neither a wrapper nor a multiplewrapper, therefore it cannot have detach actions [ERROR] Cannot run detach action on device wholebodydynamics [WARNING] There was some problem running actions for shutdown phase . Please check the log and your configuration [ERROR] Error in shutdown phase... see previous messages for more info ```

Here the output of yarp name list:

``` registration name /clock ip 10.240.2.15 port 10002 type tcp registration name /clock/rpc ip 10.240.2.15 port 10003 type tcp registration name /gabri ip 172.17.0.1 port 10000 type tcp registration name /icubSim/head/command:i ip 10.240.2.15 port 10033 type tcp registration name /icubSim/head/inertials/measures:o ip 10.240.2.15 port 10092 type tcp registration name /icubSim/head/inertials/rpc:o ip 10.240.2.15 port 10093 type tcp registration name /icubSim/head/rpc:i ip 10.240.2.15 port 10032 type tcp registration name /icubSim/head/state:o ip 10.240.2.15 port 10034 type tcp registration name /icubSim/head/stateExt:o ip 10.240.2.15 port 10035 type tcp registration name /icubSim/left_arm/command:i ip 10.240.2.15 port 10021 type tcp registration name /icubSim/left_arm/measures:o ip 10.240.2.15 port 10084 type tcp registration name /icubSim/left_arm/rpc:i ip 10.240.2.15 port 10020 type tcp registration name /icubSim/left_arm/rpc:o ip 10.240.2.15 port 10085 type tcp registration name /icubSim/left_arm/state:o ip 10.240.2.15 port 10022 type tcp registration name /icubSim/left_arm/stateExt:o ip 10.240.2.15 port 10023 type tcp registration name /icubSim/left_foot_heel_tiptoe/measures:o ip 10.240.2.15 port 10080 type tcp registration name /icubSim/left_foot_heel_tiptoe/rpc:o ip 10.240.2.15 port 10081 type tcp registration name /icubSim/left_leg/command:i ip 10.240.2.15 port 10013 type tcp registration name /icubSim/left_leg/rpc:i ip 10.240.2.15 port 10012 type tcp registration name /icubSim/left_leg/state:o ip 10.240.2.15 port 10014 type tcp registration name /icubSim/left_leg/stateExt:o ip 10.240.2.15 port 10015 type tcp registration name /icubSim/left_leg_hip/measures:o ip 10.240.2.15 port 10090 type tcp registration name /icubSim/left_leg_hip/rpc:o ip 10.240.2.15 port 10091 type tcp registration name /icubSim/right_arm/command:i ip 10.240.2.15 port 10017 type tcp registration name /icubSim/right_arm/measures:o ip 10.240.2.15 port 10086 type tcp registration name /icubSim/right_arm/rpc:i ip 10.240.2.15 port 10016 type tcp registration name /icubSim/right_arm/rpc:o ip 10.240.2.15 port 10087 type tcp registration name /icubSim/right_arm/state:o ip 10.240.2.15 port 10018 type tcp registration name /icubSim/right_arm/stateExt:o ip 10.240.2.15 port 10019 type tcp registration name /icubSim/right_foot_heel_tiptoe/measures:o ip 10.240.2.15 port 10082 type tcp registration name /icubSim/right_foot_heel_tiptoe/rpc:o ip 10.240.2.15 port 10083 type tcp registration name /icubSim/right_leg/command:i ip 10.240.2.15 port 10025 type tcp registration name /icubSim/right_leg/rpc:i ip 10.240.2.15 port 10024 type tcp registration name /icubSim/right_leg/state:o ip 10.240.2.15 port 10026 type tcp registration name /icubSim/right_leg/stateExt:o ip 10.240.2.15 port 10027 type tcp registration name /icubSim/right_leg_hip/measures:o ip 10.240.2.15 port 10088 type tcp registration name /icubSim/right_leg_hip/rpc:o ip 10.240.2.15 port 10089 type tcp registration name /icubSim/torso/command:i ip 10.240.2.15 port 10029 type tcp registration name /icubSim/torso/rpc:i ip 10.240.2.15 port 10028 type tcp registration name /icubSim/torso/state:o ip 10.240.2.15 port 10030 type tcp registration name /icubSim/torso/stateExt:o ip 10.240.2.15 port 10031 type tcp registration name /wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10072 type tcp registration name /wholeBodyDynamics/left_foot/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10074 type tcp registration name /wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10108 type tcp registration name /wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10109 type tcp registration name /wholeBodyDynamics/left_lower_leg/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10106 type tcp registration name /wholeBodyDynamics/left_upper_leg/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10104 type tcp registration name /wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10073 type tcp registration name /wholeBodyDynamics/right_foot/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10075 type tcp registration name /wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10110 type tcp registration name /wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10111 type tcp registration name /wholeBodyDynamics/right_lower_leg/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10107 type tcp registration name /wholeBodyDynamics/right_upper_leg/cartesianEndEffectorWrench:o ip 10.240.2.15 port 10105 type tcp registration name fallback ip 224.2.1.1 port 10000 type mcast ```

Here the complete logger wbd_log.txt

cc @traversaro

traversaro commented 1 year ago

Ok, there are several regression related to that, mainly:

traversaro commented 1 year ago

Anyhow all this modifications should be just on the configuration file side, as most of the required changes in the code were done by @HosameldinMohamed in https://github.com/robotology/whole-body-estimators/pull/167 .

HosameldinMohamed commented 1 year ago

@gabrielenava where is the file launch-wholebodydynamics.xml you're referring? In #173 we updated the configuration files saved in this repo. Now they attach FTs and the IMU as MAS devices.

In particular the file launch-wholebodydynamics-icub3-sim.xml should run with iCubGazeboV3, and can be used as an example.

Let me know if this solves the issue.

gabrielenava commented 1 year ago

I think the procedure I described in https://github.com/robotology/whole-body-estimators/issues/169#issue-1655168820 is now outdated.

I have tried what you suggested in https://github.com/robotology/whole-body-estimators/issues/169#issuecomment-1712951265 and it kind of worked, but there are still some issues:

iCubGazeboV2_5

I did not know which file to run so I chose

YARP_ROBOT_NAME=iCubGazeboV2_5 yarprobotinterface --config launch-wholebodydynamics-icub-six-fts-sim.xml

when running on a terminal, it failed with file not found error. It was necessary to go into https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics/app and open a terminal there: at this point, it runs correctly.

I have checked my system files and the only launch-wholebodydynamics-icub-six-fts-sim.xml I have found is in whole-body-estimators source folder. So it seems the file is not installed in my system. I have installed this repo via the robotology-superbuild.

iCubGazebov3

Same as the other robot, it worked only when launching from the folder https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics/app

but there was another problem: the robot is oscillating and slowly drifting away in Gazebo:

https://github.com/robotology/whole-body-estimators/assets/12396934/27f00d38-3fa0-4638-9cc4-c9b2be324ef5

I think the contacts parameters are not tuned correctly. This is another problem however, not related (I think) to WBD, and I don't know if it might depend also on my Gazebo version (11.0).

When I checked the feet contact forces estimated by WBD for iCubGazebov3 were all close to zero, while for iCubGazeboV2_5 were reasonable values.

HosameldinMohamed commented 1 year ago

@gabrielenava Thanks for your feedback!

Concerning

when running on a terminal, it failed with file not found error. It was necessary to go into https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics/app and open a terminal there: at this point, it runs correctly.

It seems that I broke the configuration files installation in #173. It should be fixed in #175.

And regarding iCubGazeboV3 shaking and drifting, I agree it's not related to WBD, but I don't have this issue in my system. I'm using v2.2.2 in icub-models. Not sure when was the issue fixed.

https://github.com/robotology/whole-body-estimators/assets/45564317/6ed58c8e-b97e-4389-8c24-7e23f0da1d0d

Also, the estimation of feet contacts seems "normal" in my case:

Left foot front

3.66120783448098574553 0.922849225597064504178 127.771546140796417035 -0.969730728577567924198 -3.06654214145255021862 0.139412258084381790457
3.68063511939020315822 1.57882201919868681017 127.439501052720459029 -0.988778676293657787788 -2.89611911850393877188 0.15138702973839499033
3.34450876972631405692 1.9595538444575260062 126.557223862387417057 -1.12985253274846031424 -2.78592242000950296443 0.190348312759847509978

Left foot rear

1.90324373461110751826 1.71193289999333653206 132.168051298029723739 -0.376816046801032422664 10.3783654958028961346 0.0472784119789355222752
1.78014510121063085357 2.04096348574618779992 131.178842930555134672 -0.585956843020796536159 10.3949124380116959543 0.0239582450138559610586
1.19668942240051268122 1.03917998447048609556 132.963678933346159283 -0.314559367198131600851 10.3527959393531929777 0.0916490211487893391418
0.0602791002565042852157 -0.235721996812413625566 134.426944968608211184 0.121798309755493563977 10.035671886909408812 0.166763060407070479174

Right foot front

-1.93057435140804911633 0.108327445202765521026 110.90075537904820635 0.816810731544289625461 -3.52935255684884463534 0.0419903649484275054427
-2.19194614647399799168 -1.0268733579710953574 111.92008926446276007 0.803546095386475478506 -3.56453435303649657229 0.027390876980444851807
-2.28158148958283435448 -1.9496044757572561501 113.081138564320710316 0.729527879087176867046 -3.63336368468010517674 0.00987268678912646413526
-2.39416577738379388052 -2.95295389007842290496 113.460253050381595585 0.666477159036421906713 -3.79481222778000226725 -0.0599495509844482771245

Right foot rear

-0.702729271066882876795 -5.27550203417193053212 140.873992058737201205 0.853386694974511250678 9.98670668977817754808 0.389462827114523846284
0.1900180702693928525 -4.91737269468590820765 138.858794936449925217 0.555764805242231707538 10.2693770858969966042 0.417942617423682161171
0.674106979688482432778 -5.28655719650498756579 138.107253081257084659 0.584125663163876929218 10.45979622069633308 0.414348810889140994895
1.40193417461878966357 -5.93679864804027346281 136.227351857297009019 0.643046462630222803725 10.5100276879799849894 0.434433895289880411816
gabrielenava commented 1 year ago

And regarding iCubGazeboV3 shaking and drifting, I agree it's not related to WBD, but I don't have this issue in my system. I'm using v2.2.2 in icub-models. Not sure when was the issue fixed.

might be something related to my Gazebo version, which is 11.13?

I have recently (last week) cloned the robotology-superbuild from scracth, so I should be up-to date with the most recent versions of model-related repos.

When I drop the robot in gazebo, contacts are very unstable in my case and the robot drifts away:

image

this is not happening with iRonCub3

HosameldinMohamed commented 1 year ago

@gabrielenava I upgraded Gazebo to 11.13 (before it was 11.12), and the shaking issue appeared!

I think it's worth opening an issue in icub-models.

HosameldinMohamed commented 1 year ago

I think we can close this issue since the configuration files of WBD are now aligned!

gabrielenava commented 1 year ago

yes! and I have also opened the issue for the model: https://github.com/robotology/icub-models/issues/209

HosameldinMohamed commented 1 year ago

Great! Closing!