Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Add objectives to teach and pick objects relative to AprilTag pose #106

Merged
merged 6 commits into from
Oct 2, 2023
+70 −12
Merged

Add objectives to teach and pick objects relative to AprilTag pose #106

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
65eb863
Add objectives to teach and pick objects relitive to AprilTag pose ob…
MarqRazz Sep 15, 2023
916c7b2
update behavior names
MarqRazz Sep 26, 2023
6f2188e
pre-commit
MarqRazz Sep 26, 2023
5dfa28c
update objectives to work with tutorial
MarqRazz Sep 27, 2023
c372d81
Refactor to April Tag expect where Apriltag is requied
MarqRazz Sep 27, 2023
0ef2dc7
cleanup and add defaults for tags 2 & 3
MarqRazz Oct 2, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/apriltag1_grasp_offset.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
GraspOffset:
MarqRazz marked this conversation as resolved.
Show resolved Hide resolved
x: -0.1
y: 0.0
z: 0.05
qx: 0.707
qy: -0.707
qz: 0.0
qw: 0.0
4 changes: 1 addition & 3 deletions src/picknik_ur_gazebo_config/objectives/loop_detect_apriltag.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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">
Expand All @@ -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>
Expand Down
13 changes: 6 additions & 7 deletions src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,18 @@
<?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="true">
<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 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 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}"/>
<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>
Expand Down
4 changes: 2 additions & 2 deletions src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?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">
<Decorator ID="Repeat" num_cycles="{num_cycles}" name="SamplePose">
<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}"/>
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
Expand Down
12 changes: 12 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/teach_apriltag_grasp_offset.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
<?xml version="1.0"?>
<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="GetPoseFromUser" parameter_name="calculate_offset.grasp_pose" parameter_value="{user_grasp_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}"/>
</Control>
</BehaviorTree>
</root>
25 changes: 25 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
<root>
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
<TreeNodesModel>
<SubTree ID="Sample April Tag">
<description>
<p>
Samples the pose of an Apriltag and returns the average measurement.
MarqRazz marked this conversation as resolved.
Show resolved Hide resolved
</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>
MarqRazz marked this conversation as resolved.
Show resolved Hide resolved
<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>