<?xml version="1.0"?>
<!-- This is a hand-made URDF XACRO file for simulating the position where the Microsoft Kinect-2 sensor is looking -->
<!-- Copyright Universitaet Bremen - Instititute for Artificial Intelligence. Author: Minerva Vargas -->
<!-- License: GPLv3+ -->

<robot xmlns:xacro="http://ros.org/wiki/xacro" name="kinect2_target"> 

	<xacro:macro name="kinect2_target_xacro" params ="prefix parent *origin">

		<joint name="${prefix}_joint_end" type="prismatic">
			<xacro:insert_block name="origin"/>
			<parent link="${parent}"/>
			<child link="${prefix}_look_target"/>
			<limit effort="1000.0" lower="0" upper="3.5" velocity="0.5"/>
			<origin xyz="0 0.0 0.2" rpy="0 0 0" />
			<axis xyz="0 0 1" />
		</joint>

		<link name="${prefix}_look_target">
			<visual>
				<origin rpy="0 0 0" xyz="0 0 ${Length/2+.03}"/>
				<geometry>
					<cylinder length="0.001" radius="0.001"/>
				</geometry>
				<material name="blue">
					<color rgba="0.2 0.2 0.7 1"/>
				</material>
			</visual>
			<collision>
        <origin rpy="0 0 0" xyz="0 0 0.02"/>
        <geometry>
          <cylinder length="${Length}" radius="${R}"/>
        </geometry>
      </collision>
		</link>

		<link name="${prefix}_obstacle">
			<visual>
				<origin rpy="0 0.0 0" xyz="0 0 0.1"/>
				<geometry>
					<mesh filename="package://iai_kinect2_look_target/meshes/Cilinder.stl" scale="0.0001 0.0001 0.0001" />
				</geometry>
				<material name="blue">
					<color rgba="0.2 0.2 0.7 1"/>
				</material>
			</visual>
			<collision>
        <origin rpy="0 1.5708 0" xyz="0 0 0.1"/>
        <geometry>
          <mesh filename="package://iai_kinect2_look_target/meshes/Cilinder.stl" scale="0.01 0.01 0.01" />
        </geometry>
				<material name="blue">
					<color rgba="0.2 0.2 0.5 1"/>
				</material>
      </collision>
		</link>

<!-- Joints at the end of the target, to allow any orientation -->

		<joint name="head1" type="continuous">
			<parent link="${prefix}_look_target"/>
			<child link="${prefix}_look_target_2"/>
			<origin xyz="0 0 0" rpy="0 0 0" />
			<axis xyz="0 0 1" />
		</joint>

		<joint name="head" type="fixed">
			<parent link="${parent}"/>
			<child link="${prefix}_obstacle"/>
			<origin xyz="0 0.0 0.01" rpy="0 0 0" />
			<axis xyz="0 0 1" />
		</joint>

		<joint name="head2" type="continuous">
			<parent link="${prefix}_look_target_2"/>
			<child link="${prefix}_look_target_3"/>
			<origin xyz="0 0 0" rpy="0 0 0" />
			<axis xyz="0 1 0" />
		</joint>

		<joint name="head3" type="continuous">
			<parent link="${prefix}_look_target_3"/>
			<child link="${prefix}_look_target_4"/>
			<origin xyz="0 0 0" rpy="0 0 0" />
			<axis xyz="1 0 0" />
		</joint>

		<link name="${prefix}_look_target_2"/>
		<link name="${prefix}_look_target_3"/>
		<link name="${prefix}_look_target_4"/>

	</xacro:macro>

	<xacro:property name="R" value="0.022" />
	<xacro:property name="Length" value="0.35" />


</robot> 
