Prepare COLLADA/SDF models and generate working config

[PeterBowman]

We are moving to Gazebo! Main reason being: OpenRAVE is scarcely supported (installation is a complete mess and allegedly stable branches lack proper CI testing), we've never fully implemented all YARP interfaces (e.g. torque control) and recent versions are missing basic functionality (no qtcoin viewer on Ubuntu 20.04 = no off-screen rendering for RGB sensors).

It turns out YARP folks have paved the way for this transition. We can take advantage of a wide range of Gazebo plugins for YARP, including torque control, and their SDF models are a nice reference as well:

• https://github.com/robotology/icub-gazebo (COLLADA+SDF models)
• https://github.com/robotology/gazebo-yarp-plugins

This is a tracking issue in a WIP state at time of writing. Commit e081305 introduces a set of .dae models originating from the VRML 1.0 files of teo-openrave-models. In order to perform the conversion (via meshlabserver -i part.wrl -o part.dae), first I had to upgrade them to VRML 2.0 via FreeCAD. This is documented at roboticslab-uc3m/teo-openrave-models#13. Note I had a missing libnglib.so error on Ubuntu focal, just install it via apt and update the linker path.

It is important to note these models do not reflect the current state of the robot: roboticslab-uc3m/teo-openrave-models#28. Also, the torso has undergone some tweaks, too. We have not yet updated the original SolidWorks files in that respect: roboticslab-uc3m/teo-openrave-models#28.

A few more comments and links regarding the SDF/COLLADA stuff:

• Gazebo 9 (available via apt on Ubuntu 18.04 and 20.04) adheres to SDF v1.6: specs, ref.
• All poses in SDF are referred to the parent frame, unless world frame is explicitly used: pose frame semantics. In OpenRAVE and starting from SDF v1.7, one can simply specify the frame of reference, usually a sibling link within the same kinematic chain.
• Just in case: Gazebo robot tutorials.
• The COLLADA files I obtained from VRML 2.0 do not define material properties, i.e. these parts have no color and Gazebo renders them in a shiny black color. Materials can be defined in two ways: either in the .dae files or in the .sdf models.

[PeterBowman]

Full model, static hands, no gravity nor colors (yet):

Motor control (floating in zero-grav, hence the tilt):

[PeterBowman]

Link inertial data added (using dynamic_info_relative.csv). For some weird reason, the upper limbs start shaking frantically unless this information is commented out from both axial sagittal links.

[PeterBowman]

RGBD sensor added at 347ce73:

There is something phishy, though. Specs say I should provide <sensor type="depth_camera">, but it only seems to work with <sensor type="depth">. Also, the <clip> tag nested in <depth_camera> is not recognized, thus I need to set a "global" clip planes tag thus affecting both RGB and depth streams. The above screenshot was taken with no clipping enabled, hence the pixel overflow (rainbow in the lower half of the depth image).

PS the <depth_camera><clip></clip></depth_camera> thing was added in SDF 1.6, so it seems Gazebo 9 adheres to SDF 1.5.

PS2 nope, SDF 1.6 is supported since Gazebo 7 according to this document. Also, while SDF 1.5 only supports camera and depth sensor types, SDF 1.6 expands this list to camera, depth_camera and rgbd_camera. It might be that depth was intentionally left for backwads compatibility (although not documented in the SDF description) and the YARP plugin only understands the old spec, by now.

[PeterBowman]

Gazebo 11 is available on Ubuntu bionic and focal from OSRF package server, while Ubuntu xenial is compatible up to Gazebo 10 (installer script, focal package).

[PeterBowman]

Remarks regarding control and stability:

• If dynamics are not required, remove all <inertial> link data from the .sdf files, disable gravity and fix the root joint (so that the robot doesn't bend as in Prepare COLLADA/SDF models and generate working config #1 (comment)).
• With dynamics and gravity enabled, make sure to set reasonable PID parameters in the .ini files read by the Gazebo-YARP controlboard plugin (example). Default values lead to excessive overshoot, as far as I can tell.

Now, focusing on the PID part, keep in mind Gazebo (and presumably the real iCub, too) doesn't implement cascade control such as our Technosoft iPOS drives do (scheme). The following formula is provided in the iCub wiki:

u = -Kp * (q-qd) - Kd * (dq-dqd) - Ki * int(q-qd) + Offset

Regarding the units of Kp, Kd, Ki, check robotology/gazebo-yarp-plugins#218 (comment). An additional .ini parameter, controlUnits, was introduced to deal with the radians/degrees mismatch between Gazebo (using radians) and YARP (using degrees). The naming is somewhat infortunate (robotology/gazebo-yarp-plugins#218 (comment), robotology/gazebo-yarp-plugins#218 (comment)): metric_units refers to gains expressed in Nm/deg, while si_units implies Nm/rad. See implementation.

[PeterBowman]

Moreover, real joints have friction! Estimations on TEO's left sagittal shoulder revealed a static friction coefficient of ~0.7 and a viscous damping coefficient of ~0.001, encoded in SDF as <friction> and <damping>, respectively (ref). Check Force-Sensorless Friction and Gravity Compensation for Robots by S. Morante and @jgvictores regarding the underlying methodology.

[PeterBowman]

Gazebo understands two sets of 3D models represented via VRML/COLLADA/STL/...: visual and collision. The former is meant purely for visualization purposes and it's expected to provide full detail of the looks of the robot in the simulated environment. The latter is usually a low-polygon mesh generated from the visual counterpart and it intervenes in collision simulation.

@jgvictores has spotted an useful feature of OpenRAVE that composes simpler models from complex meshes using convex decomposition: ref. Check also this link for a .pp-to-.stl converter tool.

[PeterBowman]

The COLLADA files I obtained from VRML 2.0 do not define material properties, i.e. these parts have no color and Gazebo renders them in a shiny black color. Materials can be defined in two ways: either in the .dae files or in the .sdf models.

The .wrl files we've been using along with OpenRAVE are subpar regarding part colors, with little detail and granularity (e.g. F/T sensors are also painted in that same grayish metal tone). Ideally, we'd wait until the original SolidWorks project is updated and better VRML/COLLADA files are generated upon that. Until then, I'd just patch the material data into the .sdf files we have now.

Commit ca35820 achieves this, see <material> specs. Colors have been obtained from old VRML 1.0 models (ref, example).

Result:

PS not all values have been coded into the .sdf models. For instance, I don't know how to interpret "shininess".

PS2 I've just noticed there is an advantage in defining material properties inside the VRML/COLLADA model files: a single .wrl/.dae can define multiple pieces, each of them having a singular material definition (i.e. distinct color schemes). In fact, the hip&waist links (RootWaist_links) should look a bit different, not entirely black.

[PeterBowman]

I'm using the following Python script to obtain joint positions, speeds and accelerations in order to plot them on a yarpscope:

#! /usr/bin/env python

import sys
import yarp

yarp.Network.init()

p_pos = yarp.BufferedPortBottle()
p_pos.open('/remoteEncoders/pos')

p_vel = yarp.BufferedPortBottle()
p_vel.open('/remoteEncoders/vel')

p_acc = yarp.BufferedPortBottle()
p_acc.open('/remoteEncoders/acc')

options = yarp.Property()
options.put('device', 'remote_controlboard')
options.put('remote', sys.argv[1])
options.put('local', '/remoteEncoders')

dd = yarp.PolyDriver(options)
enc = dd.viewIEncoders()
axes = enc.getAxes()

pos = yarp.DVector(axes)
vel = yarp.DVector(axes)
acc = yarp.DVector(axes)

while True:
    enc.getEncoders(pos)
    p_pos.prepare().fromString(' '.join(map(str, list(pos))))
    p_pos.write()
    
    enc.getEncoderSpeeds(vel)    
    p_vel.prepare().fromString(' '.join(map(str, list(vel))))
    p_vel.write()
    
    enc.getEncoderAccelerations(acc)    
    p_acc.prepare().fromString(' '.join(map(str, list(acc))))
    p_acc.write()
    
    yarp.delay(0.01)

dd.close()
yarp.Network.fini()

YARP_PORT_PREFIX=/ys/pos yarpscope --remote /remoteEncoders/pos --index 3 &
YARP_PORT_PREFIX=/ys/vel yarpscope --remote /remoteEncoders/vel --index 3 &
YARP_PORT_PREFIX=/ys/acc yarpscope --remote /remoteEncoders/acc --index 3 &

Note accelerations are not implemented yet.

Also, these three streams can be easily concatenated (and thus plotted in a single yarpscope window) with:

yarp merge --input /remoteEncoders/pos /remoteEncoders/vel /remoteEncoders/acc --output /merged

[jgvictores]

Gazebo understands two sets of 3D models represented via VRML/COLLADA/STL/...: visual and collision. The former is meant purely for visualization purposes and it's expected to provide full detail of the looks of the robot in the simulated environment. The latter is usually a low-polygon mesh generated from the visual counterpart and it intervenes in collision simulation.

@jgvictores has spotted an useful feature of OpenRAVE that composes simpler models from complex meshes using convex decomposition: ref. Check also this link for a .pp-to-.stl converter tool.

I also just recalled/checked that this was done at https://github.com/roboticslab-uc3m/teo-simox-models/tree/develop/teo/xmlfiles (perma): see models vs collisionModels folders.

[PeterBowman]

Thanks! Collision models added at 0d9fae5 in STL format. Even though these are binaries, it's just 152 kB, much less than the 1.4 MB worth of COLLADA visual models.

Note that this addition also enables self collisions via <self_collide> tag in the .sdf files, BUT:

• If set model-wide within <model>: the robot goes bananas due to non-consecutive links colliding even in zero pose (mainly shoulders and hips, I presume).
• Two non-consecutive links collide if any of them defines this property in their <link> tag. I gave it a try. The following clip reflects the behavior of the robot if root (waist) links are configured with <self_collide>1</self_collide> and the right arm is set on a collision path with it:

gazebo-self-collide.mp4

Long story short, internal position controllers are trying to proceed further despite having found an obstacle, max torque is exceeded and boom. Gazebo-YARP code should be tweaked in order to account for this kind of situations. Note: the iCub does not enable the self collision feature.

[PeterBowman]

The Gazebo-YARP control board currently supports constant-speed and minimum-jerk trajectories in position control. A new trapezoidal speed profile generator has been proposed at robotology/gazebo-yarp-plugins#527 to match the behavior of our iPOS drives. Config files have been updated accordingly at 0fd0c1b.

[PeterBowman]

I have moved PID-related comments to #2, which will cover further improvement on that field, and restored iCub PID coefficients at 6d85686. I'm just adding here that Kp values for both trunk joints have been substantially raised (and now they're equal to Kp in leg joints) in order to prevent oscillation due to arm motion: caaadf3.

[PeterBowman]

Note I had to disable inertial information from axial joints in both arms (4 in total): fb229eb. They used to start violent motion whenever other arm joints were commanded to move. Not sure why, but might be an intrinsic issue of the model itself since I believe this behavior could be also observed without loading YARP-Gazebo plugins and simply issuing a simulation restart command (Ctrl+R). By the way, I realised that mass information should not be a problem, just moments of inertia: 57e6396.

[PeterBowman]

This is TEO self-presentation demo on Gazebo (order is not strict, I was commanding it step by step via RPC):

teo-self-presentation-gazebo-resized.mp4

[PeterBowman]

Current work merged at a544547. Seems already usable, so I'm closing this issue and will focus on specific elements next (e.g. #2).

There is still so much nice robotology stuff to exploit. Useful links:

• list of Gazebo-YARP plugins
• gazebo-yarp-plugins installation guide

[jgvictores]

Really cool!!! Thank you so much @PeterBowman !!!!