Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[TPE] Support setting individual link velocity #427

Merged
merged 43 commits into from
Apr 24, 2021
Merged

[TPE] Support setting individual link velocity #427

Show file tree
Hide file tree
Changes from 19 commits
Commits
Show all changes
43 commits
Select commit Hold shift + click to select a range
4325f22
set link vel
claireyywang Oct 7, 2020
a4f43db
add sdf
claireyywang Oct 14, 2020
eba1aae
add link controller demo
claireyywang Oct 21, 2020
dcde2ee
add link position controller for link positions demo and testing with…
claireyywang Oct 22, 2020
b45a27c
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Oct 22, 2020
ddd0092
change name to reflect velocity
claireyywang Oct 22, 2020
e65e92f
remove two_links
claireyywang Oct 22, 2020
9fa7e78
add link vel control system
claireyywang Oct 23, 2020
6f61b2e
add vector3d msg, need to resolve link name parsing issue
claireyywang Oct 24, 2020
c73a5f8
correct link name in sdf
claireyywang Oct 24, 2020
30ebfb1
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 10, 2020
d73fb28
set one link vel
claireyywang Dec 17, 2020
df88d1a
remove profiler
claireyywang Dec 17, 2020
096bab8
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 17, 2020
8d58761
remove std::cout
claireyywang Dec 18, 2020
b91a4b5
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 18, 2020
ffa48d3
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 18, 2020
7519b1c
only move lower arm
claireyywang Dec 18, 2020
15cb0d5
Merge branch 'claire/link-velocity' of github.com:ignitionrobotics/ig…
claireyywang Dec 18, 2020
e4b20da
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 22, 2020
628e279
add camera pose
claireyywang Dec 22, 2020
9651d97
add link velocity to velocity control
claireyywang Dec 23, 2020
f2f7847
add upper link topic
claireyywang Dec 23, 2020
672db40
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Dec 23, 2020
a1efb39
Merge branch 'ign-gazebo3' into claire/link-velocity
iche033 Jan 4, 2021
56d258c
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Jan 5, 2021
fb14920
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Jan 28, 2021
dec41a8
accept dup link_name elems
claireyywang Jan 28, 2021
83bf607
clean code
claireyywang Jan 28, 2021
a39f99a
correct type
claireyywang Jan 28, 2021
8995447
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Feb 10, 2021
ff17c31
Merge branch 'ign-gazebo3' into claire/link-velocity
claireyywang Feb 17, 2021
546658b
fix frame, comments
iche033 Feb 19, 2021
7c2e603
tidyup
iche033 Feb 19, 2021
7f43028
Merge branch 'ign-gazebo3' into claire/link-velocity
iche033 Feb 22, 2021
e3cd459
indentation, comments
iche033 Feb 22, 2021
7c17416
fix world files
iche033 Feb 22, 2021
468e2f6
Merge branch 'ign-gazebo3' into claire/link-velocity
iche033 Feb 25, 2021
2a8a5f6
merge from ign-gazebo3
iche033 Apr 1, 2021
c018610
update example world, codecheck
iche033 Apr 1, 2021
d4bfca6
Merge branch 'ign-gazebo3' into claire/link-velocity
chapulina Apr 15, 2021
b9e6779
add test, clear vel cmds
iche033 Apr 15, 2021
abdcfb2
codecheck
iche033 Apr 15, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
346 changes: 346 additions & 0 deletions examples/worlds/link_velocity_controller.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,346 @@
<!--
Ignition Gazebo link velocity control demo

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

-->

<?xml version="1.0" ?>
<sdf version="1.7">
<world name="double_pendulum">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="libignition-gazebo-physics-system.so"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="libignition-gazebo-user-commands-system.so"
name="ignition::gazebo::systems::UserCommands">
</plugin>
<plugin
filename="libignition-gazebo-scene-broadcaster-system.so"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<gui fullscreen="0">

<!-- 3D scene -->
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>

<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
claireyywang marked this conversation as resolved.
Show resolved Hide resolved
</plugin>

<!-- World control -->
<plugin filename="WorldControl" name="World control">
<ignition-gui>
<title>World control</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">72</property>
<property type="double" key="width">121</property>
<property type="double" key="z">1</property>

<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="left" target="left"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>

<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>
<service>/world/link_velocity_controller/control</service>
<stats_topic>/world/link_velocity_controller/stats</stats_topic>
</plugin>

<!-- World statistics -->
<plugin filename="WorldStats" name="World stats">
<ignition-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>

<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</ignition-gui>

<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
<topic>/world/link_velocity_controller/stats</topic>

</plugin>

<!-- Entity tree -->
<plugin filename="EntityTree" name="Entity tree">
</plugin>

</gui>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="double_pendulum_with_base">
<link name="base">
<inertial>
<mass>100</mass>
</inertial>
<visual name="vis_plate_on_ground">
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<visual name="vis_pole">
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name="col_plate_on_ground">
<pose>0 0 0.01 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.8</radius>
<length>0.02</length>
</cylinder>
</geometry>
</collision>
<collision name="col_pole">
<pose>-0.275 0 1.1 0 0 0</pose>
<geometry>
<box>
<size>0.2 0.2 2.2</size>
</box>
</geometry>
</collision>
</link>
<!-- upper link, length 1, IC -90 degrees -->
<link name="upper_link">
<pose>0 0 2.1 -1.5708 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
</inertial>
<visual name="vis_upper_joint">
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<visual name="vis_lower_joint">
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<visual name="vis_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name="col_upper_joint">
<pose>-0.05 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name="col_lower_joint">
<pose>0 0 1.0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</collision>
<collision name="col_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- lower link, length 1, IC ~-120 degrees more -->
<link name="lower_link">
<pose>0.25 1.0 2.1 -2 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
</inertial>
<visual name="vis_lower_joint">
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<visual name="vis_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name="col_lower_joint">
<pose>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.3</length>
</cylinder>
</geometry>
</collision>
<collision name="col_cylinder">
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.9</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- pin joint for upper link, at origin of upper link -->
<joint name="upper_joint" type="revolute">
<parent>base</parent>
<child>upper_link</child>
<axis>
<xyz>1.0 0 0</xyz>
</axis>
</joint>
<!-- pin joint for lower link, at origin of child link -->
<joint name="lower_joint" type="revolute">
<parent>upper_link</parent>
<child>lower_link</child>
<axis>
<xyz>1.0 0 0</xyz>
</axis>
</joint>

<plugin
filename="ignition-gazebo-link-velocity-controller-system"
name="ignition::gazebo::systems::LinkVelocityController">
<link1_name>upper_link</link1_name>
<link2_name>lower_link</link2_name>
claireyywang marked this conversation as resolved.
Show resolved Hide resolved
</plugin>
</model>
</world>
</sdf>
1 change: 1 addition & 0 deletions src/systems/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,7 @@ add_subdirectory(joint_controller)
add_subdirectory(joint_position_controller)
add_subdirectory(joint_state_publisher)
add_subdirectory(lift_drag)
add_subdirectory(link_velocity_controller)
add_subdirectory(log)
add_subdirectory(log_video_recorder)
add_subdirectory(logical_audio_sensor_plugin)
Expand Down
7 changes: 7 additions & 0 deletions src/systems/link_velocity_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
gz_add_system(link-velocity-controller
SOURCES
LinkVelocityController.cc
PUBLIC_LINK_LIBS
ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER}
)
Loading