<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find iai_kuka_lwr4_description)/defs/materials.xml"/>
  <xacro:include filename="$(find iai_kuka_lwr4_description)/urdf/kuka_lwr_arm.urdf.xacro"/>
  <xacro:include filename="$(find iai_wsg_50_description)/urdf/wsg_50.urdf.xacro"/>
  <xacro:include filename="$(find iai_kms_40_description)/urdf/kms_40.urdf.xacro"/>
  <xacro:include filename="$(find iai_adapters_description)/urdf/lwr4_cabling_adaptor.urdf.xacro"/>
  <!--xacro:include filename="$(find iai_kinect_description)/urdf/kinect.urdf.xacro"/-->
  <xacro:include filename="$(find iai_adapters_description)/urdf/adapter_fwa050_wsg50.urdf.xacro"/>
  <xacro:include filename="$(find iai_adapters_description)/urdf/adapter_kms40_fwk050.urdf.xacro"/>
  <xacro:include filename="$(find iai_adapters_description)/urdf/fwk_fwa_050.urdf.xacro"/>
  <xacro:include filename="$(find iai_adapters_description)/urdf/adapter_boxy_ur3.urdf.xacro"/>
  <xacro:include filename="$(find iai_ur_description)/urdf/ur3.urdf.xacro"/>
  <xacro:include filename="$(find iai_kinect2_description)/urdf/kinect2.urdf.xacro"/>
  <xacro:include filename="$(find iai_adapter_iso50_kinect2_description)/urdf/adapter_iso50_kinect2.urdf.xacro"/>
  <!-- Property definitions e.g. constants -->
  <xacro:property name="head_joint_x" value="0.2"/>
  <xacro:property name="head_joint_y" value="-0.21"/>
  <xacro:property name="head_joint_z" value="1.48"/>
  <xacro:property name="torso_joint_x" value="0"/>
  <!--this is the diamiter of one of the aluminium (/2) pipes (20mm) = the diamiter of the piece that holds the slider (45mm) -->
  <xacro:property name="torso_joint_y" value="0.065"/>
  <!-- distance from the axis 0 to the first alu pipe 74 + distance between the pipes 150 + diamiter of the slider 200 /2   ( = center ) -->
  <xacro:property name="torso_joint_z" value="0.410"/>
  <!-- /Property definitions e.g. constants -->
  <xacro:include filename="$(find iai_boxy_base)/robots/base.URDF"/>
  <xacro:macro name="boxy">
    <!-- virtual joints to model odometry -->
    <link name="odom"/>
    
    <joint name="odom_x_joint" type="prismatic">
      <parent link="odom"/>
      <child link="odom_x_frame"/>
      <axis xyz="1 0 0"/>
      <!-- limits chosen heuristically
              lower/upper: +-1km to create a quasi limitless prismatic joint
              velocity: slow walking speed is 3-4km/hour ~= 1m/s
              effort: reach max speed within 1s, Boxy has a mass of 200kg, so Fmax=200kg*1m/s2 -->
      <limit lower="-1000" upper="1000" velocity="1" effort="200"/>
    </joint>
    <link name="odom_x_frame"/>

    <joint name="odom_y_joint" type="prismatic">
      <parent link="odom_x_frame"/>
      <child link="odom_y_frame"/>
      <axis xyz="0 1 0"/>
      <!-- limits chosen heuristically
               lower/upper: +-1km to create a quasi limitless prismatic joint
               velocity: slow walking speed is 3-4km/hour ~= 1m/s
               effort: reach max speed within 1s, Boxy has a mass of 200kg, so Fmax=200kg*1m/s2 -->
      <limit lower="-1000" upper="1000" velocity="1" effort="200"/>
    </joint>
    <link name="odom_y_frame"/>

    <joint name="odom_z_joint" type="continuous">
      <parent link="odom_y_frame"/>
      <child link="base_footprint"/>
      <axis xyz="0 0 1"/>
      <!-- limits chosen heuristically
               velocity: one slow complete turn around in 5-6s, so 6.2rad/6s ~= 1rad/s
               effort: reach max speed within 1s, Boxy has a mass of 200kg and a diagonale of 1m, so Fmax=200kg*1m*1m/s2 -->
      <limit velocity="1" effort="200"/>
    </joint>

    <link name="base_footprint">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <material name="White"/>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.071"/>
        <geometry>
          <box size="0.001 0.001 0.001"/>
        </geometry>
      </collision>
    </link>

    <!-- JOINT to define base_footprint -->
    <joint name="base_footprint_joint" type="fixed">
      <origin xyz="0.36 0.25 0.16" rpy="0 0 0"/>
      <parent link="base_footprint"/>
      <child link="base_link"/>
    </joint>

    <!-- /JOINT for base_footprint -->
    <!-- NEW NECK ASSEMBLY -->
    <!-- mounting plate of UR3 -->
    <xacro:property name="ur3_prefix" value="neck_"/>
    <adapter_boxy_ur3_xacro prefix="${ur3_prefix}" parent="base_link">
      <origin xyz="0.175 -0.227 1.48" rpy="0 0 0"/>
    </adapter_boxy_ur3_xacro>
    <xacro:ur3_robot prefix="${ur3_prefix}" joint_limited="true" transmission_hw_interface="$(arg transmission_hw_interface)" kinematics_file="${load_yaml('$(arg kinematics_config)')}"/>

    <joint name="${ur3_prefix}base_mounting_joint" type="fixed">
      <parent link="${ur3_prefix}adapter_ur3_frame_out" />
      <child link="${ur3_prefix}base_link" />
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -${M_PI/2.0}"/>
    </joint>

    <!-- The following URDF has frame_in / frame_out, so it is placed at the parent -->
    <adapter_iso50_kinect2_xacro parent="neck_tool0" prefix="neck">
      <origin xyz="0 0 0" rpy="0 0 -1.570795"/>
    </adapter_iso50_kinect2_xacro>
 
    <kinect2_xacro parent="neck_adapter_iso50_kinect2_frame_out" prefix="head_mount">
      <origin xyz="-0.031284 0.098627 0.023544" rpy="4.728550 6.279385 1.578295"/>
    </kinect2_xacro>


    <!-- JOINT to connect the base with the torso -->
    <joint name="torso_joint" type="fixed">
      <origin xyz="${torso_joint_x} ${torso_joint_y} ${torso_joint_z}" rpy="0 0 0"/>
      <parent link="base_link"/>
      <child link="torso_base_link"/>
    </joint>

    <!-- /JOINT to connect the base with the torso -->
    <xacro:include filename="$(find iai_boxy_torso)/robots/torso.URDF"/>

    <!--FIXME: triangle_left_arm_link should be rotated, and not the origin of the arm (which right now is calib_left_arm_base_link -->
    <kuka_lwr_arm parent="triangle_left_arm_link" prefix="left_" color_postfix="">
      <!-- From CAD -->
      <!--origin xyz="0 0 0" rpy="0 0 1.047198"/-->
      <!-- from calibration on 20160903 -->
      <origin xyz="-0.006185 0.004080 -0.010726" rpy="6.276354 6.281653 1.074679"/>
    </kuka_lwr_arm>
    <lwr4_cabling_adaptor parent="left_arm_flange_link" prefix="left_arm_">
      <origin xyz="0 0 0.01" rpy="0 0 0"/>
    </lwr4_cabling_adaptor>

    <!-- The following URDFs have frame_in / frame_out, so they are simply placed at their parent -->
    <kms_40_xacro parent="left_arm_cabling_adaptor_link" prefix="left_arm_">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <!--places the kms40 frame 13mm from the parent -->
    </kms_40_xacro>
    <adapter_kms40_fwk050_xacro parent="left_arm_kms40_frame_out" prefix="left_arm_">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </adapter_kms40_fwk050_xacro>
    <fwk_fwa_050_xacro parent="left_arm_adapter_kms40_fwk050_frame_out" prefix="left_arm_">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </fwk_fwa_050_xacro>
    <adapter_fwa050_wsg50_xacro parent="left_arm_fwk_fwa_050_frame_out" prefix="left_arm_">
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
    </adapter_fwa050_wsg50_xacro>
    <wsg_50_xacro name="left_gripper" parent="left_arm_adapter_fwa050_wsg50_frame_out">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </wsg_50_xacro>

    <link name="left_wrist_optical_frame_x">
      <inertial>
        <mass value="1"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <material name="White"/>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.0"/>
        <geometry>
          <box size="0.001 0.001 0.001"/>
        </geometry>
      </collision>
    </link>

    <!-- Box that limits the position of the head while planning -->
    <link name="limit_box">
      <visual>
        <origin rpy="0 0 0" xyz="0.25 -0.2 1.35"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <material name="box_color">
          <color rgba="0.2 0.2 0.2 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0.25 -0.2 1.35"/>
        <geometry>
          <box size="0.4 0.35 0.1"/>
        </geometry>
      </collision>
    </link>
    <joint name="fixed_box" type="fixed">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <parent link="base_link"/>
      <child link="limit_box"/>
    </joint>

    <link name="limit_box2">
      <visual>
        <origin rpy="0 0 0" xyz="-0.2 -0.2 1.55"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <material name="box_color">
          <color rgba="0.2 0.2 0.2 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="-0.2 -0.2 1.55"/>
        <geometry>
          <box size="0.25 0.40 0.15"/>
        </geometry>
      </collision>
    </link>

    <joint name="fixed_box2" type="fixed">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <parent link="base_link"/>
      <child link="limit_box2"/>
    </joint>

    <!-- JOINT to define base_footprint -->
    <joint name="left_wrist_optical_joint_x" type="fixed">
      <origin xyz="0.000836 -0.053028 0.073603" rpy="0.007552 6.274592 3.141201"/>
      <parent link="left_gripper_base_link"/>
      <child link="left_wrist_optical_frame_x"/>
    </joint>

    <!-- /JOINT for base_footprint -->
    <link name="left_wrist_optical_frame">
      <inertial>
        <mass value="1"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
        <material name="White"/>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.0"/>
        <geometry>
          <box size="0.001 0.001 0.001"/>
        </geometry>
      </collision>
    </link>

    <!-- JOINT to define base_footprint -->
    <joint name="left_wrist_optical_joint" type="fixed">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 3.141519"/>
      <parent link="left_wrist_optical_frame_x"/>
      <child link="left_wrist_optical_frame"/>
    </joint>

    <!-- /JOINT for base_footprint -->
    <!--wsg_50_xacro name="left_gripper" parent="left_arm_cabling_adaptor_link">
          <origin xyz="0 0 0" rpy="0 0 2.35619183661"/>
	</wsg_50_xacro-->
    <!--This is a 60deg rotation to calib_right_arm_base_link -->
    <kuka_lwr_arm parent="triangle_right_arm_link" prefix="right_" color_postfix="">
      <origin xyz="0 0 0" rpy="0 0 -1.047198"/>
    </kuka_lwr_arm>
    <lwr4_cabling_adaptor parent="right_arm_flange_link" prefix="right_arm_">
      <origin xyz="0 0 0.01" rpy="0 0 0"/>
    </lwr4_cabling_adaptor>

    <!-- The following URDFs have frame_in / frame_out, so they are simply placed at their parent -->
    <kms_40_xacro parent="right_arm_cabling_adaptor_link" prefix="right_arm_">
      <origin xyz="0 0 0" rpy="0 0 3.14159265359"/>
      <!--places the kms40 frame 13mm from the parent -->
    </kms_40_xacro>
    <adapter_kms40_fwk050_xacro parent="right_arm_kms40_frame_out" prefix="right_arm_">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </adapter_kms40_fwk050_xacro>
    <fwk_fwa_050_xacro parent="right_arm_adapter_kms40_fwk050_frame_out" prefix="right_arm_">
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </fwk_fwa_050_xacro>
    <adapter_fwa050_wsg50_xacro parent="right_arm_fwk_fwa_050_frame_out" prefix="right_arm_">
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
    </adapter_fwa050_wsg50_xacro>
    <wsg_50_xacro name="right_gripper" parent="right_arm_adapter_fwa050_wsg50_frame_out">
      <origin xyz="0 0 0" rpy="0 0 3.14159265359"/>
    </wsg_50_xacro>
  </xacro:macro>
</robot>
