<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="boxy_description">
	<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_boxy_description)/sensors/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_kinect2_description)/urdf/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" />
	
	<link name="base_footprint">
	</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 -->

	<!-- JOINT to connect the base with the pw070 -->
	<joint name="head_joint" type="fixed">
		<origin xyz="${head_joint_x} ${head_joint_y} ${head_joint_z}" rpy="0 0 0" />
		<parent link="base_link" />
		<child link="pw070_base_link" />
	</joint>
	<!-- /JOINT to connect the base with the pw070 -->
	
	<xacro:include filename="$(find iai_schunk_pw070_v6)/robots/pw070_v6.URDF" />

	<!-- Kinect2 -->
        <!-- Pose comes from tool_H_clink in the calibration
             The quaternion was converted to RPY using python and KDL
               halcon has quat: w x y z
               kdl and tf_static_transform_publisher use: x y z w
             So take the first number from the halcon quat and put it last for KDL.
               import PyKDL as p; r2 = p.Rotation.Quaternion(-0.494547,0.491515,-0.506787,0.506955) ; r2.GetRPY() 
               answer: (-1.5431199762943342, -0.0029080109443817533, -1.5676362600579434) -->

        <!-- The following kinect2_xacro defines the _kinect2_rgb_optical_frame
        And includes the fixed joint to place the kinect on the pw070  -->
        <kinect2_xacro parent="pw070_frame_out" prefix="head_mount">
		<origin xyz="0.0402529 -0.0959434 0.0332714" rpy="-1.5431199762943342 -0.0029080109443817533 -1.5676362600579434" />
        </kinect2_xacro>




        <!-- shoulder kinect -->
        <!-- ORIGINAL DATA FROM HALCON CALIBRATION:
             from: base_link to: shoulder_kinect_link
             translation: -0.135394 -0.00764754 1.54755 (xyz)
             quaternion: 0.0177781 0.202257 -0.0472198 0.978032 (xyzw) -->
        <kinect name="shoulder_kinect" parent="base_link">
          <origin xyz="-0.135394 -0.00764754 1.54755"
                  rpy="0.01708077682636155 0.4085798647123734 -0.09294704527264909"/> 
       </kinect>
	
	<!-- 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" name="left">
          <origin xyz="0 0 0" rpy="0 0 1.047198"/>
	</kuka_lwr_arm>

	<wsg_50_xacro name="left_gripper" parent="left_arm_cabling_adaptor_link">
          <origin xyz="0 0 0" rpy="0 0 0.78539816339"/>
	</wsg_50_xacro>

	<!--This is a 60deg rotation to calib_right_arm_base_link -->
	<kuka_lwr_arm parent="triangle_right_arm_link" name="right">
          <origin xyz="0 0 0" rpy="0 0 -1.047198"/>
	</kuka_lwr_arm>

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

	
	<xacro:include filename="$(find iai_roller_description)/urdf/iai_roller.urdf.xacro" />
	<!--
	  if you need old logs you might have to rename this to
	
	right_arm_fwk_fwa_050_frame_in instead of roller
	-->
        <iai_roller name="right_arm_roller" parent="right_arm_adapter_fwa050_wsg50_frame_out">
          <origin xyz="0 0 0" rpy="0 0 3.14159265359"/>
	</iai_roller>
	<!--
        <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>
	-->
        <!-- Temporary place for the spoon frames -->



</robot>
