Update WBD launch files

[HosameldinMohamed]

This PR updates the launch files for wholeBodyDynamics to align them with the latest robots-configuration and icub-models settings. The modifications include:

• F/T sensors are interfaced as MAS instead of analogSensor devices.
• IMU is interfaced as MAS instead of GenericSensor device.
• IMU frame updated.
• Fixed including the estimator files.

I tested the launch file launch-wholebodydynamics-icub-six-fts-sim.xml with iCubGazebo2_5, and launch-wholebodydynamics-icub3-sim.xml with iCubGazeboV3.

For the others, that are supposed to be with real robots, I haven't tested them.

This may fix #169

[HosameldinMohamed]

I'm not sure if we should maintain these files launch-wholebodydynamics-icubheidelberg01.xml, wholebodydynamics-icubheidelberg01-external.xml and wholebodydynamics-icubheidelberg01-robot.xml that point to a specific robot, especially that I don't see icubheidelberg01 anymore in the latest robots-configurations.

[traversaro]

Thanks a lot!

[traversaro]

I'm not sure if we should maintain these files launch-wholebodydynamics-icubheidelberg01.xml, wholebodydynamics-icubheidelberg01-external.xml and wholebodydynamics-icubheidelberg01-robot.xml that point to a specific robot, especially that I don't see icubheidelberg01 anymore in the latest robots-configurations.

Yes, I think we can remove them. Those were "special" as that robot only had legs.

[HosameldinMohamed]

I tested the launch file launch-wholebodydynamics-icub-six-fts-sim.xml with iCubGazebo2_5, and launch-wholebodydynamics-icub3-sim.xml with iCubGazeboV3.

WBD runs with no errors, I haven't really checked what it's estimating!

[HosameldinMohamed]

Also, the files wholebodydynamics-icub-eth-six-fts.xml and wholebodydynamics-icub-external-and-can-six-fts.xml seem identical. I'm not sure why they are here, and why they are called can and eth?

[traversaro]

Also, the files wholebodydynamics-icub-eth-six-fts.xml and wholebodydynamics-icub-external-and-can-six-fts.xml seem identical. I'm not sure why they are here, and why they are called can and eth?

The difference is inside the <action phase="startup" level="15" type="attach"> tag. For ETH there are not special virtual analog sensors to attach, while for can and external there are separate device for virtual analog sensors:

                <elem name="left_leg_vsens">left_leg_virtual_strain</elem>
                <elem name="right_leg_vsens">right_leg_virtual_strain</elem>
                <elem name="torso_vsens">torso_virtual_strain</elem>
                <elem name="right_arm_vsens">right_arm_virtual_strain</elem>
                <elem name="left_arm_vsens">left_arm_virtual_strain</elem>

Why is this happening? We need a bit of history here.

The first versions of the iCubs used internally a set of CAN bus for communications with their internal control and sensor embedded boards. This had limitation of the amount of data that could be sent, so iCub 2.* was developed with an internal ETH (i.e. ethernet) bus that connected it with its internal control and sensor boards. This meant that yarprobotinterface of ETH and CAN robots used different devices to communicate with controlboard, i.e. canBusMotionControl for CAN robots and embObjMotionControl for ETH robots.

However, the main difference for us is how joint torques estimates computed by WBD are sent down to the control boards for closing the torque loop. This measure is exposed to control boards via the updateVirtualAnalogSensorMeasure method contained in the yarp::dev::IVirtualAnalogSensor interface, that WBD calls inside the publishTorques method.

In the case of CAN robots, there was a dedicated device called canBusVirtualAnalogSensor that exposed the updateVirtualAnalogSensorMeasure method, see for example the devices instantiated in https://github.com/robotology/robots-configuration/blob/85237b0397c206588031289298e268c7e229e893/iCubGenova03/icub_all.xml#L39 (iCubGenova03 is a CAN robot).

Instead, for ETH robots the updateVirtualAnalogSensorMeasure method was directly exposed by the embObjMotionControl device used also for motor control, see https://github.com/robotology/icub-main/blob/v2.2.1/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h . So, for ETH robots you can connect to the embObjMotionControl devices for both reading the encoders and updating the estimated torques, while whe CAN robot you need to connect to canBusMotionControl to read encoders and to canBusVirtualAnalogSensor to update the torques estimated.

What I just described applies to when the wholebodydynamics device runs inside the main yarprobotinterface of the robot. But what happens instead when wholebodydynamics is launched externally, i.e. with an external yarprobotinterface ? In that case, the situation is similar to CAN robots, as there are two separate nws/remapper/nwc triples for reading encoders and for set measured torques. controlBoard_yarp_nws/controlboardremapper/remote_controlboard are the devices to read encoders, while virtualAnalogServer/virtualAnalogRemapper/virtualAnalogClient are the devices to update torque estimates. For this reason, when the wbd is launched externally, we need two different devices, one type to read encoders (remote_controlboard) and one to update the torque estimates (virtualAnalogClient), so this is the reason why the external case and the CAN case can be dealt with the same configuration file, while the eth with a different configuration file.

Sorry for the wall of text, if you got lost with all the names please let me know and we can discuss in person.

[HosameldinMohamed]

Thanks a lot for the clear explanation. Got it now!
I was also wondering about the virtual torque devices, now I understand!