<?xml version="1.0" ?>
<!--
  The pendulum has an extra link between the arm and the base links which is
  used in conjunction with a fixed joint to measure applied and constraint
  forces at the revolute joint (named motor_joint). -->
<sdf version="1.7">
  <world name="pendulum_joint_wrench">
    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>1 1 1 1</diffuse>
      <specular>0.5 0.5 0.5 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>
            <emissive>0.8 0.8 0.8 1</emissive>
          </material>
        </visual>
      </link>
    </model>

    <!-- Box used for contact force test. Initially positioned so that it doesn't collide with the pendulum -->
    <model name="box">
      <static>true</static>
      <pose>0.0 1 11.48 0 0 0</pose>
      <link name="box_link">
        <collision name="collision">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.0</mu>
                <mu2>0.0</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box><size>1 1 1</size></box>
          </geometry>
          <material>
            <ambient>0.8 0.2 0.2 1</ambient>
            <diffuse>0.8 0.2 0.2 1</diffuse>
            <emissive>0.8 0.2 0.2 1</emissive>
          </material>
        </visual>
      </link>
    </model>

    <model name="pendulum">
      <link name="base">
        <inertial>
          <mass>1000</mass>
        </inertial>
        <visual name="vis_plate_on_ground">
          <pose>0 0 0.01 0 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.8</radius>
              <length>0.02</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
          </material>
        </visual>
        <visual name="vis_pole">
          <pose>-0.1 0 1.115 0 0 0</pose>
          <geometry>
            <box>
              <size>0.2 0.2 2.2</size>
            </box>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
          </material>
        </visual>
        <!-- Using 4 spheres as contact points to ensure stable constraint force readings -->
        <collision name="col1">
          <pose>0.8 0 0.05  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.05</radius>
            </sphere>
          </geometry>
        </collision>
        <collision name="col2">
          <pose>-0.8 0 0.05  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.05</radius>
            </sphere>
          </geometry>
        </collision>
        <collision name="col3">
          <pose>0 0.8 0.05  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.05</radius>
            </sphere>
          </geometry>
        </collision>
        <collision name="col4">
          <pose>0 -0.8 0.05  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.05</radius>
            </sphere>
          </geometry>
        </collision>
      </link>

      <joint name="motor_joint" type="revolute">
        <parent>base</parent>
        <child>sensor_link</child>
        <axis>
          <xyz>0 0 1</xyz>
        </axis>
      </joint>

      <link name="sensor_link">
        <pose>0.125 0.0 2.1 0 1.5707963267948966 0</pose>
        <inertial>
          <mass>0.4</mass>
          <inertia>
            <ixx>0.01333</ixx>
            <iyy>0.01333</iyy>
            <izz>0.02</izz>
          </inertia>
        </inertial>
        <visual name="motor_viz">
          <geometry>
            <cylinder>
              <radius>0.08</radius>
              <length>0.25</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.0 0.8 0.0 1</ambient>
            <diffuse>0.0 0.8 0.0 1</diffuse>
          </material>
        </visual>
      </link>

      <joint name="sensor_joint" type="fixed">
        <pose relative_to="motor_joint"/>
        <parent>sensor_link</parent>
        <child>arm</child>
      </joint>

      <link name="arm">
        <pose>0.125 0.0 1.6   0 0 0</pose>
        <inertial>
          <mass>6.0</mass>
          <inertia>
            <ixx>.515</ixx>
            <iyy>.515</iyy>
            <izz>.03</izz>
          </inertia>
        </inertial>

        <visual name="vis_upper_joint">
          <pose>0 0 0.5 0 1.5707963267948966 0</pose>
          <geometry>
            <cylinder>
              <radius>0.1</radius>
              <length>0.2</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.0 0.0 0.8 1</ambient>
            <diffuse>0.0 0.0 0.8 1</diffuse>
          </material>
        </visual>
        <collision name="col_upper_joint">
          <pose>0 0 0.5 0 1.5707963267948966 0</pose>
          <geometry>
            <cylinder>
              <radius>0.1</radius>
              <length>0.3</length>
            </cylinder>
          </geometry>
        </collision>

        <visual name="vis_contact_sphere">
          <pose>0 0 -0.5   0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.11</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.5 0.5 0.0 1</diffuse>
          </material>
        </visual>
        <collision name="col_contact_sphere">
          <pose>0 0 -0.5   0 0 0</pose>
          <geometry>
            <sphere>
              <radius>0.11</radius>
            </sphere>
          </geometry>
        </collision>

        <visual name="vis_cylinder">
          <geometry>
            <cylinder>
              <radius>0.1</radius>
              <length>1.0</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
          </material>
        </visual>
        <collision name="col_cylinder">
          <geometry>
            <cylinder>
              <radius>0.1</radius>
              <length>1.0</length>
            </cylinder>
          </geometry>
        </collision>
      </link>
    </model>
  </world>
</sdf>

