Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Closed
gabrielenava opened this issue Apr 5, 2023 · 10 comments · Fixed by #173

Comments

@gabrielenava
Copy link

gabrielenava commented Apr 5, 2023

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 <genericSensorClient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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 <analogsensorclient> 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
Copy link
Member

traversaro commented Apr 5, 2023

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

@traversaro
Copy link
Member

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 #167 .

@HosameldinMohamed
Copy link
Collaborator

@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
Copy link
Author

gabrielenava commented Sep 12, 2023

I think the procedure I described in #169 (comment) is now outdated.

I have tried what you suggested in #169 (comment) 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:

vokoscreen-2023-09-12_17-44-52.mp4

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
Copy link
Collaborator

HosameldinMohamed commented Sep 12, 2023

@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.

Screencast_20230912_235556.mp4

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
Copy link
Author

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
Copy link
Collaborator

@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
Copy link
Collaborator

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

@gabrielenava
Copy link
Author

yes! and I have also opened the issue for the model: robotology/icub-models#209

@HosameldinMohamed
Copy link
Collaborator

Great!
Closing!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging a pull request may close this issue.

3 participants