<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <physics type="ode">
      <gravity>0 0 -10.0</gravity>
      <ode>
        <solver>
          <type>quick</type>
          <iters>10</iters>
          <sor>1.3</sor>
        </solver>
        <constraints>
          <cfm>0.0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
          <contact_surface_layer>0.0</contact_surface_layer>
        </constraints>
      </ode>
      <max_step_size>0.001</max_step_size>
    </physics>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <!-- 25.3kg total mass -->
    <model name="test_model">
      <pose>0 0 0.7 0 0 0</pose>
      <link name="link">
        <inertial>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="collision">
          <pose>0 0 -0.55 0 0 0</pose>
          <geometry>
            <box>
              <size>1 1 0.1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <pose>0 0 -0.55 0 0 0</pose>
          <geometry>
            <box>
              <size>1 1 0.1</size>
            </box>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
        <collision name="collision2">
          <pose>0 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.5 0.5 0.5</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual2">
          <pose>0 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.5 0.5 0.5</size>
            </box>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <link name="bar_1">
        <pose>0.45 0 -0.5 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.001</iyy>
            <iyz>0</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.02 1.0 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.02 1.0 0.02</size>
            </box>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="bar_12_joint" type="revolute">
        <parent>bar_2</parent>
        <child>bar_1</child>
        <pose>0 0.5 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <joint name="bar_13_joint" type="revolute">
        <parent>bar_3</parent>
        <child>bar_1</child>
        <pose>0 -0.5 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="bar_2">
        <pose>0.5 0.5 -0.5 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.001</iyy>
            <iyz>0</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>
        <collision name="collision">
          <pose>-0.025 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.05 0.02 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <pose>-0.025 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.05 0.02 0.02</size>
            </box>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="bar_2_joint" type="revolute">
        <parent>link</parent>
        <child>bar_2</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="bar_3">
        <pose>0.5 -0.5 -0.5 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.001</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.001</iyy>
            <iyz>0</iyz>
            <izz>0.001</izz>
          </inertia>
          <mass>0.1</mass>
        </inertial>
        <collision name="collision">
          <pose>-0.025 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.05 0.02 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <pose>-0.025 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>0.05 0.02 0.02</size>
            </box>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="bar_3_joint" type="revolute">
        <parent>link</parent>
        <child>bar_3</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="wheel_1">
        <pose>0.5 0.55 -0.5 1.57079633 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.01</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.01</iyy>
            <iyz>0</iyz>
            <izz>0.01</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <fdir1>1 0 0</fdir1>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="wheel_1_joint" type="revolute">
        <parent>bar_2</parent>
        <child>wheel_1</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="wheel_2">
        <pose>0.5 -0.55 -0.5 1.57079633 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.01</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.01</iyy>
            <iyz>0</iyz>
            <izz>0.01</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <fdir1>1 0 0</fdir1>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="wheel_2_joint" type="revolute">
        <parent>bar_3</parent>
        <child>wheel_2</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 1 0</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="wheel_3">
        <pose>-0.5 0.55 -0.5 1.57079633 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.01</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.01</iyy>
            <iyz>0</iyz>
            <izz>0.01</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <fdir1>1 0 0</fdir1>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="wheel_3_joint" type="revolute">
        <parent>link</parent>
        <child>wheel_3</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <dynamics>
            <damping>0.0</damping>
            <friction>0.0</friction>
          </dynamics>
          <xyz>0 1 0</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="wheel_4">
        <pose>-0.5 -0.55 -0.5 1.57079633 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.01</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.01</iyy>
            <iyz>0</iyz>
            <izz>0.01</izz>
          </inertia>
          <mass>1.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <fdir1>1 0 0</fdir1>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>100</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.2</radius>
              <length>0.1</length>
            </cylinder>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="wheel_4_joint" type="revolute">
        <parent>link</parent>
        <child>wheel_4</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <dynamics>
            <damping>0.0</damping>
            <friction>0.0</friction>
          </dynamics>
          <xyz>0 1 0</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="sphere_link">
        <pose>0 0 0.5 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.1</iyy>
            <iyz>0</iyz>
            <izz>0.1</izz>
          </inertia>
          <mass>10.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <sphere>
              <radius>0.25</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <fdir1>1 0 0</fdir1>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>10</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <sphere>
              <radius>0.25</radius>
            </sphere>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="joint_sphere" type="revolute">
        <parent>link</parent>
        <child>sphere_link</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <link name="cylinder_link">
        <pose>0 0 0.25 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.1</iyy>
            <iyz>0</iyz>
            <izz>0.1</izz>
          </inertia>
          <mass>10.0</mass>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.25</radius>
              <length>0.5</length>
            </cylinder>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
            </friction>
            <contact>
              <ode>
                <max_vel>10</max_vel>
              </ode>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.25</radius>
              <length>0.5</length>
            </cylinder>
          </geometry>
          <material>
            <script>Gazebo/WoodPallet</script>
          </material>
        </visual>
      </link>
      <joint name="joint_cylinder" type="revolute">
        <parent>sphere_link</parent>
        <child>cylinder_link</child>
        <pose>0 0 0 0 0 0</pose>
        <axis>
          <xyz>0 0 1</xyz>
          <use_parent_model_frame>true</use_parent_model_frame>
        </axis>
      </joint>
      <plugin name="cart_test_plugin" filename="libCartDemoPlugin.so">
        <steer>bar_2_joint</steer>
        <steer_pid>10 0.1 0.5</steer_pid>
        <steer_ilim>-10 10</steer_ilim>
        <steer_pos>0.0</steer_pos>
        <!-- steer -->
        <steer_vel>0</steer_vel>
        <!-- not used -->
        <steer_eff>100.0</steer_eff>
        <!-- max pos pid effort -->
        <right>wheel_3_joint</right>
        <right_pid>0.1 0.0 0.001</right_pid>
        <right_ilim>-.1 .1</right_ilim>
        <right_pos>0</right_pos>
        <!-- final position pid -->
        <right_vel>0</right_vel>
        <!-- final velocity pid -->
        <right_eff>0.1</right_eff>
        <!-- drive torque -->
        <left>wheel_4_joint</left>
        <left_pid>0.1 0.0 0.001</left_pid>
        <left_ilim>-.1 .1</left_ilim>
        <left_pos>0</left_pos>
        <!-- final position pid -->
        <left_vel>0</left_vel>
        <!-- final velocity pid -->
        <left_eff>0.1</left_eff>
        <!-- drive torque -->
      </plugin>
    </model>
    <light type="directional" name="sun">
      <pose>0 0 10 0 0 0</pose>
      <diffuse>.8 .8 .8 1</diffuse>
      <specular>.1 .1 .1 1</specular>
      <attenuation>
        <range>10</range>
        <linear>0.01</linear>
        <constant>0.8</constant>
        <quadratic>0.0</quadratic>
      </attenuation>
      <direction>0 .5 -.5</direction>
      <cast_shadows>true</cast_shadows>
    </light>
  </world>
</sdf>
