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 https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/e081305eb7ff65d4abb6a002c6509cc75316f8af 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 https://github.com/roboticslab-uc3m/teo-openrave-models/issues/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: https://github.com/roboticslab-uc3m/teo-openrave-models/issues/28. Also, the torso has undergone some tweaks, too. We have not yet updated the original SolidWorks files in that respect: https://github.com/roboticslab-uc3m/teo-openrave-models/issues/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 https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/347ce73fc04bf722efa5e0448cc1c9bd86718205:

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 https://github.com/roboticslab-uc3m/teo-gazebo-models/issues/1#issuecomment-752938607).
• 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 https://github.com/robotology/gazebo-yarp-plugins/issues/218#issuecomment-165185365. 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 (https://github.com/robotology/gazebo-yarp-plugins/issues/218#issuecomment-183043888, https://github.com/robotology/gazebo-yarp-plugins/issues/218#issuecomment-191179893): 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 https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/ca35820248786732dbd3cadbd70a32c5feb6a8e0 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 https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/0d9fae5ceb904fda9ba3ff2d0647b2f7b49a6d7e 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:

https://user-images.githubusercontent.com/9977198/104313921-0563e000-54d9-11eb-9960-d84bf902485c.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 https://github.com/robotology/gazebo-yarp-plugins/pull/527 to match the behavior of our iPOS drives. Config files have been updated accordingly at https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/0fd0c1b883d1be683c956df1b4585012e03b6c5a.

[PeterBowman]

I'm focusing now on PID joint controllers. Real TEO hardware is somewhat tricky:

• As stated earlier, iPOS drives implement cascading control, i.e. position control entails position PID + speed PI + current PI.
• Each drive appears to be configured differently, or at least there are several common variations (see screenshots at https://github.com/roboticslab-uc3m/teo-hardware-issues/issues/45#issuecomment-759550140). Most importantly, current control is always operated in "fast loop". Some joints are configured to operate position/speed in the same manner, while others use a relaxed sampling period ("slow loop"). Slow loop sample times are ten times greater than fast loop ones. At time of writing, sampling periods in slow loop range 0.5-1 ms. Consequently, fast loop periods range 50-100 microseconds.
• The "High resolution speed estimation" checkbox is sometimes marked, sometimes not.
• A feed forward signal is applied to current control. I'm not sure to fully understand this part as it is identified as a "speed" magnitude, while there is another one referring to "acceleration" (both in the feed forward section of the position controller).
• This should be less of a concern, but keep in mind that Gazebo does not understand currents, just torques instead. Known motor constants need to be used to perform conversion between those magnitudes.
• What are the units of iPOS PID coefficients?

Ideally, we'd like to implement a new control type in Gazebo-YARP that would more closely match real robot behavior. Then, similar or exact same values as provided in the iPOS configuration would be reused in this plugin. However, the variable sampling periods are a significant obstacle. Physics in Gazebo are updated each millisecond by default and defined by two elements: <max_step_size> and real_time_update_rate (ref). If not provided, Gazebo assumes the following <physics> element:

<world>
  <!-- ... -->
  <physics type="ode">
    <max_step_size>0.001</max_step_size>
    <real_time_update_rate>1000</real_time_update_rate>
  </physics>
</world>

Gazebo's ODE plugin updates physics at a fixed rate of 1 ms, then Gazebo itself will strive to advance steps at 1000 Hz to match real time, i.e. real time factor is 0.001*1000=1.0. If too computationally expensive, simulation time will lag behind real time (real time factor less than 1.0). Info:

• http://gazebosim.org/tutorials?tut=physics_params&cat=physics
• https://answers.gazebosim.org//question/2477/what-does-update-rate-control/
• https://answers.gazebosim.org//question/16053/max_step_size-real_time_factor-and-real_time_update_rate/

I've tried to tighten simulation times and the most I can achieve without degrading the real time factor is 0.2-0.25 ms (200-250 microseconds). It is virtually not possible to achieve a fast control loop of 100 microseconds, much less 50 microseconds, as resource usage is too high.

In short, I'd need to take shortcuts, perhaps forcing all three stages (position PID, speed PI, current PI) to operate at the same "slow loop" sampling period. It is yet to be determined whether that same PID coefficients originating from the iPOS configuration would still be valid in a simulated environment and with the improvements proposed here. Moreover, what if the slow loop period is 0.5 ms instead of 1 ms, but we reuse PID coeffs in a 1 ms loop?

Options:

• Implement cascading PID control. If this turns out to differ significantly from the real PIDs, the simulated ones would need to be tuned (3 per joint, 28 joints total).
• Stick to default joint_pid_gazebo_v1, which is a position PID + speed PID (i.e. 2 per joint), and tune with care.

[jgvictores]

As spoken, both @smcdiaz and I agree on Implement cascading PID control would be a great option, even if the parameters would have to be different (hopefully, proportional, and no problem if fully re-tuned). : )

[PeterBowman]

See https://github.com/roboticslab-uc3m/yarp-devices/issues/253#issuecomment-766271908 regarding sampling rates and slow/fast loop configuration. Besides, MotionChip II Data Sheet (P091.055.MCII.DSH.0704) states in Section 3.5: Special Features:

The MotionChip II uses a fast loop for current/torque control and a slow loop for position/speed sampling. The sampling rates of these loops are synchronized and linked in a fixed ratio with the PWM frequency in order to eliminates the beat-frequency problems. The maximum sampling frequency on the fast loop can be half of the PWM frequency. The PWM frequency and the divider ratios for fast and slow loops are user programmable in a wide range. The maximum values for PWM frequency and sampling rate frequencies depend on the application configuration. (...)

[PeterBowman]

Just in case, regarding gazebo::common::PID: implementation, reference.

[PeterBowman]

MotionChip™ II Configuration Setup User Manual provides details on the implementation of the motion controllers in Section 3.5 (see also the description of their TML parameters, especially in Section 4.2.5). There is a correspondence between numerical values and TML parameters in the EasySetUp's "Drive Setup" dialog, as shown below:

All TML parameters are saved in the parameters.cfg file by EasySetUp.

[PeterBowman]

I have moved PID-related comments to https://github.com/roboticslab-uc3m/teo-gazebo-models/issues/2, which will cover further improvement on that field, and restored iCub PID coefficients at https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/6d856862ddaaaac072450795d062bea34d21d31f. 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: https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/caaadf307575e19fe7c619d49b1e96f57701ada8.

[PeterBowman]

Note I had to disable inertial information from axial joints in both arms (4 in total): https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/fb229eb44fda5a6b5eb3ac209a2f23d899101fa8. 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: https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/57e639640f12d69b7747b57be2facd70ee48b5ff.

[PeterBowman]

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

https://user-images.githubusercontent.com/9977198/108708127-4f48d880-7511-11eb-86f3-7faa3bfac95c.mp4

[PeterBowman]

Current work merged at https://github.com/roboticslab-uc3m/teo-gazebo-models/commit/a54454740ca64cc99bf957d10b2f921433dfb11b. Seems already usable, so I'm closing this issue and will focus on specific elements next (e.g. https://github.com/roboticslab-uc3m/teo-gazebo-models/issues/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 !!!!