From 5a92f9b7bc0dbb1bf471dd780ae9444d70421547 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Fri, 14 Jul 2023 14:00:28 -0400
Subject: [PATCH 1/5] Fix Inspect Surface in Gazebo config

---
 .../objectives/inspect_surface.xml             | 18 ------------------
 1 file changed, 18 deletions(-)
 delete mode 100644 src/picknik_ur_gazebo_config/objectives/inspect_surface.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml b/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml
deleted file mode 100644
index 0c4769a6..00000000
--- a/src/picknik_ur_gazebo_config/objectives/inspect_surface.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-<?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Inspect Surface">
-    <BehaviorTree ID="Inspect Surface" _description="Move the wrist camera to point towards a selected surface" _favorite="true">
-        <Control ID="Sequence">
-            <Action ID="InitializeMTCTask" task_id="inspect_surface" controller_names="/joint_trajectory_controller" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCCurrentState" task="{move_to_pose_task}"/>
-            <Action ID="SetupMTCMoveToPose" ik_frame="manual_grasp_link" planning_group_name="manipulator" target_pose="{inspect_surface.target_pose}" task="{move_to_pose_task}"/>
-            <Action ID="PlanMTCTask" solution="{move_to_pose_solution}" task="{move_to_pose_task}"/>
-            <Fallback name="wait_for_approval_if_user_available">
-                <Inverter>
-                    <Action ID="IsUserAvailable" />
-                </Inverter>
-                <Action ID="WaitForUserTrajectoryApproval" solution="{move_to_pose_solution}" />
-            </Fallback>
-            <Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
-        </Control>
-    </BehaviorTree>
-</root>

From 5590c5b8e28119333fab2d82a9954ef62f68c2ca Mon Sep 17 00:00:00 2001
From: Erik Holum <erik.holum@picknik.ai>
Date: Fri, 14 Jul 2023 13:41:25 -0600
Subject: [PATCH 2/5] Also remove the inspect surface objective

---
 .../objectives/take_snapshot.xml                      | 11 -----------
 1 file changed, 11 deletions(-)
 delete mode 100644 src/picknik_ur_gazebo_config/objectives/take_snapshot.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml b/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml
deleted file mode 100644
index 1c843877..00000000
--- a/src/picknik_ur_gazebo_config/objectives/take_snapshot.xml
+++ /dev/null
@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-  <root BTCPP_format="4" main_tree_to_execute="Take Snapshot">
-    <!-- ////////// -->
-    <BehaviorTree ID="Take Snapshot" _description="Take a point cloud snapshot and add to collision scene" _favorite="true">
-        <Control ID="Sequence">
-            <Action ID="GetPointCloud" target_point_cloud_topic="/wrist_mounted_camera/depth/color/points" uuid="{take_snapshot.uuid}" point_cloud="{point_cloud}" point_cloud_uuid="{request_uuid}" />
-            <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
-            <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" pcd_topic="/pcd_pointcloud_captures" point_cloud_uuid="{request_uuid}"/>
-        </Control>
-    </BehaviorTree>
-</root>

From ed669b71792df297ac914b8343a25d76d5427fd5 Mon Sep 17 00:00:00 2001
From: Erik Holum <erik.holum@picknik.ai>
Date: Fri, 14 Jul 2023 14:10:22 -0600
Subject: [PATCH 3/5] Favorite joint diagnostics, unfavorite waypoint cycles

---
 .../objectives/cycle_between_waypoints.xml                      | 2 +-
 src/picknik_ur_base_config/objectives/joint_diagnostic.xml      | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
index a67d4dbe..1e9c45bd 100644
--- a/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
+++ b/src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
   <root BTCPP_format="4" main_tree_to_execute="Cycle Between Waypoints">
-    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure" _favorite="true">
+    <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
                 <Action ID="MoveToJointState" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
diff --git a/src/picknik_ur_base_config/objectives/joint_diagnostic.xml b/src/picknik_ur_base_config/objectives/joint_diagnostic.xml
index c8105f5f..bb3783e4 100644
--- a/src/picknik_ur_base_config/objectives/joint_diagnostic.xml
+++ b/src/picknik_ur_base_config/objectives/joint_diagnostic.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
   <!-- ////////// -->
-  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly">
+  <BehaviorTree ID="Joint Diagnostic" _description="Move to a known pose and cycle through the min and max limits of a joint repeatedly" _favorite="true">
     <Control ID="Sequence" name="TopLevelSequence">
       <Action ID="MoveToJointState" waypoint_name="Home" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
       <Decorator ID="KeepRunningUntilFailure">

From b7cb6edca177e351a5a02f61b6144829c976cce7 Mon Sep 17 00:00:00 2001
From: Erik Holum <erik.holum@picknik.ai>
Date: Fri, 14 Jul 2023 14:26:42 -0600
Subject: [PATCH 4/5] Cleanup default start position

---
 src/picknik_ur_base_config/config/initial_positions.yaml  | 8 ++++----
 .../config/initial_positions.yaml                         | 6 ------
 2 files changed, 4 insertions(+), 10 deletions(-)
 delete mode 100644 src/picknik_ur_gazebo_config/config/initial_positions.yaml

diff --git a/src/picknik_ur_base_config/config/initial_positions.yaml b/src/picknik_ur_base_config/config/initial_positions.yaml
index ace7747c..0d8b33f7 100644
--- a/src/picknik_ur_base_config/config/initial_positions.yaml
+++ b/src/picknik_ur_base_config/config/initial_positions.yaml
@@ -1,6 +1,6 @@
 shoulder_pan_joint: 0.0
-shoulder_lift_joint: -0.6
-elbow_joint: -1.4
-wrist_1_joint: -1.2
-wrist_2_joint: 1.7
+shoulder_lift_joint: -1.41
+elbow_joint: -0.7
+wrist_1_joint: -2.12
+wrist_2_joint: 1.67
 wrist_3_joint: 0.0
diff --git a/src/picknik_ur_gazebo_config/config/initial_positions.yaml b/src/picknik_ur_gazebo_config/config/initial_positions.yaml
deleted file mode 100644
index ace7747c..00000000
--- a/src/picknik_ur_gazebo_config/config/initial_positions.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-shoulder_pan_joint: 0.0
-shoulder_lift_joint: -0.6
-elbow_joint: -1.4
-wrist_1_joint: -1.2
-wrist_2_joint: 1.7
-wrist_3_joint: 0.0

From dafed1eeb49ef5c813a61cd209a22840d1d7f74e Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Fri, 14 Jul 2023 18:01:53 -0400
Subject: [PATCH 5/5] Fix looping pick and place and Gazebo Look at Pick and
 Place waypoint

---
 .../objectives/looping_pick_and_place_object.xml       |  8 ++++----
 src/picknik_ur_gazebo_config/waypoints/waypoints.yaml  | 10 +++++-----
 2 files changed, 9 insertions(+), 9 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
index 6e484cf4..6ea0fdcd 100644
--- 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 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,14 +69,14 @@
                                     </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>
@@ -84,7 +84,7 @@
                     <!-- 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>
diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
index c9850ed5..97ea7869 100644
--- a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
+++ b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -165,11 +165,11 @@
       - wrist_2_joint
       - wrist_3_joint
     position:
-      - 3.03
-      - -0.78
-      - -0.51
-      - -2.39
-      - 1.65
+      - 0
+      - -1.41
+      - -0.7
+      - -2.12
+      - 1.67
       - 0
     velocity: []
   name: Look at Pick and Place Zone