git_ws/rbx1/rbx1_description/urdf/pi_robot_arm.xacro

<?xml version="1.0"?>

<robot name="pi_robot_arm" xmlns:xacro="http://ros.org/wiki/xacro">
  <property name="MESH_SCALE" value="100"/>

  <xacro:include filename="$(find rbx1_description)/urdf/pi_robot_arm_hardware.xacro" />

  <material name="white">
    <color rgba="0.87 0.90 0.87 1.0"/>
  </material>
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.22 0.32 0.14 1.0"/>
  </material>

  <macro name="pi_robot_arm" params="parent color gripper_color *origin"> 
    <link name="arm_base_link" />     
    <joint name="arm_base_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="arm_base_link" />
    </joint>

    <!-- fake gripper_link joint -->
    <link name="gripper_link" />
    <joint name="gripper_link_joint" type="fixed">
      <origin xyz="0 0 .112" rpy="0 -${M_PI/2} 0" />
      <parent link="arm_wrist_flex_link"/>
      <child link="gripper_link" />
    </joint>

    <!-- shoulder pan joint -->
    <dynamixel_AX12_fixed parent="arm_base_link" name="arm_shoulder_pan_servo">
      <origin xyz="0 0 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
    </dynamixel_AX12_fixed>
    <bioloid_F3_revolute parent="arm_shoulder_pan_servo_link" name="arm_shoulder_pan" color="${color}" vlimit="0.785" llimit="-2.617" ulimit="2.617">
      <origin xyz="0 ${AX12_WIDTH/2} 0" rpy="${-M_PI/2} ${M_PI/2} ${M_PI}" />
    </bioloid_F3_revolute>

    <!-- shoulder lift joint -->
    <dynamixel_AX12_fixed parent="arm_shoulder_pan_link" name="arm_shoulder_lift_servo">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0" />
    </dynamixel_AX12_fixed>
    <bioloid_F4_revolute parent="arm_shoulder_lift_servo_link" name="arm_shoulder_lift" color="${color}" vlimit="1.571" llimit="-2.617" ulimit="2.617">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F4_revolute>
    <bioloid_F10_fixed parent="arm_shoulder_lift_link" name="arm_shoulder_F10_0" color="${color}">
      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arm_shoulder_F10_0_link" name="arm_shoulder_F10_1" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arm_shoulder_F10_1_link" name="arm_shoulder_F10_2" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F3_fixed parent="arm_shoulder_F10_2_link" name="arm_shoulder_F3_0" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>

    <!-- elbow joint -->
    <dynamixel_AX12_fixed parent="arm_shoulder_F3_0_link" name="arm_elbow_flex_servo">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
    </dynamixel_AX12_fixed>
    <bioloid_F4_revolute parent="arm_elbow_flex_servo_link" name="arm_elbow_flex" color="${color}" vlimit="1.571" llimit="-2.617" ulimit="2.617">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F4_revolute>
    <bioloid_F10_fixed parent="arm_elbow_flex_link" name="arm_elbow_F10_0" color="${color}">
      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arm_elbow_F10_0_link" name="arm_elbow_F10_1" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arm_elbow_F10_1_link" name="arm_elbow_F10_2" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F3_fixed parent="arm_elbow_F10_2_link" name="arm_elbow_F3_0" color="${color}">
      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>

    <!-- wrist joint -->
    <dynamixel_AX12_fixed parent="arm_elbow_F3_0_link" name="arm_wrist_flex_servo">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
    </dynamixel_AX12_fixed>
    <bioloid_F2_revolute parent="arm_wrist_flex_servo_link" name="arm_wrist_flex" color="${color}" vlimit="1.571" llimit="-1.745" ulimit="1.745">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F2_revolute>
    <bioloid_F3_fixed parent="arm_wrist_flex_link" name="arm_wrist_F3_0" color="${color}">
      <origin xyz="0 0.016 ${F2_HEIGHT}" rpy="0 ${M_PI} ${-M_PI/2}" />
    </bioloid_F3_fixed>

    <!-- gripper joint -->
    <dynamixel_AX12_fixed parent="arm_wrist_F3_0_link" name="gripper_servo">
      <origin xyz="-0.02275 0 ${-AX12_WIDTH/2}" rpy="${M_PI} ${M_PI/2} 0"/>
    </dynamixel_AX12_fixed>

    <!-- finger 1 -->
    <joint name="gripper_joint" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <limit effort="30" velocity="0.785" lower="-0.060" upper="0.50" />
      <parent link="gripper_servo_link"/>
      <child link="gripper_active_link" />
    </joint>
    <link name="gripper_active_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/F2.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0.00775" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.0485 0.0375"/>
        </geometry>
      </collision>
    </link>
    <finger_fixed parent="gripper_active_link" name="gripper_active_finger" color="${gripper_color}">
      <origin xyz="0 0 ${F2_HEIGHT}" rpy="0 0 0" />
    </finger_fixed>

    <!-- finger 2 -->
    <bioloid_F3_fixed parent="gripper_servo_link" name="gripper_static" color="${color}">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>
    <finger_fixed parent="gripper_static_link" name="gripper_static_finger" color="${gripper_color}">
      <origin xyz="0 0 0" rpy="0 0 ${M_PI}" />
    </finger_fixed>

  </macro>

</robot>