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

Commit

Permalink
Add pose averaging to Pick Apriltag Labeled Object
Browse files Browse the repository at this point in the history
  • Loading branch information
@MarqRazz
MarqRazz committed Sep 8, 2023
1 parent 59fb32e commit 4716301
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 9 deletions.
19 changes: 19 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/loop_detect_april.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Loop Detect April">
<!-- ////////// -->
<BehaviorTree ID="Loop Detect April" _description="Loops finding April Tag marker until failure." _favorite="true">
<Control ID="Sequence">
<!-- Keep executing until failure -->
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence" name="GetDetection">
<Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
<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 April tag comment out the line below -->
<Action ID="GetDetectionPose" detections="{detections}" target_id="0" target_label="" detection_pose="{detection_pose}"/>
</Control>
</Decorator>
</Control>
</BehaviorTree>
</root>
13 changes: 4 additions & 9 deletions src/picknik_ur_gazebo_config/objectives/pick_apriltag_labeled_object.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,19 +8,14 @@
<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>
<Control ID="Sequence" name="GetDetection">
<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}"/>
<Action ID="GetDetectionPose" detections="{detections}" target_id="1" target_label="" detection_pose="{detection_pose}"/>
</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}"/>
<Control ID="Sequence" name="GraspObject">
<Action ID="TransformPoseFrame" input_pose="{detection_pose}" target_frame_id="world" output_pose="{detection_pose_world}"/>
<Action ID="TransformPose" input_pose="{detection_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="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"/>
</Control>
</Control>
</BehaviorTree>
</root>
</root>
28 changes: 28 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/sample_april_tag.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?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 April tag and returns the average measurement." _favorite="true">
<Decorator ID="Repeat" num_cycles="{num_cycles}" 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}"/>
<Decorator ID="RetryUntilSuccessful" num_attempts="5">
<Control ID="Sequence">
<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}"/>
<Action ID="GetDetectionPose" detections="{detections}" target_id="{tag_id}" target_label="" detection_pose="{detection_pose}"/>
</Control>
</Decorator>
<Action ID="AveragePoseStamped" pose_sample="{detection_pose}" avg_pose="{avg_pose}" max_distance="{max_distance}" max_rotation="{max_rotation}"/>
<!-- TODO: update this objective to allow rejecting noisy samples but fail after X attempts
<Fallback name="Average pose or sample more">
<Action ID="AveragePoseStamped"/>
<Control ID="Sequence">
<Script code="num_cycles={num_cycles - 1}" />
<Script code="num_failures={num_failures + 1}" />
</Control>
</Fallback> -->
</Control>
</Decorator>
</BehaviorTree>
</root>

0 comments on commit 4716301

Please sign in to comment.