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] 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>