Fix broken objectives

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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+            <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" 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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" 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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                                <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" 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 Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <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="WaitForDuration" delay_duration="5"/>
                         <Action ID="AlwaysSuccess"/>
                     </Control>

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 to the Look at Table pose. -->
+                <!-- The "Then" condition of the TryGraspOuter IfThenElse control. After successful grasp, move to dropoff pose and release object, then return to the Look at Pick and Place Zone 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="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                    <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                 </Control>
 
-                <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Table pose if all retry attempts failed. -->
+                <!-- The "Else" condition of the TryGraspOuter IfThenElse control. Move to the Look at Pick and Place Zone pose if all retry attempts failed. -->
                 <Control ID="Sequence">
                     <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
-                    <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="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     <Action ID="AlwaysFailure"/>
                 </Control>
             </Control>

src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml

@@ -23,6 +23,18 @@
     position: [3.03, -0.78, -0.51, -2.39, 1.65, 0]
     velocity: []
     effort: []
+- name: Look at Pick and Place Zone
+  favorite: true
+  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: [3.03, -0.78, -0.51, -2.39, 1.65, 0]
+    velocity: []
+    effort: []
 - name: Forward Down
   favorite: true
   joint_state:

src/picknik_ur_gazebo_config/waypoints/waypoints.yaml

@@ -148,6 +148,31 @@
       - 0
     velocity: []
   name: Look at Table
+- description: ''
+  favorite: true
+  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:
+      - 3.03
+      - -0.78
+      - -0.51
+      - -2.39
+      - 1.65
+      - 0
+    velocity: []
+  name: Look at Pick and Place Zone
 - description: ''
   favorite: false
   joint_state: