forked from PickNikRobotics/moveit_pro_empty_ws
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
6 changed files
with
308 additions
and
203 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
96 changes: 79 additions & 17 deletions
96
src/lab_sim/objectives/constrained_pick_and_place_subtree.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 |
|---|---|---|
| @@ -1,44 +1,106 @@ | ||
| <root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place Subtree"> | ||
| <?xml version="1.0" encoding="utf-8" ?> | ||
| <root | ||
| BTCPP_format="4" | ||
| main_tree_to_execute="Constrained Pick and Place Subtree" | ||
| > | ||
| <!--//////////--> | ||
| <BehaviorTree ID="Constrained Pick and Place Subtree" _subtreeOnly="true" _description=""> | ||
| <BehaviorTree | ||
| ID="Constrained Pick and Place Subtree" | ||
| _subtreeOnly="true" | ||
| _description="" | ||
| > | ||
| <Control ID="Sequence"> | ||
| <!--Create a special type of motion planning configuration that includes an upwards orientation requirement--> | ||
| <Action ID="InitializeMotionConstraints" constraints_name="gripper pointing down" constraints="{constraints}"/> | ||
| <Action ID="AppendOrientationConstraint" config_file_name="my_constraints.yaml" constraints="{constraints}"/> | ||
| <Action | ||
| ID="InitializeMotionConstraints" | ||
| constraints_name="gripper pointing down" | ||
| constraints="{constraints}" | ||
| /> | ||
| <Action | ||
| ID="AppendOrientationConstraint" | ||
| config_file_name="my_constraints.yaml" | ||
| constraints="{constraints}" | ||
| /> | ||
| <!--Move to pick location--> | ||
| <!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success--> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" waypoint_name="{pre_pick}" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="{pre_pick}" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| </Decorator> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" waypoint_name="{pick}" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="{pick}" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| </Decorator> | ||
| <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)--> | ||
| <Decorator ID="ForceSuccess"> | ||
| <SubTree ID="Close Gripper"/> | ||
| <SubTree ID="Close Gripper" /> | ||
| </Decorator> | ||
| <!--Move to place (drop) location--> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="{pre_pick}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| _collapsed="true" | ||
| waypoint_name="{pre_pick}" | ||
| /> | ||
| </Decorator> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="{pre_place}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| _collapsed="true" | ||
| waypoint_name="{pre_place}" | ||
| /> | ||
| </Decorator> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="{place}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| _collapsed="true" | ||
| waypoint_name="{place}" | ||
| /> | ||
| </Decorator> | ||
| <SubTree ID="Open Gripper"/> | ||
| <SubTree ID="Open Gripper" /> | ||
| <Decorator ID="RetryUntilSuccessful" num_attempts="3"> | ||
| <SubTree ID="Move to Waypoint" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" joint_group_name="manipulator" planner_interface="moveit_default" constraints="{constraints}" _collapsed="true" waypoint_name="{pre_place}"/> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| _collapsed="true" | ||
| waypoint_name="{pre_place}" | ||
| /> | ||
| </Decorator> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <TreeNodesModel> | ||
| <SubTree ID="Constrained Pick and Place Subtree"> | ||
| <inout_port name="pre_pick" default=""/> | ||
| <inout_port name="pick" default=""/> | ||
| <inout_port name="pre_place" default=""/> | ||
| <inout_port name="place" default=""/> | ||
| <inout_port name="pre_pick" default="" /> | ||
| <inout_port name="pick" default="" /> | ||
| <inout_port name="pre_place" default="" /> | ||
| <inout_port name="place" default="" /> | ||
| </SubTree> | ||
| </TreeNodesModel> | ||
| </root> | ||
| </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
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.