[TPE] Support setting individual link velocity

[claireyywang]

depends on gazebosim/gz-physics#179

Support setting link velocity with TPE. Link velocity demo is add to velocity_control. velocity_control now supports both velocity_control.sdf and pendulum_links.sdf. The following is a demo moving both links of a pendulum.

exploding.pendulum.mp4

To test:

ign gazebo -v 4 pendulum_links.sdf --physics-engine ignition-physics-tpe-plugin

and publish link vel cmds, e.g.:

ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {x: 0.5}"


ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "linear: {x:0.2}, angular: {x: 0.5}"

[iche033]

works for me! Made some comments about supporting multiple links.

I am also thinking that we could merge this with the model velocity_control system. Usage would be very similar: A model with velocity control system has a cmd_vel topic for the model. If the system has <link_name> params, it then adds new cmd_vel topics for those links. What do you think?

[iche033]

I made some fixes in 546658b. The main ones are:

• fix vel frame
• add mutex to protect vel cmd msgs in velocity control system
• fix finding link to set vel cmd to based on topic name

This is ready for review

[codecov]

Codecov Report

Merging #427 (22d963e) into ign-gazebo3 (33b37c5) will increase coverage by 0.04%.
The diff coverage is 79.28%.

❗ Current head 22d963e differs from pull request most recent head abdcfb2. Consider uploading reports for the commit abdcfb2 to get more accurate results

@@               Coverage Diff               @@
##           ign-gazebo3     #427      +/-   ##
===============================================
+ Coverage        77.77%   77.81%   +0.04%     
===============================================
  Files              214      214              
  Lines            11923    12050     +127     
===============================================
+ Hits              9273     9377     +104     
- Misses            2650     2673      +23     

Impacted Files	Coverage Δ
src/systems/physics/Physics.cc	70.58% <66.66%> (+0.42%)	⬆️
src/systems/velocity_control/VelocityControl.cc	88.59% <90.54%> (+5.57%)	⬆️
src/SimulationRunner.cc	92.27% <0.00%> (-1.13%)	⬇️
src/ServerConfig.cc	90.95% <0.00%> (+0.79%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 33b37c5...abdcfb2. Read the comment docs.

[chapulina]

Works for me, the demo is very trippy

Would it be possible to add some tests?