Implement FT and IMU sensors

[PeterBowman]

Luís prepared a modified TEO model including force-torque sensors: roboticslab-uc3m/teo-main#51. Review and upload them here. I recall that the placement was not ideal, i.e. we had to add a dummy link or whatever. Perhaps there is a better way?

Follow-up to #1.

[PeterBowman]

I'm pasting here the lines added by Gerson, for better visibility.

the modified TEO model are the following:
teo URDF: /usr/local/share/iCub/robots/teoSim/ (teoc.urdf)
teo SDF: /usr/local/share/gazebo/models/teo/ (teo.sdf)
this directories have to be in the following paths:
teo.zip
teoSim.zip
If you want to use the Icub's walking-controller you need to modify the walking-controller configuration file, this is found in ( /usr/local/share/ICUBcontrib/robots/teoSim)

[PeterBowman]

FT/IMU plugins need to take care of the new wrapping method as previously covered in #6 for motor control and cameras. See robotology/gazebo-yarp-plugins#586.

[PeterBowman]

I'm following the 1.6 sensor specs defined here. Note I am adjusting the IMU orientation frame via <pose> since <orientation_reference_frame> with <localization>CUSTOM</localization> does not seem to work for me. The IMU local frame is defined in the user manual uploaded to https://github.com/roboticslab-uc3m/datasheets-and-manuals. Our model is MTi-28A53G35 according to the BOM.

[PeterBowman]

The Gazebo-YARP plugin requires some tweaks in order to overcome the frame orientation issue. In robotology/gazebo-yarp-plugins#639 I'm proposing a new useWorldReferenceOrientation configuration option that seeks to match the behavior of the real sensor.

[PeterBowman]

Done at ff74a2c. I had to split the terminal hand/foot link models in order to place an intermediate fixed joint for the corresponding FT sensor plugin. The masses are just estimates (0.45 kg for the LacqueyFetch, which is somewhat accurate, and 1 kg for the feet) and the inertias have not been adjusted post-split, so those values need to be reviewed at some point.

Notable collateral changes:

• Added non-fixed lacquey fetch and stump worlds.
• The default robot model now lacks grippers. We may add them back easily since I have preserved the original pre-split, unactuated models; just wasn't sure if necessary or worthy.

Note the unmerged commit 8532631 remains pending review of robotology/gazebo-yarp-plugins#639. (edit: this is no longer necessary, the new default behavior matches our expectations regarding how the IMU works)