|
| 1 | +<?xml version="1.0" ?> |
| 2 | +<!-- |
| 3 | + Ignition Gazebo joint position controller plugin demo |
| 4 | +
|
| 5 | + Try sending joint position commands: |
| 6 | +
|
| 7 | + ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: -1.0" |
| 8 | +
|
| 9 | + ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: 1.0" |
| 10 | +--> |
| 11 | +<sdf version="1.6"> |
| 12 | + <world name="default"> |
| 13 | + <plugin |
| 14 | + filename="libignition-gazebo-physics-system.so" |
| 15 | + name="ignition::gazebo::systems::Physics"> |
| 16 | + </plugin> |
| 17 | + <plugin |
| 18 | + filename="libignition-gazebo-scene-broadcaster-system.so" |
| 19 | + name="ignition::gazebo::systems::SceneBroadcaster"> |
| 20 | + </plugin> |
| 21 | + |
| 22 | + <gui fullscreen="0"> |
| 23 | + |
| 24 | + <!-- 3D scene --> |
| 25 | + <plugin filename="GzScene3D" name="3D View"> |
| 26 | + <ignition-gui> |
| 27 | + <title>3D View</title> |
| 28 | + <property type="bool" key="showTitleBar">false</property> |
| 29 | + <property type="bool" key="resizable">false</property> |
| 30 | + <property type="double" key="z">0</property> |
| 31 | + <anchor line="right" target="window" target_line="right"/> |
| 32 | + <anchor line="left" target="window" target_line="left"/> |
| 33 | + <anchor line="top" target="window" target_line="top"/> |
| 34 | + <anchor line="bottom" target="window" target_line="bottom"/> |
| 35 | + </ignition-gui> |
| 36 | + |
| 37 | + <engine>ogre</engine> |
| 38 | + <scene>scene</scene> |
| 39 | + <ambient_light>0.4 0.4 0.4</ambient_light> |
| 40 | + <background_color>0.8 0.8 0.8</background_color> |
| 41 | + <camera_pose>1 0 0.1 0 0.05 3.14</camera_pose> |
| 42 | + </plugin> |
| 43 | + |
| 44 | + <!-- World control --> |
| 45 | + <plugin filename="WorldControl" name="World control"> |
| 46 | + <ignition-gui> |
| 47 | + <title>World control</title> |
| 48 | + <property type="bool" key="showTitleBar">false</property> |
| 49 | + <property type="bool" key="resizable">false</property> |
| 50 | + <property type="double" key="height">72</property> |
| 51 | + <property type="double" key="width">121</property> |
| 52 | + <property type="double" key="z">1</property> |
| 53 | + <anchor line="left" target="window" target_line="left"/> |
| 54 | + <anchor line="bottom" target="window" target_line="bottom"/> |
| 55 | + </ignition-gui> |
| 56 | + |
| 57 | + <play_pause>true</play_pause> |
| 58 | + <step>true</step> |
| 59 | + <start_paused>true</start_paused> |
| 60 | + <service>/world/default/control</service> |
| 61 | + <stats_topic>/world/default/stats</stats_topic> |
| 62 | + |
| 63 | + </plugin> |
| 64 | + </gui> |
| 65 | + <light type="directional" name="sun"> |
| 66 | + <cast_shadows>true</cast_shadows> |
| 67 | + <pose>0 0 10 0 0 0</pose> |
| 68 | + <diffuse>1 1 1 1</diffuse> |
| 69 | + <specular>0.5 0.5 0.5 1</specular> |
| 70 | + <attenuation> |
| 71 | + <range>1000</range> |
| 72 | + <constant>0.9</constant> |
| 73 | + <linear>0.01</linear> |
| 74 | + <quadratic>0.001</quadratic> |
| 75 | + </attenuation> |
| 76 | + <direction>-0.5 0.1 -0.9</direction> |
| 77 | + </light> |
| 78 | + <model name="joint_position_controller_demo"> |
| 79 | + <pose>0 0 0 0 1.57 0</pose> |
| 80 | + <link name="base_link"> |
| 81 | + <pose>0.0 0.0 0.0 0 0 0</pose> |
| 82 | + <inertial> |
| 83 | + <inertia> |
| 84 | + <ixx>2.501</ixx> |
| 85 | + <ixy>0</ixy> |
| 86 | + <ixz>0</ixz> |
| 87 | + <iyy>2.501</iyy> |
| 88 | + <iyz>0</iyz> |
| 89 | + <izz>5</izz> |
| 90 | + </inertia> |
| 91 | + <mass>120.0</mass> |
| 92 | + </inertial> |
| 93 | + <visual name="base_visual"> |
| 94 | + <pose>0.0 0.0 0.0 0 0 0</pose> |
| 95 | + <geometry> |
| 96 | + <box> |
| 97 | + <size>0.5 0.5 0.01</size> |
| 98 | + </box> |
| 99 | + </geometry> |
| 100 | + </visual> |
| 101 | + <collision name="base_collision"> |
| 102 | + <pose>0.0 0.0 0.0 0 0 0</pose> |
| 103 | + <geometry> |
| 104 | + <box> |
| 105 | + <size>0.5 0.5 0.01</size> |
| 106 | + </box> |
| 107 | + </geometry> |
| 108 | + </collision> |
| 109 | + </link> |
| 110 | + <link name="rotor"> |
| 111 | + <pose>0.0 0.0 0.1 0 0 0</pose> |
| 112 | + <inertial> |
| 113 | + <inertia> |
| 114 | + <ixx>0.032</ixx> |
| 115 | + <ixy>0</ixy> |
| 116 | + <ixz>0</ixz> |
| 117 | + <iyy>0.032</iyy> |
| 118 | + <iyz>0</iyz> |
| 119 | + <izz>0.00012</izz> |
| 120 | + </inertia> |
| 121 | + <mass>0.6</mass> |
| 122 | + </inertial> |
| 123 | + <visual name="visual"> |
| 124 | + <geometry> |
| 125 | + <box> |
| 126 | + <size>0.25 0.1 0.05</size> |
| 127 | + </box> |
| 128 | + </geometry> |
| 129 | + <material> |
| 130 | + <ambient>0.2 0.8 0.2 1</ambient> |
| 131 | + </material> |
| 132 | + </visual> |
| 133 | + <collision name="collision"> |
| 134 | + <geometry> |
| 135 | + <box> |
| 136 | + <size>0.25 0.1 0.05</size> |
| 137 | + </box> |
| 138 | + </geometry> |
| 139 | + </collision> |
| 140 | + </link> |
| 141 | + |
| 142 | + <joint name="world_fixed" type="fixed"> |
| 143 | + <parent>world</parent> |
| 144 | + <child>base_link</child> |
| 145 | + </joint> |
| 146 | + |
| 147 | + <joint name="j1" type="revolute"> |
| 148 | + <pose>0 0 -0.5 0 0 0</pose> |
| 149 | + <parent>base_link</parent> |
| 150 | + <child>rotor</child> |
| 151 | + <axis> |
| 152 | + <xyz>0 0 1</xyz> |
| 153 | + <dynamics> |
| 154 | + <damping>0.5</damping> |
| 155 | + </dynamics> |
| 156 | + </axis> |
| 157 | + </joint> |
| 158 | + <plugin |
| 159 | + filename="libignition-gazebo-joint-position-controller-system.so" |
| 160 | + name="ignition::gazebo::systems::JointPositionController"> |
| 161 | + <joint_name>j1</joint_name> |
| 162 | + <p_gain>1</p_gain> |
| 163 | + <i_gain>0.1</i_gain> |
| 164 | + <d_gain>0.01</d_gain> |
| 165 | + <i_max>1</i_max> |
| 166 | + <i_min>-1</i_min> |
| 167 | + <cmd_max>1000</cmd_max> |
| 168 | + <cmd_min>-1000</cmd_min> |
| 169 | + </plugin> |
| 170 | + </model> |
| 171 | + </world> |
| 172 | +</sdf> |
0 commit comments