Cherry-Pick Fixes from v2.2 into Main (#44)

* Rename Behind Waypoint to Look at Table (#38) * Fix looping pick n place (#41) * Update ur5e_waypoints.yaml * Update looping_pick_and_place_object.xml * Update waypoints.yaml * Update pick_place_object.xml (#42) --------- Co-authored-by: Cassidy Elliott <cassidy@picknik.ai>
PickNikRobotics · Jun 21, 2023 · 1ae02ce · 1ae02ce
1 parent 65b14d7
commit 1ae02ce
Show file tree

Hide file tree

Showing 6 changed files with 20 additions and 56 deletions.
diff --git a/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml b/src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml
@@ -10,7 +10,7 @@
 
             <!-- Open the gripper and move to a consistent state. -->
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-            <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
             <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -69,22 +69,22 @@
                                     </Control>
                                 </Decorator>
                                 <!-- Move home. -->
-                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
 
                                 <!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. -->
                                 <Action ID="MoveToJointState" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                                 <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
 
                                 <!-- Finally, return home. -->
-                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                             </Control>
                         </Decorator>
                     </Decorator>
 
                     <!-- TryGraspOuter fallback condition. Reset scene objects, move home, and then wait a short while before triggering a retry. -->
                     <Control ID="Sequence">
                         <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-                        <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="WaitForDuration" delay_duration="5"/>
                         <Action ID="AlwaysSuccess"/>
                     </Control>

diff --git a/src/picknik_ur_base_config/objectives/pick_place_object.xml b/src/picknik_ur_base_config/objectives/pick_place_object.xml
@@ -95,17 +95,17 @@
                     </Control>
                 </Decorator>
 
-                <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return home. -->
+                <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Table pose. -->
                 <Control ID="Sequence">
                     <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/>
-                    <Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                    <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                 </Control>
 
-                <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Home pose if all retry attempts failed. -->
+                <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Table pose if all retry attempts failed. -->
                 <Control ID="Sequence">
                     <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-                    <Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                    <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     <Action ID="AlwaysFailure"/>
                 </Control>
             </Control>

diff --git a/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml b/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml
@@ -11,7 +11,7 @@
     position: [2.7712432925819273, -1.4531914890259676, -1.96615025453015, -1.0617789845470658, 1.5878968566255958, -0.07080028000697597]
     velocity: []
     effort: []
-- name: Behind
+- name: Look at Table
   favorite: true
   joint_state:
     header:
@@ -159,17 +159,6 @@
     position: [-0.3130109945880335, -2.2752829990782679, -0.99421918392181396, -1.4464575362256546, 1.6087517738342285, 0.3333250880241394]
     velocity: []
     effort: []
-- name: Look at Pick and Place Zone
-  joint_state:
-    header:
-      frame_id: world
-      stamp:
-        sec: 0
-        nanosec: 0
-    name: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
-    position: [-0.51890546480287725, -1.6439539394774378, -0.50593507289886475, -2.1812287769713343, 1.8930472135543823, -0.48202354112734014]
-    velocity: []
-    effort: []
 - name: Place Block
   joint_state:
     header:

diff --git a/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,7 +5,7 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -14,19 +14,19 @@
                     <Control ID="Sequence">
                         <Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
                     <Control ID="Sequence">
                         <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveToJointState" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>
                 </Control>
             </Decorator>

diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -147,32 +147,7 @@
       - 1.65
       - 0
     velocity: []
-  name: Behind
-- description: ''
-  favorite: false
-  joint_state:
-    effort: []
-    header:
-      frame_id: world
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    position:
-      - -0.5189054648028772
-      - -1.6439539394774378
-      - -0.5059350728988647
-      - -2.1812287769713343
-      - 1.8930472135543823
-      - -0.48202354112734014
-    velocity: []
-  name: Look at Pick and Place Zone
+  name: Look at Table
 - description: ''
   favorite: false
   joint_state:

diff --git a/src/picknik_ur_site_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_site_config/objectives/3_waypoint_pick_and_place.xml
@@ -5,7 +5,7 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
@@ -14,19 +14,19 @@
                     <Control ID="Sequence">
                         <Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
                     <Control ID="Sequence">
                         <Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>
                 </Control>
             </Decorator>