Write configuration file for launching wholebodydynamics for ergoCubGazeboV1 (simulated robot) as part of the gazebo simulation process via the gazebo_yarp_robotinterface

[traversaro]

For running walking and other demos, it is necessary to estimate external forces acting on the robot, and this can be done via the wholebodydynamics YARP device. This YARP device can run:

• wbd-int. Inside the gazebo simulator, via the gazebo_yarp_robotinterface plugin, a bit like on the real robot wholebodydynamics is running in the main yarprobotinterface of the real robot ergoCubSN000 (see https://github.com/robotology/robots-configuration/blob/v2.1.0/ergoCubSN000/estimators/wholebodydynamics.xml)
• wbd-ext Outside of the gazebo simulator, by accessing the relevant sensor via the multipleanalogsensorclient device

This issue tracks the wbd-int option.

[traversaro]

To solve this, we should add the appropriate conffile for wbd in https://github.com/icub-tech-iit/ergocub-software/blob/master/urdf/ergoCub/conf/ergocub.xml .

[mebbaid]

I started by adding the line

  <!--  estimators  -->
<xi:include href="estimators/wbd_ecub_sim.xml"/>

at the end of https://github.com/icub-tech-iit/ergocub-software/blob/master/urdf/ergoCub/conf/ergocub.xml, and then populating the corresponding wbd_ecub_sim.xml config file.

The estimator file in my local repo contains among other stuff the following lines

    <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
        <param name="/wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o">(l_foot_front,l_sole,l_sole)</param>
        <param name="/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o">(l_foot_rear,l_sole,l_sole)</param>
        <param name="/wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o">(r_foot_front,r_sole,r_sole)</param>
        <param name="/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o">(r_foot_rear,r_sole,r_sole)</param>
    </group>

    <action phase="startup" level="15" type="attach">
        <paramlist name="networks">
            <!-- motorcontrol -->
            <elem name="left_leg">left_leg-mc_remapper</elem>
            <elem name="right_leg">right_leg-mc_remapper</elem>
            <elem name="torso">torso-mc_remapper</elem>
            <elem name="left_arm">left_arm-mc_remapper</elem>
            <elem name="right_arm">right_arm-mc_remapper</elem>
            <!-- imu -->
            <elem name="imu">head-inertials_wrapper</elem>
            <!-- ft -->
            <elem name="l_leg_ft_sensor">ergocub_left_leg_ft</elem>
            <elem name="r_leg_ft_sensor">ergocub_right_leg_ft</elem>
            <elem name="l_foot_front_ft_sensor">ergocub_left_foot_front_ft</elem>
            <elem name="r_foot_front_ft_sensor">ergocub_right_foot_front_ft</elem>
            <elem name="l_foot_rear_ft_sensor">ergocub_left_foot_rear_ft</elem>
            <elem name="r_foot_rear_ft_sensor">ergocub_right_foot_rear_ft</elem>
            <elem name="l_arm_ft_sensor">ergocub_left_arm_ft</elem>
            <elem name="r_arm_ft_sensor">ergocub_right_arm_ft</elem>
        </paramlist>
    </action>

    <action phase="shutdown" level="2" type="detach" />

</device>

Which when dropping the model in Gazebo currently gives the following error

[DEBUG] wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas 16 where analog are  8  and mas are  8
[ERROR] WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper
[DEBUG] wholeBodyDynamics Statistics: Attaching all Fts took  0 s.
[DEBUG] wholeBodyDynamics Statistics: Attaching all IMUs.
[DEBUG] wholeBodyDynamics Statistics: Attaching all IMUs took  0 s.
[DEBUG] wholeBodyDynamics Statistics: Calibrating offsets.
[DEBUG] wholeBodyDynamics Statistics: Calibrating took  0 s.
[ERROR] Device wholebodydynamics cannot execute attach
[ERROR] Cannot run attach action on device wholebodydynamics

[mebbaid]

After some modifications on the estimator xml configuration file following the one set for the real robot in https://github.com/robotology/robots-configuration/blob/v2.1.0/ergoCubSN000/estimators/wholebodydynamics.xml, i am now facing this error

[ERROR] wholeBodyDynamics : openContactFrames : missing default/override contact for submodel composed by the links: 
[ERROR] wholeBodyDynamics : openContactFrames : r_shoulder_3
[ERROR] wholeBodyDynamics : openContactFrames : r_upperarm
[ERROR] wholeBodyDynamics : openContactFrames : r_forearm
[ERROR] wholeBodyDynamics : openContactFrames : r_wrist_1
[ERROR] wholeBodyDynamics : openContactFrames : r_wrist_2
[ERROR] wholeBodyDynamics : openContactFrames : r_hand_palm
[ERROR] wholeBodyDynamics: Problem in opening default contact frame settings.
[ERROR] |yarp.dev.PolyDriver|wholebodydynamics| Driver <wholebodydynamics> was found but could not open
[WARNING] Cannot open device wholebodydynamics

[traversaro]

I think we need to add a default contact in r_hand_palm, i.e. change r_hand in r_hand_palm in https://github.com/robotology/robots-configuration/blob/v2.1.0/ergoCubSN000/estimators/wholebodydynamics.xml#L9 . In general we should change defaultContactFrames to only contain link or frames that actually exist in URDF.

[mebbaid]

After a f2f with @traversaro, errors related to the coupling in the hands were fixed by applying #35. We got back to this error

[DEBUG] wholeBodyDynamicsDevice :: number of ft sensors found in both ft + mas 16 where analog are  8  and mas are  8
[ERROR] WholeBodyDynamicsDevice::attachAll Invalid number of MAS FT sensors after remapper

which is triggered in the following lines in the wholebodydynamics device https://github.com/robotology/whole-body-estimators/blob/c46c1852974776038a75b7c0e542ccb2917d7c7f/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp#L1743-L1747.

Further inspecting the contents of both nrMASFTSensors and remappedMASInterfaces.ftMultiSensors->getNrOfSixAxisForceTorqueSensors() I get 8 MAS FT sensors and 0 MAS rempped FT sensors respectively. This seems strange to me

[mebbaid]

It seems the same error was investigated in robotology/whole-body-estimators#163
by @HosameldinMohamed , maybe some group in the configuration is missing as per this comment
robotology/whole-body-estimators#163 (comment)

[mebbaid]

The xml files to run the wholebodydynamics device were prepared and tested with the robot in simulation. In case it is ok, I can PR adding the files to the conf directory (namely this directly of the repo https://github.com/icub-tech-iit/ergocub-software/tree/master/urdf/ergoCub/conf )

otherwise @pattacini @Nicogene feel free to let me know of a better place to commit those files.
The same applies for #34

[pattacini]

Being material related to the simulation, I'd say that your proposal is fine with me.
If @Nicogene agrees as well, you can proceed @mebbaid.

[Nicogene]

The PR has been merged, shall we close this issue?

[pattacini]

I think they aim to churn out a second PR for that.