diff --git a/README.md b/README.md
index 826d9a0..5ef445d 100644
--- a/README.md
+++ b/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:
-
+
diff --git a/turtlebot_arm_block_manipulation/launch/block_manipulation.launch b/turtlebot_arm_block_manipulation/launch/block_manipulation.launch
index 7a49899..8b60515 100644
--- a/turtlebot_arm_block_manipulation/launch/block_manipulation.launch
+++ b/turtlebot_arm_block_manipulation/launch/block_manipulation.launch
@@ -1,5 +1,5 @@
-
+
diff --git a/turtlebot_arm_bringup/config/pincher_arm.yaml b/turtlebot_arm_bringup/config/pincher_arm.yaml
new file mode 100644
index 0000000..a7d1f1e
--- /dev/null
+++ b/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 }
+}
diff --git a/turtlebot_arm_bringup/config/pincher_gripper.yaml b/turtlebot_arm_bringup/config/pincher_gripper.yaml
new file mode 100644
index 0000000..6eb7efc
--- /dev/null
+++ b/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
diff --git a/turtlebot_arm_bringup/config/arm.yaml b/turtlebot_arm_bringup/config/turtlebot_arm.yaml
similarity index 66%
rename from turtlebot_arm_bringup/config/arm.yaml
rename to turtlebot_arm_bringup/config/turtlebot_arm.yaml
index a4a8499..e181173 100644
--- a/turtlebot_arm_bringup/config/arm.yaml
+++ b/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 }
diff --git a/turtlebot_arm_bringup/config/turtlebot_gripper.yaml b/turtlebot_arm_bringup/config/turtlebot_gripper.yaml
new file mode 100644
index 0000000..058431d
--- /dev/null
+++ b/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
diff --git a/turtlebot_arm_bringup/launch/arm.launch b/turtlebot_arm_bringup/launch/arm.launch
index 6b9dfbe..4333927 100644
--- a/turtlebot_arm_bringup/launch/arm.launch
+++ b/turtlebot_arm_bringup/launch/arm.launch
@@ -1,27 +1,21 @@
+
+
+
-
+
-
+
-
-
-
-
-
-
-
-
-
+
+
diff --git a/turtlebot_arm_description/meshes/pincher_finger.stl b/turtlebot_arm_description/meshes/pincher_finger.stl
new file mode 100644
index 0000000..11db3d5
Binary files /dev/null and b/turtlebot_arm_description/meshes/pincher_finger.stl differ
diff --git a/turtlebot_arm_description/meshes/pincher_finger_base.stl b/turtlebot_arm_description/meshes/pincher_finger_base.stl
new file mode 100644
index 0000000..1050f22
Binary files /dev/null and b/turtlebot_arm_description/meshes/pincher_finger_base.stl differ
diff --git a/turtlebot_arm_description/meshes/finger.stl b/turtlebot_arm_description/meshes/turtlebot_finger.stl
similarity index 100%
rename from turtlebot_arm_description/meshes/finger.stl
rename to turtlebot_arm_description/meshes/turtlebot_finger.stl
diff --git a/turtlebot_arm_description/urdf/arm.urdf.xacro b/turtlebot_arm_description/urdf/arm.urdf.xacro
deleted file mode 100644
index 4b7ee2f..0000000
--- a/turtlebot_arm_description/urdf/arm.urdf.xacro
+++ /dev/null
@@ -1,20 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/turtlebot_arm_description/urdf/arm_hardware.xacro b/turtlebot_arm_description/urdf/arm_hardware.xacro
index a6307d2..5d9e678 100644
--- a/turtlebot_arm_description/urdf/arm_hardware.xacro
+++ b/turtlebot_arm_description/urdf/arm_hardware.xacro
@@ -1,4 +1,5 @@
+
@@ -7,6 +8,7 @@
+
@@ -67,7 +69,7 @@
-
+
@@ -108,7 +110,7 @@
-
+
@@ -329,5 +331,73 @@
true
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/${color}
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/${color}
+ true
+ true
+
+
diff --git a/turtlebot_arm_description/urdf/pincher_arm.urdf b/turtlebot_arm_description/urdf/pincher_arm.urdf
new file mode 100644
index 0000000..23d3452
--- /dev/null
+++ b/turtlebot_arm_description/urdf/pincher_arm.urdf
@@ -0,0 +1,685 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Gray
+ true
+ true
+
+
+
diff --git a/turtlebot_arm_description/urdf/pincher_arm.urdf.xacro b/turtlebot_arm_description/urdf/pincher_arm.urdf.xacro
new file mode 100644
index 0000000..5d7fc2c
--- /dev/null
+++ b/turtlebot_arm_description/urdf/pincher_arm.urdf.xacro
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_description/urdf/pincher_gripper.xacro b/turtlebot_arm_description/urdf/pincher_gripper.xacro
new file mode 100644
index 0000000..57adae1
--- /dev/null
+++ b/turtlebot_arm_description/urdf/pincher_gripper.xacro
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_description/urdf/turtlebot_arm.urdf b/turtlebot_arm_description/urdf/turtlebot_arm.urdf
new file mode 100644
index 0000000..51bf889
--- /dev/null
+++ b/turtlebot_arm_description/urdf/turtlebot_arm.urdf
@@ -0,0 +1,710 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Green
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/White
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Green
+ true
+ true
+
+
+
diff --git a/turtlebot_arm_description/urdf/turtlebot_arm.urdf.xacro b/turtlebot_arm_description/urdf/turtlebot_arm.urdf.xacro
new file mode 100644
index 0000000..dc1492d
--- /dev/null
+++ b/turtlebot_arm_description/urdf/turtlebot_arm.urdf.xacro
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_description/urdf/arm.xacro b/turtlebot_arm_description/urdf/turtlebot_arm.xacro
similarity index 63%
rename from turtlebot_arm_description/urdf/arm.xacro
rename to turtlebot_arm_description/urdf/turtlebot_arm.xacro
index 8e594fe..7952528 100644
--- a/turtlebot_arm_description/urdf/arm.xacro
+++ b/turtlebot_arm_description/urdf/turtlebot_arm.xacro
@@ -1,5 +1,5 @@
-
+
@@ -7,14 +7,16 @@
-
+
+
+
+
-
+
@@ -48,7 +50,7 @@
+ vlimit="${joints_vlimit}" llimit="${shoulder_llimit}" ulimit="${shoulder_ulimit}">
@@ -69,7 +71,7 @@
+ vlimit="${joints_vlimit}" llimit="${elbow_llimit}" ulimit="${elbow_ulimit}">
@@ -90,59 +92,17 @@
+ vlimit="${joints_vlimit}" llimit="${wrist_llimit}" ulimit="${wrist_ulimit}">
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_description/urdf/turtlebot_gripper.xacro b/turtlebot_arm_description/urdf/turtlebot_gripper.xacro
new file mode 100644
index 0000000..9070a59
--- /dev/null
+++ b/turtlebot_arm_description/urdf/turtlebot_gripper.xacro
@@ -0,0 +1,53 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_moveit_config/config/pincher_arm.srdf b/turtlebot_arm_moveit_config/config/pincher_arm.srdf
new file mode 100644
index 0000000..b208898
--- /dev/null
+++ b/turtlebot_arm_moveit_config/config/pincher_arm.srdf
@@ -0,0 +1,261 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/turtlebot_arm_moveit_config/launch/planning_context.launch b/turtlebot_arm_moveit_config/launch/planning_context.launch
index e985a79..7c3a9d2 100644
--- a/turtlebot_arm_moveit_config/launch/planning_context.launch
+++ b/turtlebot_arm_moveit_config/launch/planning_context.launch
@@ -4,13 +4,15 @@
+
+
+ command="$(find xacro)/xacro.py '$(find turtlebot_arm_description)/urdf/$(arg arm_type)_arm.urdf.xacro'"/>
-
+
diff --git a/turtlebot_arm_moveit_config/launch/turtlebot_arm_moveit.launch b/turtlebot_arm_moveit_config/launch/turtlebot_arm_moveit.launch
index d083902..d827bc2 100644
--- a/turtlebot_arm_moveit_config/launch/turtlebot_arm_moveit.launch
+++ b/turtlebot_arm_moveit_config/launch/turtlebot_arm_moveit.launch
@@ -8,10 +8,14 @@
+
+
+
+
diff --git a/turtlebot_arm_moveit_demos/bin/pick_and_place.py b/turtlebot_arm_moveit_demos/bin/pick_and_place.py
index 9a1ca34..d5e7fe3 100755
--- a/turtlebot_arm_moveit_demos/bin/pick_and_place.py
+++ b/turtlebot_arm_moveit_demos/bin/pick_and_place.py
@@ -6,6 +6,10 @@
Command the gripper to grasp a target object and move it to a new
location, all while avoiding simulated obstacles.
+ Before running, set environment variable TURTLEBOT_ARM1 to either:
+ turtlebot - for original turtlebot arm
+ pincher - for PhantomX Pincher arm
+
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2014 Patrick Goebel. All rights reserved.
@@ -40,15 +44,9 @@
GROUP_NAME_GRIPPER = 'gripper'
GRIPPER_FRAME = 'gripper_link'
-
-GRIPPER_OPENED = [0.053]
-GRIPPER_CLOSED = [0.001]
-GRIPPER_NEUTRAL = [0.028]
-GRASP_OVERTIGHTEN = 0.002
-
GRIPPER_JOINT_NAMES = ['gripper_joint']
-
GRIPPER_EFFORT = [1.0]
+GRIPPER_PARAM = '/gripper_controller'
REFERENCE_FRAME = '/base_link'
ARM_BASE_FRAME = '/arm_base_link'
@@ -59,6 +57,12 @@ def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
+
+ self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") ]
+ self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") ]
+ self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral") ]
+
+ self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
@@ -67,10 +71,10 @@ def __init__(self):
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
- self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
+ self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
- self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped)
+ self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
@@ -102,6 +106,7 @@ def __init__(self):
# Set a limit on the number of place attempts
max_place_attempts = 3
+ rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling')))
# Give the scene a chance to catch up
rospy.sleep(2)
@@ -127,15 +132,33 @@ def __init__(self):
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
+ rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
- arm.go()
-
- # Open the gripper to the neutral position
- gripper.set_joint_value_target(GRIPPER_NEUTRAL)
- gripper.go()
+ if arm.go() != True:
+ rospy.logwarn(" Go failed")
+ rospy.sleep(2)
- rospy.sleep(1)
+ # Move the gripper to the closed position
+ rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) )
+ gripper.set_joint_value_target(self.gripper_closed)
+ if gripper.go() != True:
+ rospy.logwarn(" Go failed")
+ rospy.sleep(2)
+
+ # Move the gripper to the neutral position
+ rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) )
+ gripper.set_joint_value_target(self.gripper_neutral)
+ if gripper.go() != True:
+ rospy.logwarn(" Go failed")
+ rospy.sleep(2)
+ # Move the gripper to the open position
+ rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
+ gripper.set_joint_value_target(self.gripper_opened)
+ if gripper.go() != True:
+ rospy.logwarn(" Go failed")
+ rospy.sleep(2)
+
# Set the height of the table off the ground
table_ground = 0.4
@@ -212,7 +235,7 @@ def __init__(self):
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
- grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - GRASP_OVERTIGHTEN])
+ grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
@@ -220,6 +243,7 @@ def __init__(self):
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
+ rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
@@ -230,11 +254,11 @@ def __init__(self):
break
n_attempts += 1
- rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
+ rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
@@ -243,6 +267,7 @@ def __init__(self):
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
+ rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
@@ -253,22 +278,24 @@ def __init__(self):
break
n_attempts += 1
- rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
+ else:
+ rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
+
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
- gripper.set_joint_value_target(GRIPPER_NEUTRAL)
+ gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
@@ -330,7 +357,7 @@ def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
- g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPENED)
+ g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired