<?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>
