Merge pull request #1 from corb555/indigo-devel

Adding support for PhantomX Pincher gripper

README.md

@@ -3,10 +3,13 @@ TurtleBot Arm
 
 Indigo version of turtlebot arm code. It should easily work on Hydro, too. Package turtlebot_arm_moveit_demos provides use examples to start playing with the arm on MoveIt!, while the recovered on indigo turtlebot_arm_block_manipulation provides a more complete and interesting demo.
 
+## Selecting Arm Type
+By default this will work with the original white/green TurtleBot arm. To use the PhantomX Pincher, set environment variable "TURTLEBOT_ARM1" to pincher. You will need arbotix_ros version 0.11.0 or higher for PhantomX Pincher.
+
 ## Attaching the Arm to a Robot
 Open your xacro-macro-magic URDF, and add something like:
 
-  <include filename="$(find turtlebot_arm_description)/urdf/arm.xacro" />
+  <include filename="$(find turtlebot_arm_description)/urdf/$(optenv TURTLEBOT_ARM1 turtlebot)_arm.xacro" />
   <turtlebot_arm parent="base_link" color="white" gripper_color="green"
   joints_vlimit="1.571" pan_llimit="-2.617" pan_ulimit="2.617">
   <origin xyz="0 0 1"/>

turtlebot_arm_block_manipulation/launch/block_manipulation.launch

@@ -1,5 +1,5 @@
 <launch>
-
+<!-- Launch block manipulation demo -->
  <node name="block_detection_action_server" pkg="turtlebot_arm_block_manipulation" type="block_detection_action_server" output="screen">
  </node>
 

turtlebot_arm_bringup/config/pincher_arm.yaml

@@ -0,0 +1,14 @@
+source: pincher_arm.yaml
+port: /dev/ttyUSB0
+read_rate: 15
+write_rate: 25
+joints: {
+ arm_shoulder_pan_joint: {id: 1, neutral: 512, max_angle: 140, min_angle: -140, max_speed: 90, type: dynamixel},
+ arm_shoulder_lift_joint: {id: 2, max_angle: 126, min_angle: -119, max_speed: 90, type: dynamixel},
+ arm_elbow_flex_joint: {id: 3, max_angle: 136, min_angle: -139, max_speed: 90, type: dynamixel},
+ arm_wrist_flex_joint: {id: 4, max_angle: 96, min_angle: -98, max_speed: 90, type: dynamixel},
+ gripper_joint: {id: 5, max_angle: 0, min_angle: -145, max_speed: 90, type: prismatic, radius: .0078, connector: .024, offset: .016}
+}
+controllers: {
+ arm_controller: {type: follow_controller, joints: [arm_shoulder_pan_joint, arm_shoulder_lift_joint, arm_elbow_flex_joint, arm_wrist_flex_joint], action_name: arm_controller/follow_joint_trajectory, onboard: False }
+}

turtlebot_arm_bringup/config/pincher_gripper.yaml

@@ -0,0 +1,10 @@
+source: pincher_gripper.yaml
+model: parallel
+invert: false
+pad_width: 0.002
+min_opening: 0.002
+max_opening: 0.031
+neutral: 0.015
+tighten: .001
+center: 0
+joint: gripper_joint

turtlebot_arm_bringup/config/arm.yaml → turtlebot_arm_bringup/config/turtlebot_arm.yaml

@@ -1,12 +1,13 @@
-port: /dev/ttyUSB1
+source: turtlebot_arm.yaml
+port: /dev/ttyUSB0
 read_rate: 15
 write_rate: 25
 joints: {
- arm_shoulder_pan_joint: {id: 1, neutral: 205, max_angle: 180, min_angle: -60, max_speed: 90},
- arm_shoulder_lift_joint: {id: 2, max_angle: 150, min_angle: -150, max_speed: 90},
- arm_elbow_flex_joint: {id: 3, max_angle: 150, min_angle: -150, max_speed: 90},
- arm_wrist_flex_joint: {id: 4, max_angle: 100, min_angle: -100, max_speed: 90},
- gripper_joint: {id: 5, max_speed: 90},
+ arm_shoulder_pan_joint: {id: 1, neutral: 205, max_angle: 180, min_angle: -60, max_speed: 90, type: dynamixel},
+ arm_shoulder_lift_joint: {id: 2, max_angle: 150, min_angle: -150, max_speed: 90, type: dynamixel},
+ arm_elbow_flex_joint: {id: 3, max_angle: 150, min_angle: -150, max_speed: 90, type: dynamixel},
+ arm_wrist_flex_joint: {id: 4, max_angle: 100, min_angle: -100, max_speed: 90, type: dynamixel},
+ gripper_joint: {id: 5, max_speed: 90, type: dynamixel},
 }
 controllers: {
  arm_controller: {type: follow_controller, joints: [arm_shoulder_pan_joint, arm_shoulder_lift_joint, arm_elbow_flex_joint, arm_wrist_flex_joint], action_name: arm_controller/follow_joint_trajectory, onboard: False }

turtlebot_arm_bringup/config/turtlebot_gripper.yaml

@@ -0,0 +1,10 @@
+source: turtlebot_gripper.yaml
+model: singlesided
+invert: true
+pad_width: 0.0025
+min_opening: 0.0
+max_opening: 0.054
+neutral: 0.028
+tighten: 0.002
+center : 0.07
+finger_length : 0.03

turtlebot_arm_bringup/launch/arm.launch

@@ -1,27 +1,21 @@
 <launch>
+ <!-- To use, first set Environment variable TURTLEBOT_ARM1 to either:
+ turtlebot or pincher (for Trossen PhantomX Pincher) 
+ NOTE: passing arm_type as argument NOT yet fully supported! -->
+
  <arg name="simulation" default="false"/>
+ <arg name="arm_type" default="$(optenv TURTLEBOT_ARM1 turtlebot)"/>
 
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_arm_description)/urdf/arm.urdf.xacro'"/>
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_arm_description)/urdf/$(arg arm_type)_arm.urdf.xacro'"/>
  <node name="robot_state_pub" pkg="robot_state_publisher" type="robot_state_publisher"/>
  <node name="fake_joint_pub" pkg="turtlebot_arm_bringup" type="fake_joint_pub.py"/>
 
  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
- <rosparam file="$(find turtlebot_arm_bringup)/config/arm.yaml" command="load"/>
+ <rosparam file="$(find turtlebot_arm_bringup)/config/$(arg arm_type)_arm.yaml" command="load"/>
  <param name="sim" value="$(arg simulation)"/>
  </node>
 
- <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller">
- <!--
- Gripper controller parameters:
- more than the physical properties of the gripper, these values are an ad-hoc configuration
- that works with arbotix_ros/gripper_controller/OneSideGripperModel
- -->
- <param name="model" value="singlesided"/>
- <param name="invert" value="true"/>
- <param name="center" value="0.07"/>
- <param name="pad_width" value="0.025"/>
- <param name="finger_length" value="0.03"/>
- <param name="min_opening" value="0.0"/>
- <param name="max_opening" value="0.054"/>
+ <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller" output="screen">
+ <rosparam file="$(find turtlebot_arm_bringup)/config/$(arg arm_type)_gripper.yaml" command="load" />
  </node>
 </launch>

turtlebot_arm_description/urdf/arm_hardware.xacro

@@ -1,4 +1,5 @@
 <?xml version="1.0"?>
+<!-- Basic URDF components for Turtlebot Arm and PhantomX Pincher Arm-->
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:property name="M_PI" value="3.14159"/>
  <xacro:property name="M_SCALE" value="0.001"/>
@@ -7,6 +8,7 @@
  <xacro:property name="F3_HEIGHT" value="0.009"/>
  <xacro:property name="AX12_HEIGHT" value="0.0385"/>
  <xacro:property name="AX12_WIDTH" value="0.038"/>
+ <xacro:property name="AX12_LENGTH" value="0.050"/>
  <xacro:property name="F2_HEIGHT" value="0.0265"/>
 
  <xacro:macro name="arm_mount" params="parent name color *origin">
@@ -67,7 +69,7 @@
  <visual>
  <origin xyz=" 0 0 0 " rpy="0 0 0"/>
  <geometry>
- <mesh filename="package://turtlebot_arm_description/meshes/finger.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
+ <mesh filename="package://turtlebot_arm_description/meshes/turtlebot_finger.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
  </geometry>
  <material name="${color}"/>
  </visual>
@@ -108,7 +110,7 @@
  <geometry>
  <mesh filename="package://turtlebot_arm_description/meshes/ax12_box.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
  </geometry>
- <material name="black"/>
+ <material name="Black"/>
  </visual>
 
  <collision>
@@ -329,5 +331,73 @@
  <gravity>true</gravity>
  </gazebo>
  </xacro:macro>
-
+
+ <xacro:macro name="gripper_finger_base" params="parent name color *origin">
+ <joint name="${name}_joint" type="fixed">
+ <insert_block name="origin"/>
+ <parent link="${parent}"/>
+ <child link="${name}_link"/>
+ </joint>
+
+ <link name="${name}_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.016 0 -.015 " rpy="${M_PI} ${-M_PI/2} ${M_PI/2}"/>
+ <geometry>
+ <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger_base.stl" scale=".01 .01 .01"/>
+ </geometry>
+ <material name="${color}"/>
+ </visual>
+
+ <collision>
+ <origin xyz="0.015 0 -0.0135" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.002 0.040 0.075"/>
+ </geometry>
+ </collision>
+ </link >
+
+ <gazebo reference="${name}_link">
+ <material>Gazebo/${color}</material>
+ <selfCollide>true</selfCollide>
+ <gravity>true</gravity>
+ </gazebo>
+ </xacro:macro>
+
+ <xacro:macro name="pincher_gripper" params="name color *origin">
+ <link name="${name}_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>
+ <insert_block name="origin"/>
+ <geometry>
+ <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger.stl" scale=".01 .01 .01"/>
+ </geometry>
+ <material name="${color}"/>
+ </visual>
+ <collision>
+ <insert_block name="origin"/>
+ <geometry>
+ <box size="0.036 0.028 0.004"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="${name}_link">
+ <material>Gazebo/${color}</material>
+ <selfCollide>true</selfCollide>
+ <gravity>true</gravity>
+ </gazebo>
+ </xacro:macro>
 </robot>