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

Add coupled_pendulum tutorial #228

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
+360 −0
Draft

Add coupled_pendulum tutorial #228

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ project(gz-sim-yarp-plugins

find_package(YARP REQUIRED COMPONENTS robotinterface os)
find_package(YCM REQUIRED)
find_package(ICUB QUIET)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why we added this?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My bad, I said to @martinaxgloria to add find_package but actually since it is a run-time and not compile-time dependency this is not needed


# Initial value of GZ_SIM_YARP_PLUGINS_USED_GZ_SIM_VERSION is empty, and then is set to 8 or 9 depending if gz-sim8 or gz-sim9 are available
set(GZ_SIM_YARP_PLUGINS_USED_GZ_SIM_VERSION_DOCS "Version of gz-sim used to compile gz-sim-yarp-plugins (either 8 or 9)")
Expand Down
38 changes: 38 additions & 0 deletions tutorial/coupled_pendulum/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
# Run coupled pendulum tutorial in Gazebo

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Somewhere here we should document that this model has a runtime dependency on icub-main.

## Run model in Gazebo with YARP integration

- 1st terminal:

~~~bash
yarp server
~~~

- 2nd terminal:
- Update the `GZ_SIM_RESOURCE_PATH` environment variable to point to the `tutorial` folder:

~~~
export GZ_SIM_RESOURCE_PATH = $GZ_SIM_RESOURCE_PATH:<path-to-tutorial-folder>
~~~

- Then, launch Gazebo:

~~~
cd <path-to-tutorial-folder>/coupled_pendulum
gz sim coupled_pendulum_world.sdf
~~~

The Gazebo GUI will open and the coupled pendulum should be already spawned in the scene.

- 3rd terminal:

~~~bash
cd tutorial/coupled_pendulum
yarpmotorgui
~~~

The [yarpmotorgui](https://www.yarp.it/latest/group__yarpmotorgui.html) interface will open and it should automatically prompt you to select the `/coupledPendulumGazebo/body` part.

Click Ok.

Finally start the simulation in Gazebo (click on the Play button on the bottom left).
17 changes: 17 additions & 0 deletions tutorial/coupled_pendulum/conf/coupled_pendulum_nws.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<robot name="coupled_pendulum" portprefix="coupled_pendulum" build="0" xmlns:xi="http://www.w3.org/2001/XInclude">
<devices>
<device name="coupled_pendulum_nws_yarp" type="controlBoard_nws_yarp">
<!-- See https://www.yarp.it/latest/classControlBoard__nws__yarp.html for parameter documentation -->
<param name="name"> /coupledPendulumGazebo/body </param>
<param name="period"> 0.01 </param>
<action phase="startup" level="5" type="attach">
<!-- This is the same name that we passed with the yarpDeviceName to the gazebo_controlboard plugin -->
<param name="device"> controlboard_plugin_device </param>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
</devices>
</robot>
52 changes: 52 additions & 0 deletions tutorial/coupled_pendulum/conf/gazebo_controlboard.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
disableImplicitNetworkWrapper
yarpDeviceName controlboard_plugin_device
jointNames (fixed_base upper_joint lower_joint)

[COUPLING]
device couplingICubEye
actuatedAxesNames (fixed_base upper_joint lower_joint)
actuatedAxesPosMin (0.0 -200.0 -200.0)
actuatedAxesPosMax (0.0 200.0 200.0)

#PIDs:
[POSITION_CONTROL]
controlUnits metric_units
controlLaw joint_pid_gazebo_v1
kp (0.0 3000.0 3000.0)
kd (0.0 2.0 2.0)
ki (0.0 0.1 0.1)
maxInt (0.0 9999 9999)
maxOutput (0.0 9999 9999)
shift (0.0 0.0 0.0)
ko (0.0 0.0 0.0)
stictionUp (0.0 0.0 0.0)
stictionDwn (0.0 0.0 0.0)

[VELOCITY_CONTROL]
controlUnits metric_units
controlLaw joint_pid_gazebo_v1
velocityControlImplementationType integrator_and_position_pid
kp (0.0 500.0 500.0)
kd (0.0 2.0 2.0)
ki (0.0 0.1 0.1)
maxInt (0.0 9999 9999)
maxOutput (0.0 9999 9999)
shift (0.0 0.0 0.0)
ko (0.0 0.0 0.0)
stictionUp (0.0 0.0 0.0)
stictionDwn (0.0 0.0 0.0)

[LIMITS]
jntPosMax (0.0 200.0 200.0)
jntPosMin (0.0 -200.0 -200.0)
jntVelMax (0.0 100.0 100.0)

[TRAJECTORY_GENERATION]
# Uncomment one of the following lines to select the trajectory generation method
#trajectory_type constant_speed
#trajectory_type trapezoidal_speed
trajectory_type minimum_jerk

# Uncomment the following lines to override the default trajectory generation parameters
#refSpeed 10.0
#refAcceleration 10.0
72 changes: 72 additions & 0 deletions tutorial/coupled_pendulum/coupled_pendulum_world.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
<?xml version="1.0"?>

<sdf version="1.7">
<world name="tutorial_controlboard">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<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>
<size>100 100</size>
</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>

<include>
<uri>model://coupled_pendulum/model.sdf</uri>
</include>

</world>
</sdf>
174 changes: 174 additions & 0 deletions tutorial/coupled_pendulum/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,174 @@
<?xml version="1.0"?>

<sdf version="1.7">

<model name="coupled_pendulum">
<!-- <pose>0 0 0.2 0 0 0</pose> -->
<joint name="fixed_base" type="revolute">
<parent>world</parent>
<child>base_link</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-5</lower>
<upper>5</upper>
<effort>100</effort>
<velocity>100</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<spring_reference>0</spring_reference>
<spring_stiffness>0.0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='base_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>100</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<collision name='base_link_fixed_joint_lump__base_collision'>
<pose>0 0 1 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 2.15</size>
</box>
</geometry>
</collision>
<visual name='base_link_fixed_joint_lump__base_visual'>
<pose>0 0 1 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 2.15</size>
</box>
</geometry>
</visual>
</link>
<joint name='upper_joint' type='revolute'>
<pose relative_to='base_link'>0.15 0 2 -1.5708 0 0</pose>
<parent>base_link</parent>
<child>upper_link</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-5</lower>
<upper>5</upper>
<effort>100</effort>
<velocity>100</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<spring_reference>0</spring_reference>
<spring_stiffness>0.0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='upper_link'>
<pose relative_to='upper_joint'>0 0 0 0 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0.5 0 0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
<collision name='upper_link_collision'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 1.0</size>
</box>
</geometry>
</collision>
<visual name='upper_link_visual'>
<pose>0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 1.0</size>
</box>
</geometry>
</visual>
</link>

<joint name='lower_joint' type='revolute'>
<pose relative_to='base_link'>-0.15 0 2 -1.5708 0 0</pose>
<parent>base_link</parent>
<child>lower_link</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-5</lower>
<upper>5</upper>
<effort>100</effort>
<velocity>100</velocity>
</limit>
<dynamics>
<damping>0.0</damping>
<spring_reference>0</spring_reference>
<spring_stiffness>0.0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='lower_link'>
<pose relative_to='lower_joint'>0 0 0 0 0 0</pose>
<self_collide>0</self_collide>
<inertial>
<pose>0.0 0 0.5 0 0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
<collision name='lower_link_collision'>
<pose>0.0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 1.0</size>
</box>
</geometry>
</collision>
<visual name='lower_link_visual'>
<pose>0.0 0 0.5 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.15 1.0</size>
</box>
</geometry>
</visual>
</link>

<!-- Launch the other YARP devices, in this case a controlBoard_nws_yarp to expose the
controlboard functionalities via YARP ports -->
<plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
<yarpConfigurationFile>
model://coupled_pendulum/conf/gazebo_controlboard.ini
</yarpConfigurationFile>
<initialConfiguration>0.0</initialConfiguration>
</plugin>
<plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
<yarpRobotInterfaceConfigurationFile>
model://coupled_pendulum/conf/coupled_pendulum_nws.xml
</yarpRobotInterfaceConfigurationFile>
</plugin>
</model>

</sdf>
6 changes: 6 additions & 0 deletions tutorial/coupled_pendulum/yarpmotorgui.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
robot coupledPendulumGazebo
parts (body)



//DO NOT REMOVE THIS LINE