Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Apriltag update #72

Merged
merged 2 commits into from
Jan 2, 2025
Merged

Apriltag update #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6d246ae
Updating apriltag objective
MikeWrock Jan 1, 2025
f673621
updating objective
MikeWrock Jan 1, 2025
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
23 changes: 20 additions & 3 deletions src/lab_sim/objectives/apriltag_pick_object.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
<?xml version="1.0" encoding="utf-8" ?>
<root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object">
<!--//////////-->
<BehaviorTree
Expand All @@ -21,11 +22,13 @@
ID="GetCameraInfo"
topic_name="/wrist_camera/camera_info"
message_out="{camera_info}"
timeout_sec="5.000000"
/>
<Action
ID="GetImage"
topic_name="/wrist_camera/color"
message_out="{image}"
timeout_sec="5.000000"
/>
<SubTree
ID="Get AprilTag Pose from Image"
Expand All @@ -47,7 +50,11 @@
target_pose="{view_pose}"
move_to_pose_solution="{move_to_pose_solution}"
/>
<Action ID="ExecuteMTCTask" solution="{move_to_pose_solution}" />
<Action
ID="ExecuteMTCTask"
solution="{move_to_pose_solution}"
goal_duration_tolerance="-1.000000"
/>
<SubTree
ID="Get AprilTag Pose from Image"
image="{image}"
Expand All @@ -66,26 +73,36 @@
<Action
ID="LoadObjectiveParameters"
config_file_name="pick_object_config.yaml"
parameters="{parameters}"
/>
<Action
ID="InitializeMTCTask"
task_id="pick_object"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
task="{pick_object_task}"
/>
<Action ID="SetupMTCCurrentState" task="{pick_object_task}" />
<Action
ID="SetupMTCCurrentState"
task="{pick_object_task}"
skip_collision_check="false"
/>
<Action
ID="SetupMTCPickObject"
grasp_pose="{offset_pose}"
task="{pick_object_task}"
parameters="{parameters}"
monitored_stage="current state"
/>
<Action
ID="PlanMTCTask"
solution="{pick_object_solution}"
task="{pick_object_task}"
/>
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}" />
<Action
ID="ExecuteMTCTask"
solution="{pick_object_solution}"
goal_duration_tolerance="-1.000000"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
Expand Down
91 changes: 77 additions & 14 deletions src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<?xml version="1.0" encoding="utf-8" ?>
<root
BTCPP_format="4"
main_tree_to_execute="Pick April Tag Object With Approval"
main_tree_to_execute="Pick April Tag Labeled Object With Approval"
>
<!--//////////-->
<BehaviorTree
ID="Pick April Tag Object With Approval"
ID="Pick April Tag Labeled Object With Approval"
_description="Picks up an object that has an AprilTag marker."
_favorite="false"
>
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Look at table" _collapsed="true" />
<Action ID="ClearSnapshot" />
<SubTree ID="Take wrist camera snapshot" _collapsed="true" />
<Control ID="Sequence" name="Setup">
<SubTree ID="Open Gripper" />
Expand All @@ -23,37 +25,98 @@
ID="GetCameraInfo"
topic_name="/wrist_camera/camera_info"
message_out="{camera_info}"
timeout_sec="5.000000"
/>
<Action
ID="GetImage"
topic_name="/wrist_camera/color"
message_out="{image}"
timeout_sec="5.000000"
/>
<Action
ID="DetectAprilTags"
<SubTree
ID="Get AprilTag Pose from Image"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
detections="{detections}"
_collapsed="true"
detection_pose="{detection_pose}"
/>
<Action
ID="TransformPose"
output_pose="{view_pose}"
input_pose="{detection_pose}"
quaternion_xyzw="0;0;0;1"
translation_xyz="0;-0.1;-0.15"
/>
<SubTree
ID="Plan Move To Pose"
target_pose="{view_pose}"
move_to_pose_solution="{move_to_pose_solution}"
/>
<Action ID="GetDetectionPose" />
<Action ID="TransformPoseFrame" input_pose="{detection_pose}" />
<Action
ID="AddPoseStampedToVector"
vector="{initial_poses}"
input="{output_pose}"
ID="ExecuteMTCTask"
solution="{move_to_pose_solution}"
goal_duration_tolerance="-1.000000"
/>
<SubTree
ID="Pick first object in vector"
ID="Get AprilTag Pose from Image"
image="{image}"
camera_info="{camera_info}"
parameters="{parameters}"
_collapsed="true"
pose_vector="{initial_poses}"
detection_pose="{detection_pose}"
/>
<Action
ID="TransformPose"
output_pose="{offset_pose}"
input_pose="{detection_pose}"
quaternion_xyzw="0;0;0;1"
translation_xyz="0;0;-0.01"
/>
<Action
ID="LoadObjectiveParameters"
config_file_name="pick_object_config.yaml"
parameters="{parameters}"
/>
<Action
ID="InitializeMTCTask"
task_id="pick_object"
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
task="{pick_object_task}"
/>
<Action
ID="SetupMTCCurrentState"
task="{pick_object_task}"
skip_collision_check="false"
/>
<Action
ID="SetupMTCPickObject"
grasp_pose="{offset_pose}"
task="{pick_object_task}"
parameters="{parameters}"
monitored_stage="current state"
/>
<Action
ID="PlanMTCTask"
solution="{pick_object_solution}"
task="{pick_object_task}"
/>
<Action
ID="WaitForUserTrajectoryApproval"
solution="{pick_object_solution}"
/>
<Action
ID="ExecuteMTCTask"
solution="{pick_object_solution}"
goal_duration_tolerance="-1.000000"
/>
</Control>
</BehaviorTree>
<TreeNodesModel>
<SubTree ID="Pick April Tag Object With Approval">
<SubTree ID="Pick April Tag Labeled Object With Approval">
<MetadataFields>
<Metadata subcategory="Application - Advanced Examples" />
<Metadata runnable="true" />
</MetadataFields>
</SubTree>
</TreeNodesModel>
Expand Down
Loading