From 86aa8ee8549b5ab3f76e9965c862b8b07ab230f0 Mon Sep 17 00:00:00 2001
From: Nathan Brooks <nathanbrooks@picknik.ai>
Date: Mon, 17 Jul 2023 22:39:10 -0600
Subject: [PATCH 1/2] Unfavorite objectives sales team should not run, add some
 interesting Waypoints and remove some less interesting/redundant ones.

---
 .../objectives/push_button.xml                |  2 +-
 .../objectives/3_waypoint_pick_and_place.xml  |  2 +-
 .../objectives/open_door_affordance.xml       |  2 +-
 .../waypoints/waypoints.yaml                  | 56 ++++++++++++++++++-
 4 files changed, 56 insertions(+), 6 deletions(-)

diff --git a/src/picknik_ur_base_config/objectives/push_button.xml b/src/picknik_ur_base_config/objectives/push_button.xml
index 337e0ce6..7c0dcd75 100644
--- a/src/picknik_ur_base_config/objectives/push_button.xml
+++ b/src/picknik_ur_base_config/objectives/push_button.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Push Button">
-    <BehaviorTree ID="Push Button" _description="Push a button with the end effector" _favorite="true">
+    <BehaviorTree ID="Push Button" _description="Push a button with the end effector">
         <Control ID="Sequence">
             <Action ID="LoadObjectiveParameters" config_file_name="push_along_axis_config.yaml" parameters="{parameters}" />
             <SubTree ID="CloseGripper" />
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
index 91565e83..b1f155a2 100644
--- 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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
     <!-- ////////// -->
-    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly pick up a small object, place it at desired destination, and then navigate to specified waypoint until failure" _favorite="true">
+    <BehaviorTree ID="3 Waypoints Pick and Place" _description="Repeatedly pick up a small object, place it at desired destination, and then navigate to specified waypoint until failure">
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
diff --git a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
index a1ec0b8f..5828dbb0 100644
--- a/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
+++ b/src/picknik_ur_gazebo_config/objectives/open_door_affordance.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <root BTCPP_format="4" main_tree_to_execute="Open Cabinet Door">
-    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle" _favorite="true">
+    <BehaviorTree ID="Open Cabinet Door" _description="Open a cabinet door by grasping and pulling its handle">
         <Control ID="Sequence" name="Setup">
             <Action ID="LoadObjectiveParameters" config_file_name="open_door_affordance_config.yaml" parameters="{parameters}" />
             <Action ID="GetPoseFromUser" parameter_name="process_door_selection.hinge_axis_pose_end" parameter_value="{hinge_axis_pose_end}" />
diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
index 97ea7869..755e224b 100644
--- a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
+++ b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -49,7 +49,7 @@
     velocity: []
   name: Home
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -74,7 +74,7 @@
     velocity: []
   name: Left Corner
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -99,7 +99,7 @@
     velocity: []
   name: Right Corner
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header:
@@ -298,3 +298,53 @@
       - 2.282092208042741e-05
     velocity: []
   name: Wrist 2 Max
+- description: Move here and run Take Snapshot again!
+  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:
+    - -1.5113688855545728
+    - -0.849676772994894
+    - -1.7900954114547076
+    - -0.6712599508612579
+    - 1.6807113853283657
+    - -0.010627009718118157
+    velocity: []
+  name: Right Wall
+- 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:
+    - 0.0003205429823013286
+    - -0.3419419371606998
+    - -0.5134107235570269
+    - -2.3994585463037112
+    - 1.6501763241696035
+    - 3.878003640148636e-05
+    velocity: []
+  name: Monitor View

From 424a9be3feced0fa7d72bc4a3b8db025d584028c Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Tue, 18 Jul 2023 07:14:48 -0400
Subject: [PATCH 2/2] Fix waypoint indentation

---
 .../waypoints/waypoints.yaml                  | 48 +++++++++----------
 1 file changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
index 755e224b..fb06cc2e 100644
--- a/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
+++ b/src/picknik_ur_gazebo_config/waypoints/waypoints.yaml
@@ -308,19 +308,19 @@
         nanosec: 0
         sec: 0
     name:
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
     position:
-    - -1.5113688855545728
-    - -0.849676772994894
-    - -1.7900954114547076
-    - -0.6712599508612579
-    - 1.6807113853283657
-    - -0.010627009718118157
+      - -1.5113688855545728
+      - -0.849676772994894
+      - -1.7900954114547076
+      - -0.6712599508612579
+      - 1.6807113853283657
+      - -0.010627009718118157
     velocity: []
   name: Right Wall
 - description: ''
@@ -333,18 +333,18 @@
         nanosec: 0
         sec: 0
     name:
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
     position:
-    - 0.0003205429823013286
-    - -0.3419419371606998
-    - -0.5134107235570269
-    - -2.3994585463037112
-    - 1.6501763241696035
-    - 3.878003640148636e-05
+      - 0.0003205429823013286
+      - -0.3419419371606998
+      - -0.5134107235570269
+      - -2.3994585463037112
+      - 1.6501763241696035
+      - 3.878003640148636e-05
     velocity: []
   name: Monitor View