[TPE] Support setting individual link velocity #427

claireyywang · 2020-10-22T07:06:17Z

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}"

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

… tpe Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

…n-gazebo into claire/link-velocity

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?

examples/worlds/link_velocity_controller.sdf

src/systems/link_velocity_controller/LinkVelocityController.cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 · 2021-02-19T23:10:42Z

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

examples/worlds/pendulum_links.sdf

src/systems/velocity_control/VelocityControl.cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

src/systems/velocity_control/VelocityControl.cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

codecov · 2021-04-02T01:20:46Z

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?

src/systems/physics/Physics.cc

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* 🎈 3.8.0 (#688) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Make it so joint state publisher is quieter (#696) Signed-off-by: Michael Carroll <michael@openrobotics.org> * [BULLET] Making GetContactsFromLastStepFeature optional in Collision Features (#690) * GetContactsFromLastStepFeature made optional Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org> * Add test for thermal object temperatures below 0 kelvin (#621) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * Scenebroadcaster sensors (#698) * Add sensors to scene broadcaster Signed-off-by: Nate Koenig <nate@openrobotics.org> * Update src/systems/scene_broadcaster/SceneBroadcaster.cc Co-authored-by: Michael Carroll <michael@openrobotics.org> * Fix codecheck Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Michael Carroll <michael@openrobotics.org> * Fix diffuse and ambient values for ackermann example (#707) Signed-off-by: Ammaar Solkar <asketch8@gmail.com> * 🎈 5.0.0 (#731) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Support configuring particle scatter ratio in particle emitter system (#674) * set particle scatter ratio through sdf Signed-off-by: Ian Chen <ichen@osrfoundation.org> * address feedback Signed-off-by: Ian Chen <ichen@osrfoundation.org> * add todo note about merging forward Signed-off-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> * Update PlaybackScrubber description (#733) Signed-off-by: Ammaar Solkar <asketch8@gmail.com> * Iterate through changed links only in UpdateSim (#678) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * Do not pass -Wno-unused-parameter to MSVC compiler (#716) Signed-off-by: Silvio Traversaro <silvio.traversaro@iit.it> * Use Protobuf_IMPORT_DIRS instead of PROTOBUF_IMPORT_DIRS for compatibility with Protobuf CMake config (#715) Signed-off-by: Silvio Traversaro <silvio.traversaro@iit.it> * Fix component inspector shutdown crash (#724) Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Validate step size and RTF parameters (#740) Only set them if they are strictly positive. Signed-off-by: Luca Della Vedova <luca@openrobotics.org> * Fix compute_rtfs arguments (#737) Signed-off-by: Caio Amaral <caioaamaral@gmail.com> * Fixed collision visual bounding boxes (#746) Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * Fix CMakelists.txt merge Signed-off-by: Nate Koenig <nate@openrobotics.org> * ECM's ChangedState gets message with modified components (#742) * ecm's ChangedState to contain modified components Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * updated log_system test Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * removed unnecessary calls Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> Co-authored-by: Ian Chen <ichen@osrfoundation.org> * fixed particle emitter forward playback (#745) Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * Merge pull request #730 from ignitionrobotics/particle_emitter Particle emitter based on SDF * 4 7 0 prep (#755) * Prepare for 4.7.0 Signed-off-by: Nate Koenig <nate@openrobotics.org> * Added placeholder Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> * Fix 'invalid animation update data' msg for actors (#754) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * Update benchmark comparison instructions (#766) (#766) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * [DiffDrive] add enable/disable (#772) * add enable/disable diffdrive Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * remove debug Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * do not subscribe to enable if topic is empty Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * add test Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * lint and style Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * change enable type to bool and renamed to enabled Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr> * Add odometry publisher system (#547) * Create Initial Odometry Publisher system plugin Add code for initial plugin that gets position from Pose component and calculates velocities based on rolling mean from displacement data. Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> * Remove Linear and Angular Velocity components Also renames frames in Odometry msg to include model name, and makes various style changes. Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> * Get World pose instead of pose of robot base frame Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> * Add documentation for variables and functions Includes minor stylistic changes. Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> * Check for valid odomTopic and update copyright year Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> * Add tests for OdometryPublisherSystem and fix velocity calculation bug Swap X and Y linear velocities when calculating odometry velocities relative to robotBaseFrame. Signed-off-by: Maganty Rushyendra <mrushyendra@yahoo.com.sg> Co-authored-by: ahcorde <ahcorde@gmail.com> * Patch particle emitter2 service (#777) * Patch particle emitter2 service Signed-off-by: Nate Koenig <nate@openrobotics.org> * Remove condition variable Signed-off-by: Nate Koenig <nate@openrobotics.org> * Set emitter frame and relative pose Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> * Preparing for 4.8.0 release (#780) Signed-off-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org> * 👩‍🌾 Enable Focal CI (#646) Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Michael Carroll <michael@openrobotics.org> * [TPE] Support setting individual link velocity (#427) Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> Signed-off-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Louise Poubel <louise@openrobotics.org> * Don't store duplicate ComponentTypeId in ECM (#751) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Feature/hydrodynamics (#749) Implement hydrodynamics and thruster plugin. Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org> Co-authored-by: Mabel Zhang <mabel@openrobotics.org> Co-authored-by: Carlos Agüero <caguero@openrobotics.org> * Fix macOS build: components::Name in benchmark (#784) Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Steve Peters <scpeters@openrobotics.org> * Fix ColladaExporter submesh index bug (#763) Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com> * 👩‍🌾 Fix Windows build and some warnings (#782) Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Prevent crash on Plotting plugin with mutex (#747) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Bump ign-physics version to 3.2 (#792) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Bump to ign-msgs 7.1 / sdformat 11.1, Windows fixes (#758) Signed-off-by: Louise Poubel <louise@openrobotics.org> * Util: Use public API from libsdformat for detecting non-file source (#794) Signed-off-by: Eric Cousineau <eric.cousineau@tri.global> * Fix included nested model expansion in SDF generation (#768) * fixed included nested model expansion Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * added resource path to test Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * use orig URIs & support multi level nesting Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * save fuel version when enabled Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * retrieve uri from map Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * copy included element Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * clear attributes before copying include element Signed-off-by: Jenn Nguyen <jenn@openrobotics.org> * Map canonical links to their models (#736) Signed-off-by: Ashton Larkin <ashton@openrobotics.org> * ColladaExporter, export submesh selected (#802) * Export only submesh if selected * Add test case for the PR * Attempting a unified solution Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com> Co-authored-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Michael Carroll <michael@openrobotics.org> Co-authored-by: Jose Tomas Lorente <jtlorente@ekumenlabs.com> Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org> Co-authored-by: Ashton Larkin <ashton@openrobotics.org> Co-authored-by: Nate Koenig <nkoenig@users.noreply.github.com> Co-authored-by: Nate Koenig <nate@openrobotics.org> Co-authored-by: Ammaar Solkar <asketch8@gmail.com> Co-authored-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: Silvio Traversaro <pegua1@gmail.com> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Co-authored-by: Luca Della Vedova <luca@openrobotics.org> Co-authored-by: Caio Amaral <caioaamaral@gmail.com> Co-authored-by: Jenn Nguyen <jenn@openrobotics.org> Co-authored-by: G.Doisy <doisyg@users.noreply.github.com> Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Co-authored-by: Claire Wang <22240514+claireyywang@users.noreply.github.com> Co-authored-by: Arjo Chakravarty <arjo129@gmail.com> Co-authored-by: Mabel Zhang <mabel@openrobotics.org> Co-authored-by: Carlos Agüero <caguero@openrobotics.org> Co-authored-by: Steve Peters <scpeters@openrobotics.org> Co-authored-by: Jorge Perez <j.j.perez13@hotmail.com> Co-authored-by: Eric Cousineau <eric.cousineau@tri.global> Co-authored-by: Jorge Perez <jjperez@ekumenlabs.com>

claireyywang added 4 commits October 6, 2020 23:30

set link vel

4325f22

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

add sdf

a4f43db

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

add link controller demo

eba1aae

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

add link position controller for link positions demo and testing with…

dcde2ee

… tpe Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

claireyywang added the 🏰 citadel Ignition Citadel label Oct 22, 2020

claireyywang self-assigned this Oct 22, 2020

claireyywang added 10 commits October 22, 2020 00:06

Merge branch 'ign-gazebo3' into claire/link-velocity

b45a27c

change name to reflect velocity

ddd0092

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

remove two_links

e65e92f

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

add link vel control system

9fa7e78

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

add vector3d msg, need to resolve link name parsing issue

6f61b2e

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

correct link name in sdf

c73a5f8

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

Merge branch 'ign-gazebo3' into claire/link-velocity

30ebfb1

set one link vel

d73fb28

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

remove profiler

df88d1a

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

Merge branch 'ign-gazebo3' into claire/link-velocity

096bab8

claireyywang mentioned this pull request Dec 17, 2020

[TPE] Update link pose gazebosim/gz-physics#179

Merged

claireyywang added 2 commits December 17, 2020 18:16

remove std::cout

8d58761

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

Merge branch 'ign-gazebo3' into claire/link-velocity

b91a4b5

claireyywang marked this pull request as ready for review December 18, 2020 06:07

claireyywang requested review from azeey and chapulina as code owners December 18, 2020 06:07

Merge branch 'ign-gazebo3' into claire/link-velocity

ffa48d3

claireyywang added 2 commits December 18, 2020 14:52

only move lower arm

7519b1c

Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com>

Merge branch 'claire/link-velocity' of github.com:ignitionrobotics/ig…

15cb0d5

…n-gazebo into claire/link-velocity

claireyywang requested a review from luca-della-vedova December 18, 2020 22:54

iche033 requested changes Dec 18, 2020

View reviewed changes

chapulina assigned iche033 Feb 19, 2021

iche033 added 2 commits February 19, 2021 15:02

fix frame, comments

546658b

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

tidyup

7c2e603

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 requested a review from chapulina February 19, 2021 23:10

ahcorde requested changes Feb 22, 2021

View reviewed changes

iche033 added 3 commits February 22, 2021 10:43

Merge branch 'ign-gazebo3' into claire/link-velocity

7f43028

indentation, comments

e3cd459

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

fix world files

7c17416

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

iche033 requested a review from ahcorde February 22, 2021 20:42

ahcorde requested changes Feb 23, 2021

View reviewed changes

src/systems/velocity_control/VelocityControl.cc Show resolved Hide resolved

src/systems/velocity_control/VelocityControl.cc Show resolved Hide resolved

src/systems/velocity_control/VelocityControl.cc Show resolved Hide resolved

Merge branch 'ign-gazebo3' into claire/link-velocity

468e2f6

iche033 requested a review from ahcorde February 25, 2021 21:47

chapulina added the needs upstream release Blocked by a release of an upstream library label Mar 9, 2021

iche033 added 2 commits April 1, 2021 15:18

merge from ign-gazebo3

2a8a5f6

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

update example world, codecheck

c018610

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

chapulina reviewed Apr 5, 2021

View reviewed changes

src/systems/physics/Physics.cc Show resolved Hide resolved

chapulina mentioned this pull request Apr 14, 2021

🎈 2.4.0 gazebosim/gz-physics#247

Merged

7 tasks

chapulina and others added 3 commits April 15, 2021 12:09

Merge branch 'ign-gazebo3' into claire/link-velocity

d4bfca6

add test, clear vel cmds

b9e6779

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

codecheck

abdcfb2

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

chapulina removed the needs upstream release Blocked by a release of an upstream library label Apr 15, 2021

chapulina approved these changes Apr 15, 2021

View reviewed changes

chapulina merged commit a5f80cc into ign-gazebo3 Apr 24, 2021

chapulina deleted the claire/link-velocity branch April 24, 2021 00:01

chapulina mentioned this pull request Apr 29, 2021

3 ➡️ 4 #792

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[TPE] Support setting individual link velocity #427

[TPE] Support setting individual link velocity #427

claireyywang commented Oct 22, 2020 •

edited by iche033

Loading

iche033 left a comment

iche033 commented Feb 19, 2021

codecov bot commented Apr 2, 2021 •

edited

Loading

chapulina left a comment

[TPE] Support setting individual link velocity #427

[TPE] Support setting individual link velocity #427

Conversation

claireyywang commented Oct 22, 2020 • edited by iche033 Loading

iche033 left a comment

Choose a reason for hiding this comment

iche033 commented Feb 19, 2021

codecov bot commented Apr 2, 2021 • edited Loading

Codecov Report

chapulina left a comment

Choose a reason for hiding this comment

claireyywang commented Oct 22, 2020 •

edited by iche033

Loading

codecov bot commented Apr 2, 2021 •

edited

Loading