Add objectives to teach and pick objects relitive to AprilTag pose ob…

…servations
PickNikRobotics · Oct 2, 2023 · 65eb863 · 65eb863
1 parent a2e9e39
commit 65eb863
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 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
@@ -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
@@ -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>