This repository has been archived by the owner on Dec 13, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Enable parallel planning for all Behaviors
- Loading branch information
Showing
12 changed files
with
139 additions
and
44 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
95 changes: 95 additions & 0 deletions
95
src/picknik_ur_base_config/objectives/looping_pick_and_place_object.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,95 @@ | ||
| <?xml version="1.0"?> | ||
| <root BTCPP_format="4" main_tree_to_execute="Looping Pick and Place Object" _favorite="true"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="Looping Pick and Place Object" _description="Repeatedly attempt to detect and grasp a cuboid from a target bin, and drop it in the target area." _favorite="true"> | ||
| <Control ID="Sequence"> | ||
| <Action ID="LoadObjectiveParameters" config_file_name="looping_pick_and_place_object_config.yaml" parameters="{parameters}"/> | ||
|
|
||
| <!-- Remove any objects which might remain in the planning scene from previous pick attempts --> | ||
| <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/> | ||
|
|
||
| <!-- Open the gripper and move to a consistent state. --> | ||
| <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/> | ||
| <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/> | ||
|
|
||
| <!-- This loops forever because the behavior tree under this decorator never returns FAILURE. --> | ||
| <Decorator ID="KeepRunningUntilFailure"> | ||
| <Control ID="Fallback" name="TryGraspOuter"> | ||
| <Decorator ID="KeepRunningUntilFailure"> | ||
| <Decorator ID="ForEachString" name="IterateThroughPlaceWaypoints" vector_in="Place Right;Place Left" out="{place_waypoint}"> | ||
| <Control ID="Sequence"> | ||
| <!-- This decorator contains the logic to detect an object and then execute motion to grasp it. If grasping is aborted because the object changed while the robot was moving towards it, we will retry up to 5 times before giving up. --> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="5"> | ||
| <Control ID="Fallback" name="TryGraspInner"> | ||
| <Control ID="Sequence"> | ||
| <!-- Clear the collision octree. --> | ||
| <Action ID="ClearSnapshot"/> | ||
|
|
||
| <!-- Remove any objects which might remain in the planning scene from previous pick attempts --> | ||
| <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="apply_planning_scene"/> | ||
|
|
||
| <!-- Get the latest point cloud from the target topic--> | ||
| <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" /> | ||
|
|
||
| <!-- Detect all cuboids in that point cloud--> | ||
| <Action ID="FindSingularCuboids" point_cloud="{point_cloud}" parameters="{parameters}" detected_shapes="{cuboids}"/> | ||
|
|
||
| <!-- Get the first detected cuboid. NOTE: this is a small hack to get ForEachCollisionObject to iterate just once, which returns the first object in the vector. --> | ||
| <Control ID="Sequence"> | ||
| <Decorator ID="ForceSuccess"> | ||
| <Decorator ID="ForEachCollisionObject" vector_in="{cuboids}" out="{cuboid_object}"> | ||
| <Control ID="Sequence"> | ||
| <Action ID="AlwaysFailure"/> | ||
| </Control> | ||
| </Decorator> | ||
| </Decorator> | ||
| </Control> | ||
|
|
||
| <!-- Add this cuboid to the planning scene. --> | ||
| <Action ID="ModifyObjectInPlanningScene" object="{cuboid_object}" apply_planning_scene_service="/apply_planning_scene"/> | ||
|
|
||
| <!-- Use point cloud to update the collision octomap (happens after ModifyObjectInPlanningScene so the cuboid is excluded from the octomap)--> | ||
| <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/> | ||
|
|
||
| <!-- Set up an MTC task to pick the cuboid and then plan the task. --> | ||
| <Action ID="InitializeMTCTask" task_id="looping_pick_and_place_object" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{task}"/> | ||
| <Action ID="SetupMTCCurrentState" task="{task}"/> | ||
| <Action ID="SetupMTCPickCuboid" cuboid_object="{cuboid_object}" task="{task}" parameters="{parameters}"/> | ||
| <Action ID="PlanMTCTask" solution="{solution}" task="{task}"/> | ||
|
|
||
| <!-- Execute the planned motion. --> | ||
| <Action ID="ExecuteMTCTask" solution="{solution}"/> | ||
| </Control> | ||
|
|
||
| <!-- TryGraspInner fallback condition. Wait for a short time before triggering a retry. --> | ||
| <Control ID="Sequence"> | ||
| <Action ID="WaitForDuration" delay_duration="1"/> | ||
| <Action ID="AlwaysFailure"/> | ||
| </Control> | ||
| </Control> | ||
| </Decorator> | ||
| <!-- Move home. --> | ||
| <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/> | ||
|
|
||
| <!-- Move to the current place waypoint set by IterateThroughPlaceWaypoints, then open the gripper. --> | ||
| <Action ID="MoveToJointState" waypoint_name="{place_waypoint}" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/> | ||
| <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/> | ||
|
|
||
| <!-- Finally, return home. --> | ||
| <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/> | ||
| </Control> | ||
| </Decorator> | ||
| </Decorator> | ||
|
|
||
| <!-- TryGraspOuter fallback condition. Reset scene objects, move home, and then wait a short while before triggering a retry. --> | ||
| <Control ID="Sequence"> | ||
| <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/> | ||
| <Action ID="MoveToJointState" waypoint_name="Look at Pick and Place Zone" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/> | ||
| <Action ID="WaitForDuration" delay_duration="5"/> | ||
| <Action ID="AlwaysSuccess"/> | ||
| </Control> | ||
| </Control> | ||
| </Decorator> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.