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

Commit

Permalink
Revert "revise Pick Object to use new vector composition behaviors"
Browse files Browse the repository at this point in the history 
This reverts commit 6d81e45.
  • Loading branch information
@schornakj
schornakj committed Apr 22, 2024
1 parent 58cfec4 commit 4dbd574
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 43 deletions.
58 changes: 22 additions & 36 deletions src/picknik_ur_site_config/objectives/pick_object.xml
View file
Original file line number Diff line number Diff line change
@@ -1,39 +1,25 @@
<?xml version='1.0' encoding='UTF-8'?>
<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="Pick Object">
<BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
<Control ID="Sequence" name="root">
<SubTree ID="Open Gripper"/>
<Control ID="Sequence">
<Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/>
<Decorator ID="ForceSuccess">
<Control ID="Sequence">
<Action ID="AlwaysFailure"/>
<Action ID="TransformPointCloudFrame" output_cloud="{point_cloud}" target_frame="wrist_mounted_camera_color_optical_frame" input_cloud="{point_cloud}"/>
</Control>
</Decorator>
<Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
<Action ID="ResetPoseStampedVector" vector="{output_poses_transformed}"/>
<Decorator ID="ForEachPoseStamped" out="{pose}" vector_in="{output_poses}">
<Control ID="Sequence">
<Action ID="TransformPose" output_pose="{pose_transformed}" input_pose="{pose}" quaternion_xyzw="1;0;0;0" translation_xyz="0;0;0.12"/>
<Action ID="AddPoseStampedToVector" input="{pose_transformed}" vector="{output_poses_transformed}"/>
</Control>
</Decorator>
<Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses_transformed}" adjusted_poses="{adjusted_poses}"/>
<Action ID="ClearSnapshot"/>
</Control>
<Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml" parameters="{parameters}"/>
<Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
<Control ID="Sequence">
<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="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
<Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
<BehaviorTree ID="Pick Object" _description="Pick up and lift a small object" _favorite="true">
<Control ID="Sequence" name="root">
<SubTree ID="Open Gripper"/>
<Control ID="Sequence">
<Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
<Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
</Control>
<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}"/>
<Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
<Control ID="Sequence">
<Action ID="SetupMTCPickObject" grasp_pose="{grasp_pose}" task="{pick_object_task}" parameters="{parameters}"/>
<Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/>
<SubTree ID="Wait for Trajectory Approval if User Available" solution="{pick_object_solution}"/>
<Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/>
</Control>
</Decorator>
</Control>
</Decorator>
</Control>
</BehaviorTree>
</BehaviorTree>
</root>
16 changes: 9 additions & 7 deletions src/picknik_ur_site_config/objectives/pick_object_config.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,11 @@
SetupMTCPickObject:
# The lift vector points to the direction of the positive z-axis of the frame marked as the world frame.
world_frame_name: "world"
arm_group_name: "manipulator"
end_effector_group_name: "gripper"
end_effector_name: "moveit_ee"
hand_frame_name: "grasp_link"
end_effector_closed_pose_name: "close"

approach_distance: 0.1
arm_group_name: manipulator
end_effector_closed_pose_name: close
end_effector_group_name: gripper
end_effector_name: moveit_ee
hand_frame_name: robotiq_85_base_link
lift_distance: 0
world_frame_name: world
lift_distance: 0.1

0 comments on commit 4dbd574

Please sign in to comment.