<?xml version="1.0"?>
<robot name="wp_v4">

  <!-- material -->
  <material name = "black">
    <color rgba = "0.01 0.01 0.01 1"/>
  </material>
  <material name = "grey">
    <color rgba = "0.2 0.2 0.2 1"/>
  </material>
  <material name = "blue">
    <color rgba = "0.2 0.2 0.9 1"/>
  </material>


<link name="base_footprint">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.05 0.05 0.001" />
      </geometry>
      <material name="TransparentGreen" />
    </visual>
    
</link>

<joint name="base_joint" type="fixed">
  <origin xyz="0 0 0" rpy="0 0 0" />
  <parent link="base_footprint"/>
  <child link="base_link" />
</joint>

  <!-- base -->
  <link name="base_link">
  <visual>
   <geometry>
    <cylinder length="0.6" radius="0.0"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 0"/>
  </visual>
  </link>

  <!-- body -->
  <link name = "body_link">
    <visual>
      <geometry>
        <mesh filename="package://wpb_home_bringup/meshes/wpb_home_mani.dae" scale=" 1 1 1 "/>
      </geometry>
      <origin rpy = "1.57 0 1.57" xyz = "-.225 -0.225 0"/>
    </visual>
    <collision>
      <origin xyz=".013 0 .68" rpy="0 0 0" />
      <geometry>
        <cylinder length="1.37" radius="0.226"/>
      </geometry>
    </collision>
    <inertial>
    <mass value="10"/>
    <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
    </inertial>
  </link>
  <joint name = "base_to_body" type = "fixed">
    <parent link = "base_link"/>
    <child link = "body_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
  </joint>
  <gazebo reference = "body_link">
      <turnGravityOff>false</turnGravityOff>
  </gazebo>

<!-- lidar -->
<link name = "laser">
  <visual>
   <geometry>
    <cylinder length="0.00" radius="0.00"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 0"/>
  </visual>
</link>
<joint name="laser_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"  /> <!--pos-->
    <parent link="base_link" />
    <child link="laser" />
</joint>

<!-- kinect -->
<link name = "kinect2_dock">
  <visual>
   <geometry>
    <!-- <box size=".01 .25 .07"/>-->
    <box size="0 0 0"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 0"/>
   <material name = "red"/>
  </visual>
</link>
<joint name="kinect_height" type="prismatic">
    <parent link="base_link"/>
    <child link="kinect2_dock"/>
    <limit effort="1000.0" lower="0" upper="1.7" velocity="0.5"/>
    <origin xyz="0.145 -0.013 0" rpy="0 -1.5707963 0"/>
  </joint>

<link name = "kinect2_ir_optical_frame">
  <visual>
   <geometry>
    <!-- <box size=".25 .04 .07"/>-->
    <box size="0 0 0"/>
   </geometry>
   <origin xyz = "0 0 0" rpy = "0 0 0"/>
   <material name = "blue"/>
  </visual>
</link>

<joint name="kinect_pitch" type="revolute">
    <axis xyz="1 0 0"/>
    <limit effort="1000.0" lower="-1.57" upper="1.57" velocity="0.5"/>
    <origin xyz="0 0 0" rpy="3.1415926 0 -1.5707963" /> <!--pos-->
    <parent link="kinect2_dock" />
    <child link="kinect2_ir_optical_frame" />
</joint>
<!-- <joint name="kinect_joint" type="fixed">
    <origin xyz="0.174 0 1.35" rpy="-1.5707963 0 -1.5707963" /> 
    <parent link="base_link" />
    <child link="kinect2_ir_optical_frame" />
</joint>-->

<link name = "kinect2_rgb_optical_frame">
  <visual>
   <geometry>
    <box size="0 0 0"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 0"/>
   <material name = "blue"/>
  </visual>
</link>
<joint name="kinect_hd_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0" /> <!--pos-->
    <parent link="kinect2_ir_optical_frame" />
    <child link="kinect2_rgb_optical_frame" />
</joint>

<!-- manipulator -->
<link name = "mani_lift">
  <visual>
   <geometry>
    <box size=".03 .04 .06"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 -0.06"/>
   <material name = "black"/>
  </visual>
</link>
<joint name="mani_base" type="prismatic">
    <parent link="base_link"/>
    <child link="mani_lift"/>
    <limit effort="1000.0" lower="0" upper="0.7" velocity="0.5"/>
    <origin xyz="0.1 0 0.4" rpy="0 -1.5707963 0"/>
  </joint>

<link name = "mani_elbow">
  <visual>
   <geometry>
    <cylinder length="0.00" radius="0.00"/>
   </geometry>
   <origin rpy = "0 0 0" xyz = "0 0 0"/>
  </visual>
</link>
<joint name="lift_elbow" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 -0.08"  /> <!--pos-->
    <parent link="mani_lift" />
    <child link="mani_elbow" />
</joint>

<link name = "mani_forearm">
  <visual>
    <geometry>
      <mesh filename="package://wpb_home_bringup/meshes/forearm.dae" scale=" 1 1 1 "/>
    </geometry>
    <origin rpy = "-1.5707963 3.1415926 0" xyz = "0.365 -0.037 -0.0355"/>
  </visual>
</link>

<joint name="elbow_forearm" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="1000.0" lower="0" upper="1.57" velocity="0.5"/>
    <origin xyz="0 0 0" rpy="0 0 0" /> <!--pos-->
    <parent link="mani_elbow" />
    <child link="mani_forearm" />
</joint>

<!-- finger -->
<link name = "mani_left_finger">
    <visual>
        <geometry>
            <mesh filename="package://wpb_home_bringup/meshes/finger.dae" scale=" 1 1 1 "/>
        </geometry>
        <origin rpy = "-1.57 0 1.57" xyz = "0.154 -0.005 0.009"/>
    </visual>
</link>
<joint name = "forearm_left_finger" type = "revolute">
    <parent link = "mani_forearm"/>
    <child link = "mani_left_finger"/>
    <origin xyz = "0.32 0.0125 0"/>
    <axis xyz = "0 0 1"/>
    <limit effort = "1000.0" lower="0" upper="1.0" velocity="0.5"/>
</joint>

<link name = "mani_right_finger">
    <visual>
        <geometry>
            <mesh filename="package://wpb_home_bringup/meshes/finger.dae" scale="1 1 1"/>
        </geometry>
        <origin rpy = "1.57 0 -1.57" xyz = "0.154 0.005 -0.020"/>
    </visual>
</link>
<joint name = "forearm_right_finger" type = "revolute">
    <parent link = "mani_forearm"/>
    <child link = "mani_right_finger"/>
    <origin xyz = "0.32 -0.0125 0"/>
    <axis xyz = "0 0 -1"/>
    <limit effort = "1000.0" lower="0" upper="1.0" velocity="0.5"/>
</joint>

<!-- Gazebo plugin for ROS Control -->
<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/</robotNamespace>
  </plugin>
</gazebo>

</robot>
