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.
Merge pull request #49 from PickNikRobotics/improve_lab_sim_examples
Improve lab sim example objectives with story telling
- Loading branch information
Showing
17 changed files
with
784 additions
and
210 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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,33 @@ | ||
| <?xml version="1.0" encoding="UTF-8" ?> | ||
| <root BTCPP_format="4" main_tree_to_execute="_Scan Scene"> | ||
| <!--//////////--> | ||
| <BehaviorTree | ||
| ID="_Scan Scene" | ||
| _description="Move robot our of scene to enable full snapshot of scene." | ||
| _favorite="true" | ||
| > | ||
| <Control ID="Sequence" name="TopLevelSequence"> | ||
| <Action ID="ClearSnapshot" /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| _collapsed="true" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| controller_names="/joint_trajectory_controller" | ||
| waypoint_name="Park Far Right" | ||
| /> | ||
| <SubTree ID="Take scene camera snapshot" _collapsed="true" /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| _collapsed="true" | ||
| joint_group_name="manipulator" | ||
| planner_interface="moveit_default" | ||
| controller_names="/joint_trajectory_controller" | ||
| waypoint_name="Look at Table" | ||
| /> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <TreeNodesModel> | ||
| <SubTree ID="_Scan Scene" /> | ||
| </TreeNodesModel> | ||
| </root> |
106 changes: 106 additions & 0 deletions
106
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 |
|---|---|---|
| @@ -0,0 +1,106 @@ | ||
| <?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="" | ||
| > | ||
| <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}" | ||
| /> | ||
| <!--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}" | ||
| /> | ||
| </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}" | ||
| /> | ||
| </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" /> | ||
| </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}" | ||
| /> | ||
| </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}" | ||
| /> | ||
| </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}" | ||
| /> | ||
| </Decorator> | ||
| <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}" | ||
| /> | ||
| </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="" /> | ||
| </SubTree> | ||
| </TreeNodesModel> | ||
| </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
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.