git_ws/rbx1/rbx1_description/urdf/pi_robot_head.xacro

<?xml version="1.0"?>

<robot name="pi_robot_head" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find rbx1_description)/urdf/pi_robot_head_hardware.xacro" />

  <macro name="pi_robot_head" params="parent color *origin"> 
    <link name="head_base_link" />     
    <joint name="head_base_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="head_base_link" />
    </joint>
    
    <!-- fake head_pan_link joint -->
    <link name="head_pan_link" />
    <joint name="fake_head_pan_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 ${M_PI/2} ${M_PI/2}" />
      <parent link="head_pan_servo_link"/>
      <child link="head_pan_link" />
    </joint>
    
     <!-- fake head_tilt_link joint -->
    <link name="head_tilt_link" />
    <joint name="fake_head_tilt_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 ${M_PI}" />
      <parent link="head_tilt_servo_link"/>
      <child link="head_tilt_link" />
    </joint>
    
    <!-- head pan joint -->
    <dynamixel_AX12_fixed parent="head_base_link" name="head_pan_servo">
       <origin xyz="0 0 ${-AX12_WIDTH/2}" rpy="${M_PI/2} 0 ${-M_PI/2}"/>
    </dynamixel_AX12_fixed>
    <bioloid_F3_head_revolute parent="head_pan_servo_link" number="10" joint_name="head_pan" x_loc="0" y_loc="${AX12_WIDTH/2 + 0.005}" z_loc="0.0" ulimit="2.9" llimit="-2.9" r="${-M_PI/2}" p="${M_PI/2}" y="${M_PI}" color="${color}"/>
    
    <dynamixel_AX12_fixed parent="bioloid_F3_head_10_link" name="head_tilt_servo">
       <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
    </dynamixel_AX12_fixed>
    
    <!--head tilt joint -->
    <bioloid_F4_head_revolute parent="head_tilt_servo_link" number="10" joint_name="head_tilt" x_loc="0" y_loc="0" z_loc="0" ulimit="2.9" llimit="-2.9" color="${color}"/>
    <bioloid_F10_fixed parent="bioloid_F4_head_10_link" name="head_tilt_bracket" color="${color}">
       <origin xyz="0 0 ${F4_HEIGHT + F10_HEIGHT/2}" rpy="0 0 0"/>
    </bioloid_F10_fixed>

    <joint name="base_camera_joint" type="fixed">
      <origin xyz="-0.03 0 0.058" rpy="0 0 ${M_PI}" />
      <parent link="head_tilt_bracket_link" />
      <child link="camera_link" />
    </joint>

    <link name="camera_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz="0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/kinect.dae" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="-0.0275 0.0 0.0" rpy="0 0 0" />
        <geometry>
          <box size="0.0730 .2760 0.0720"/>
        </geometry>
      </collision>
    </link>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="0 0.018 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <child link="camera_depth_frame" />
  </joint>

  <link name="camera_depth_frame">
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <joint name="camera_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="camera_depth_frame" />
    <child link="camera_depth_optical_frame" />
  </joint>

  <link name="camera_depth_optical_frame">
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <joint name="camera_rgb_joint" type="fixed">
    <origin xyz="0 -0.005 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <child link="camera_rgb_frame" />
  </joint>

  <link name="camera_rgb_frame">
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <joint name="camera_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="camera_rgb_frame" />
    <child link="camera_rgb_optical_frame" />
  </joint>

  <link name="camera_rgb_optical_frame">
    <inertial>
      <mass value="0.000001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>
  
  <link name="left_eye_link">
    <visual>	
      <origin xyz="0 0 0" rpy="0 1.57 0" />
	  <geometry>
	     <cylinder length="0.01" radius="0.01" />
	  </geometry>
	  <material name="OffWhite" />
	</visual>
    <collision>
	  <origin xyz="0 0 0" rpy="0 1.57 0" />
	  <geometry>
        <cylinder length="0.01" radius="0.01" />
      </geometry>
    </collision>
    <inertial>
       <mass value="0.00001" />
       <origin xyz="0 0 0" />
       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
         iyy="1.0" iyz="0.0"
         izz="1.0" />
     </inertial>
  </link>
  
  <link name="right_eye_link">
    <visual>	
      <origin xyz="0 0 0" rpy="0 1.57 0" />
	  <geometry>
	     <cylinder length="0.01" radius="0.01" />
	  </geometry>
	  <material name="OffWhite" />
	</visual>
    <collision>
	  <origin xyz="0 0 0" rpy="0 1.57 0" />
	  <geometry>
        <cylinder length="0.01" radius="0.01" />
      </geometry>
    </collision>
    <inertial>
       <mass value="0.00001" />
       <origin xyz="0 0 0" />
       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
         iyy="1.0" iyz="0.0"
         izz="1.0" />
     </inertial>
  </link>
  
  
  <joint name="left_eye_joint" type="fixed">
    <parent link="camera_link" />
    <child link="left_eye_link" />
    <origin xyz="-0.005 0.035 0" rpy="0 0 0" />
  </joint>
	
  <joint name="right_eye_joint" type="fixed">
    <parent link="camera_link" />
    <child link="right_eye_link" />
    <origin xyz="-0.005 -0.035 0" rpy="0 0 0" />
  </joint>

 </macro>

</robot>