From 65eb863c72201cbbab0d9dcf200ddfa1d64e079c Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Fri, 15 Sep 2023 12:31:18 -0600
Subject: [PATCH 1/6] Add objectives to teach and pick objects relitive to
 AprilTag pose observations

---
 .../objectives/pick_apriltag_labeled_object.xml     |  7 +++----
 .../objectives/teach_apriltag_grasp_offset.xml      | 13 +++++++++++++
 2 files changed, 16 insertions(+), 4 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index 82090e3a..4bd58fac 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -8,12 +8,11 @@
         <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
       <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>
       <Control ID="Sequence" name="GraspObject">
         <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
-        <Action ID="TransformPose" input_pose="{tag_pose_world}" translation_xyz="-0.1;0.0;0.05" quaternion_xyzw="-0.707;0.707;0.0;0.0" output_pose="{grasp_pose}"/>
-        <Action ID="MoveToPose" target_pose="{grasp_pose}" ik_frame="manual_grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-        <SubTree ID="Close Gripper"/>
-        <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+        <Action ID="TransformPoseFromYaml" input_pose="{tag_pose_world}" pose_parameters="{pose_parameters}" parameter_namespace="GraspOffset" output_pose="{grasp_pose}"/>
+        <SubTree ID="PickObject" grasp_pose="{grasp_pose}"/>
       </Control>
     </Control>
   </BehaviorTree>
diff --git a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
new file mode 100644
index 00000000..edf5597a
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Calculate Offset From Apriltag">
+    <BehaviorTree ID="Calculate Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker">
+        <Control ID="Sequence" name="root">
+            <Action ID="TakeSnapshot" />
+            <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
+            <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
+            <Action ID="CalculateTransform" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+            <Action ID="WritePoseToYAML" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" pose="{tag_to_user_grasp_pose}"/>
+        </Control>
+    </BehaviorTree>
+</root>

From 916c7b23e43c0281c36574d3db19b6f63c9705cf Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Tue, 26 Sep 2023 09:58:57 -0600
Subject: [PATCH 2/6] update behavior names

---
 .../objectives/apriltag1_grasp_offset.yaml                | 8 ++++++++
 .../objectives/loop_detect_apriltag.xml                   | 2 +-
 .../objectives/pick_apriltag_labeled_object.xml           | 2 +-
 .../objectives/sample_april_tag.xml                       | 2 +-
 .../objectives/teach_apriltag_grasp_offset.xml            | 7 +++----
 5 files changed, 14 insertions(+), 7 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml

diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
new file mode 100644
index 00000000..a196344e
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.098261620631764912
+  y: 0.30160987978794107
+  z: 0.061677289758278485
+  qx: 0.7071241158041649
+  qy: -0.70708660591421257
+  qz: -0.0013366860602812623
+  qw: -0.0014927349398203876
\ No newline at end of file
diff --git a/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml b/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
index 8c4ae282..70413321 100644
--- a/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
+++ b/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Loop Detect AprilTag">
   <!-- ////////// -->
-  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers until failure." _favorite="true">
+  <BehaviorTree ID="Loop Detect AprilTag" _description="Loops finding April Tag markers until failure." _favorite="false">
     <Control ID="Sequence">
       <!-- Keep executing until failure -->
       <Decorator ID="KeepRunningUntilFailure">
diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index 4bd58fac..056b2a99 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Pick Apriltag Labeled Object">
   <!-- ////////// -->
-  <BehaviorTree ID="Pick Apriltag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="true">
+  <BehaviorTree ID="Pick Apriltag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
index b69044b1..dcc70f2e 100644
--- a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
+++ b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!-- ////////// -->
-  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="true">
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
       <Decorator ID="Repeat" num_cycles="{num_cycles}" name="SamplePose">
         <Control ID="Sequence">
         <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
index edf5597a..16efce35 100644
--- a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
+++ b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
@@ -1,12 +1,11 @@
 <?xml version="1.0"?>
-<root BTCPP_format="4" main_tree_to_execute="Calculate Offset From Apriltag">
-    <BehaviorTree ID="Calculate Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker">
+<root BTCPP_format="4" main_tree_to_execute="Teach Grasp Offset From Apriltag">
+    <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
         <Control ID="Sequence" name="root">
-            <Action ID="TakeSnapshot" />
             <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
             <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
             <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
-            <Action ID="CalculateTransform" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
+            <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
             <Action ID="WritePoseToYAML" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" pose="{tag_to_user_grasp_pose}"/>
         </Control>
     </BehaviorTree>

From 6f2188e59ea3e2b2aeaa24bc7aaac1d62e186e10 Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Tue, 26 Sep 2023 10:00:02 -0600
Subject: [PATCH 3/6] pre-commit

---
 .../objectives/apriltag1_grasp_offset.yaml                      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
index a196344e..9427aac1 100644
--- a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
@@ -5,4 +5,4 @@ GraspOffset:
   qx: 0.7071241158041649
   qy: -0.70708660591421257
   qz: -0.0013366860602812623
-  qw: -0.0014927349398203876
\ No newline at end of file
+  qw: -0.0014927349398203876

From 5dfa28c011e02991c23d40eac9edce567e5af55b Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Wed, 27 Sep 2023 10:34:09 -0600
Subject: [PATCH 4/6] update objectives to work with tutorial

---
 .../objectives/apriltag1_grasp_offset.yaml    | 14 +++++------
 .../objectives/loop_detect_apriltag.xml       |  2 --
 .../pick_apriltag_labeled_object.xml          |  2 +-
 .../objectives/sample_april_tag.xml           |  6 ++---
 .../teach_apriltag_grasp_offset.xml           |  2 +-
 .../objectives/tree_nodes_model.xml           | 25 +++++++++++++++++++
 6 files changed, 37 insertions(+), 14 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml

diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
index 9427aac1..bee43d3c 100644
--- a/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
+++ b/src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
@@ -1,8 +1,8 @@
 GraspOffset:
-  x: -0.098261620631764912
-  y: 0.30160987978794107
-  z: 0.061677289758278485
-  qx: 0.7071241158041649
-  qy: -0.70708660591421257
-  qz: -0.0013366860602812623
-  qw: -0.0014927349398203876
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0
diff --git a/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml b/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
index 70413321..eeaad793 100644
--- a/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
+++ b/src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
@@ -10,8 +10,6 @@
           <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
           <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
           <Action ID="DetectApriltags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
-          <!-- if you don't want this behavior to fail when it can't find an AprilTag comment out the line below -->
-          <Action ID="GetDetectionPose" detections="{detections}" target_id="0" target_label="" detection_pose="{detection_pose}"/>
         </Control>
       </Decorator>  
     </Control>
diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index 056b2a99..e91f4ec2 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -7,7 +7,7 @@
         <SubTree ID="Open Gripper"/>
         <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
-      <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <SubTree ID="Sample Apriltag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
       <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>
       <Control ID="Sequence" name="GraspObject">
         <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
index dcc70f2e..24937cc1 100644
--- a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
+++ b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
@@ -1,8 +1,8 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
+<root BTCPP_format="4" main_tree_to_execute="Sample Apriltag">
   <!-- ////////// -->
-  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
-      <Decorator ID="Repeat" num_cycles="{num_cycles}" name="SamplePose">
+  <BehaviorTree ID="Sample Apriltag" _description="Samples the pose of an Apriltag and returns the average measurement." _favorite="false">
+      <Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
         <Control ID="Sequence">
         <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
         <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>         
diff --git a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
index 16efce35..85c0ed8b 100644
--- a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
+++ b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
@@ -3,7 +3,7 @@
     <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
         <Control ID="Sequence" name="root">
             <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
-            <SubTree ID="Sample April Tag" num_cycles="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <SubTree ID="Sample Apriltag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
             <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
             <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
             <Action ID="WritePoseToYAML" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" pose="{tag_to_user_grasp_pose}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
new file mode 100644
index 00000000..fded7784
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
@@ -0,0 +1,25 @@
+<root>
+  <TreeNodesModel>
+    <SubTree ID="Sample Apriltag">
+        <description>
+          <p>
+              Samples the pose of an Apriltag and returns the average measurement.
+          </p>
+        </description>
+        <input_port name="num_samples" default="1">The number of times to sample the pose.</input_port>
+        <input_port name="tag_id" default="0">The Apriltag ID to detect.</input_port>
+        <input_port name="apriltag_config" default="apriltag_detection_config.yaml">The configuration file for detecting AprilTags.</input_port>
+        <input_port name="max_distance" default="0.02">The maximum distance allow between the average pose and the new sample (Euclidean distance between pose positions).</input_port>
+        <input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port>
+        <output_port name="avg_pose" default="{avg_pose}">The calculated average pose.</output_port>
+    </SubTree>
+    <SubTree ID="PickObject">
+        <description>
+          <p>
+              Picks an object at the specified grasp pose.
+          </p>
+        </description>
+        <input_port name="grasp_pose" default="{grasp_pose}">The desired grasp pose.</input_port>
+    </SubTree>
+  </TreeNodesModel>
+</root>

From c372d810ca4e8e439c9d28cfcd03960d7dbdfcb6 Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Wed, 27 Sep 2023 16:09:36 -0600
Subject: [PATCH 5/6] Refactor to April Tag expect where Apriltag is requied

---
 .../objectives/pick_apriltag_labeled_object.xml             | 6 +++---
 .../objectives/sample_april_tag.xml                         | 4 ++--
 .../objectives/teach_apriltag_grasp_offset.xml              | 2 +-
 .../objectives/tree_nodes_model.xml                         | 2 +-
 4 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
index e91f4ec2..af9a2e7b 100644
--- a/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
+++ b/src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
@@ -1,13 +1,13 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Pick Apriltag Labeled Object">
+<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
   <!-- ////////// -->
-  <BehaviorTree ID="Pick Apriltag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="false">
+  <BehaviorTree ID="Pick April Tag Labeled Object" _description="Picks up an object that is at a known pose offset from an AprilTag marker." _favorite="false">
     <Control ID="Sequence" name="TopLevelSequence">
       <Control ID="Sequence" name="Setup">
         <SubTree ID="Open Gripper"/>
         <Action ID="MoveToWaypoint" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
       </Control>
-      <SubTree ID="Sample Apriltag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+      <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
       <Action ID="LoadObjectiveParameters" config_file_name="apriltag1_grasp_offset.yaml" parameters="{pose_parameters}"/>
       <Control ID="Sequence" name="GraspObject">
         <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
index 24937cc1..2e47213d 100644
--- a/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
+++ b/src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<root BTCPP_format="4" main_tree_to_execute="Sample Apriltag">
+<root BTCPP_format="4" main_tree_to_execute="Sample April Tag">
   <!-- ////////// -->
-  <BehaviorTree ID="Sample Apriltag" _description="Samples the pose of an Apriltag and returns the average measurement." _favorite="false">
+  <BehaviorTree ID="Sample April Tag" _description="Samples the pose of an AprilTag and returns the average measurement." _favorite="false">
       <Decorator ID="Repeat" num_cycles="{num_samples}" name="SamplePose">
         <Control ID="Sequence">
         <Action ID="LoadObjectiveParameters" config_file_name="{apriltag_config}" parameters="{parameters}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
index 85c0ed8b..31181961 100644
--- a/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
+++ b/src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
@@ -3,7 +3,7 @@
     <BehaviorTree ID="Teach Grasp Offset From Apriltag" _description="Saves a grasp offset to yaml file from Apriltag observation using an interactive marker" _favorite="true">
         <Control ID="Sequence" name="root">
             <Action ID="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_pose}" />
-            <SubTree ID="Sample Apriltag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
+            <SubTree ID="Sample April Tag" num_samples="5" tag_id="1" apriltag_config="apriltag_detection_config.yaml" max_distance="0.02" max_rotation="0.2" avg_pose="{tag_pose}"/>
             <Action ID="TransformPoseFrame" input_pose="{tag_pose}" target_frame_id="world" output_pose="{tag_pose_world}"/>
             <Action ID="CalculatePoseOffset" source_pose="{tag_pose_world}" destination_pose="{user_grasp_pose}" source_to_destination_pose="{tag_to_user_grasp_pose}"/>
             <Action ID="WritePoseToYAML" yaml_filename="apriltag1_grasp_offset" namespace="GraspOffset" pose="{tag_to_user_grasp_pose}"/>
diff --git a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
index fded7784..4296b7e2 100644
--- a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
+++ b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
@@ -1,6 +1,6 @@
 <root>
   <TreeNodesModel>
-    <SubTree ID="Sample Apriltag">
+    <SubTree ID="Sample April Tag">
         <description>
           <p>
               Samples the pose of an Apriltag and returns the average measurement.

From 0ef2dc78d043ab00a03e7d5c3ef7c07c81a16b42 Mon Sep 17 00:00:00 2001
From: marqrazz <marq.razz@gmail.com>
Date: Mon, 2 Oct 2023 11:18:49 -0600
Subject: [PATCH 6/6] cleanup and add defaults for tags 2 & 3

---
 .../objectives/apriltag2_grasp_offset.yaml                | 8 ++++++++
 .../objectives/apriltag3_grasp_offset.yaml                | 8 ++++++++
 .../objectives/tree_nodes_model.xml                       | 4 ++--
 3 files changed, 18 insertions(+), 2 deletions(-)
 create mode 100644 src/picknik_ur_gazebo_config/objectives/apriltag2_grasp_offset.yaml
 create mode 100644 src/picknik_ur_gazebo_config/objectives/apriltag3_grasp_offset.yaml

diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag2_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag2_grasp_offset.yaml
new file mode 100644
index 00000000..bee43d3c
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/apriltag2_grasp_offset.yaml
@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0
diff --git a/src/picknik_ur_gazebo_config/objectives/apriltag3_grasp_offset.yaml b/src/picknik_ur_gazebo_config/objectives/apriltag3_grasp_offset.yaml
new file mode 100644
index 00000000..bee43d3c
--- /dev/null
+++ b/src/picknik_ur_gazebo_config/objectives/apriltag3_grasp_offset.yaml
@@ -0,0 +1,8 @@
+GraspOffset:
+  x: -0.1
+  y: 0.0
+  z: 0.05
+  qx: 0.707
+  qy: -0.707
+  qz: 0.0
+  qw: 0.0
diff --git a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
index 4296b7e2..eee414d6 100644
--- a/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
+++ b/src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
@@ -3,11 +3,11 @@
     <SubTree ID="Sample April Tag">
         <description>
           <p>
-              Samples the pose of an Apriltag and returns the average measurement.
+              Samples the pose of an AprilTag and returns the average measurement.
           </p>
         </description>
         <input_port name="num_samples" default="1">The number of times to sample the pose.</input_port>
-        <input_port name="tag_id" default="0">The Apriltag ID to detect.</input_port>
+        <input_port name="tag_id" default="0">The AprilTag ID to detect.</input_port>
         <input_port name="apriltag_config" default="apriltag_detection_config.yaml">The configuration file for detecting AprilTags.</input_port>
         <input_port name="max_distance" default="0.02">The maximum distance allow between the average pose and the new sample (Euclidean distance between pose positions).</input_port>
         <input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port>