164 lines
4.7 KiB
XML
Executable File
164 lines
4.7 KiB
XML
Executable File
<?xml version="1.0" ?>
|
|
<sdf version="1.9">
|
|
<world name="test_gimbal">
|
|
<physics name="1ms" type="ignore">
|
|
<max_step_size>0.001</max_step_size>
|
|
<real_time_factor>1.0</real_time_factor>
|
|
</physics>
|
|
|
|
<plugin name="gz::sim::systems::Physics"
|
|
filename="gz-sim-physics-system">
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::Sensors"
|
|
filename="gz-sim-sensors-system">
|
|
<render_engine>ogre2</render_engine>
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::UserCommands"
|
|
filename="gz-sim-user-commands-system">
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::SceneBroadcaster"
|
|
filename="gz-sim-scene-broadcaster-system">
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::Imu"
|
|
filename="gz-sim-imu-system">
|
|
</plugin>
|
|
|
|
<scene>
|
|
<ambient>1.0 1.0 1.0</ambient>
|
|
<background>0.8 0.8 0.8</background>
|
|
<sky></sky>
|
|
</scene>
|
|
|
|
<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.8 0.8 0.8 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>
|
|
|
|
<include>
|
|
<uri>model://runway</uri>
|
|
<pose degrees="true">-29 545 0 0 0 363</pose>
|
|
</include>
|
|
|
|
<!-- gimbal -->
|
|
<!-- <include>
|
|
<uri>model://gimbal_small_2d</uri>
|
|
<name>gimbal</name>
|
|
<pose degrees="true">0 0 0.11 -90 0 90</pose>
|
|
<plugin name="gz::sim::systems::JointPositionController"
|
|
filename="gz-sim-joint-position-controller-system">
|
|
<joint_name>roll_link</joint_name>
|
|
<topic>arm_cmd</topic>
|
|
<p_gain>2</p_gain>
|
|
<i_gain>0.1</i_gain>
|
|
<d_gain>0.01</d_gain>
|
|
<i_max>1</i_max>
|
|
<i_min>-1</i_min>
|
|
<cmd_max>1000</cmd_max>
|
|
<cmd_min>-1000</cmd_min>
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::JointPositionController"
|
|
filename="gz-sim-joint-position-controller-system">
|
|
<joint_name>tilt_joint</joint_name>
|
|
<topic>tilt_cmd</topic>
|
|
<p_gain>2</p_gain>
|
|
<i_gain>0.1</i_gain>
|
|
<d_gain>0.01</d_gain>
|
|
<i_max>1</i_max>
|
|
<i_min>-1</i_min>
|
|
<cmd_max>1000</cmd_max>
|
|
<cmd_min>-1000</cmd_min>
|
|
</plugin>
|
|
</include> -->
|
|
|
|
<!-- nested -->
|
|
<model name="gimbal_base">
|
|
<pose>2 0 0.5 0 0 0</pose>
|
|
<link name="base_link">
|
|
<inertial>
|
|
<mass>0.25</mass>
|
|
<inertia>
|
|
<ixx>0.000417</ixx>
|
|
<ixy>0.00</ixy>
|
|
<ixz>0.00</ixz>
|
|
<iyy>0.000417</iyy>
|
|
<iyz>0.00</iyz>
|
|
<izz>0.000417</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<box>
|
|
<size>1 1 1</size>
|
|
</box>
|
|
</geometry>
|
|
<material>
|
|
<ambient>1 1 0</ambient>
|
|
<diffuse>1 1 0</diffuse>
|
|
<specular>0.1 0.1 0 1</specular>
|
|
</material>
|
|
</visual>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<box>
|
|
<size>1 1 1</size>
|
|
</box>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<include>
|
|
<uri>model://gimbal_small_2d</uri>
|
|
<name>gimbal</name>
|
|
<pose degrees="true">0 0 0.61 -90 0 -90</pose>
|
|
</include>
|
|
<joint name="gimbal_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>gimbal::base_link</child>
|
|
<axis>
|
|
<dynamics>
|
|
<damping>0.5</damping>
|
|
</dynamics>
|
|
<limit>
|
|
<lower>0</lower>
|
|
<upper>0</upper>
|
|
</limit>
|
|
<xyz>0 0 1</xyz>
|
|
</axis>
|
|
</joint>
|
|
|
|
<plugin name="gz::sim::systems::JointStatePublisher"
|
|
filename="gz-sim-joint-state-publisher-system">
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::JointPositionController"
|
|
filename="gz-sim-joint-position-controller-system">
|
|
<joint_name>gimbal::roll_joint</joint_name>
|
|
<topic>/gimbal/cmd_roll</topic>
|
|
<p_gain>2</p_gain>
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::JointPositionController"
|
|
filename="gz-sim-joint-position-controller-system">
|
|
<joint_name>gimbal::tilt_joint</joint_name>
|
|
<topic>/gimbal/cmd_pitch</topic>
|
|
<p_gain>2</p_gain>
|
|
</plugin>
|
|
<plugin name="gz::sim::systems::JointPositionController"
|
|
filename="gz-sim-joint-position-controller-system">
|
|
<joint_name>gimbal::yaw_joint</joint_name>
|
|
<topic>/gimbal/cmd_yaw</topic>
|
|
<p_gain>2</p_gain>
|
|
</plugin>
|
|
|
|
</model>
|
|
|
|
</world>
|
|
</sdf>
|