-
Notifications
You must be signed in to change notification settings - Fork 4
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
87 changed files
with
4,976 additions
and
1,354 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,20 +1,52 @@ | ||
| <root BTCPP_format="4" main_tree_to_execute="Move to Waypoint"> | ||
| <BehaviorTree ID="Move to Waypoint" _description="This Objective is used when moving to one of the saved waypoints in your site configuration" _subtreeOnly="true"> | ||
| <Control ID="Sequence" name="root"> | ||
| <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/> | ||
| <Action ID="InitializeMTCTask" task_id="move_to_named_pose" controller_names="{controller_names}" task="{move_to_waypoint_task}"/> | ||
| <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/> | ||
| <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" constraints="{constraints}" task="{move_to_waypoint_task}"/> | ||
| <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/> | ||
| <Action ID="BlockingExecuteMTCTask" solution="{move_to_waypoint_solution}"/> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <TreeNodesModel> | ||
| <SubTree ID="Move to Waypoint"> | ||
| <input_port name="waypoint_name" default="Point A"/> | ||
| <input_port name="joint_group_name" default="manipulator"/> | ||
| <input_port name="planner_interface" default="moveit_default"/> | ||
| <input_port name="controller_names" default="/joint_trajectory_controller"/> | ||
| </SubTree> | ||
| </TreeNodesModel> | ||
| </root> | ||
| <BehaviorTree | ||
| ID="Move to Waypoint" | ||
| _description="This Objective is used when moving to one of the saved waypoints in your site configuration" | ||
| _subtreeOnly="true" | ||
| > | ||
| <Control ID="Sequence" name="root"> | ||
| <Action | ||
| ID="RetrieveWaypoint" | ||
| waypoint_joint_state="{target_joint_state}" | ||
| waypoint_name="{waypoint_name}" | ||
| joint_group_name="{joint_group_name}" | ||
| /> | ||
| <Action | ||
| ID="InitializeMTCTask" | ||
| task_id="move_to_named_pose" | ||
| controller_names="{controller_names}" | ||
| task="{move_to_waypoint_task}" | ||
| /> | ||
| <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" /> | ||
| <Action | ||
| ID="SetupMTCMoveToJointState" | ||
| joint_state="{target_joint_state}" | ||
| name="SetupMTCMoveToJointState" | ||
| planning_group_name="{joint_group_name}" | ||
| planner_interface="{planner_interface}" | ||
| constraints="{constraints}" | ||
| task="{move_to_waypoint_task}" | ||
| /> | ||
| <Action | ||
| ID="PlanMTCTask" | ||
| solution="{move_to_waypoint_solution}" | ||
| task="{move_to_waypoint_task}" | ||
| /> | ||
| <Action | ||
| ID="BlockingExecuteMTCTask" | ||
| solution="{move_to_waypoint_solution}" | ||
| /> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <TreeNodesModel> | ||
| <SubTree ID="Move to Waypoint"> | ||
| <input_port name="waypoint_name" default="Point A" /> | ||
| <input_port name="joint_group_name" default="manipulator" /> | ||
| <input_port name="planner_interface" default="moveit_default" /> | ||
| <input_port | ||
| name="controller_names" | ||
| default="/joint_trajectory_controller" | ||
| /> | ||
| </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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,6 +1,16 @@ | ||
| <root BTCPP_format="4" main_tree_to_execute="Teleoperate"> | ||
| <!--//////////--> | ||
| <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="false" _hardcoded="false"> | ||
| <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/> | ||
| </BehaviorTree> | ||
| </root> | ||
| <!--//////////--> | ||
| <BehaviorTree | ||
| ID="Teleoperate" | ||
| _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." | ||
| _favorite="false" | ||
| _hardcoded="false" | ||
| > | ||
| <SubTree | ||
| ID="Request Teleoperation" | ||
| enable_user_interaction="false" | ||
| user_interaction_prompt="" | ||
| initial_teleop_mode="3" | ||
| /> | ||
| </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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1 +1 @@ | ||
| [] | ||
| [] |
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 +1,122 @@ | ||
| <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place"><!--//////////--><BehaviorTree ID="3 Waypoints Pick and Place" _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position" _favorite="false"><Control ID="Sequence"><!--Reset the planning scene to ensure no old collision objects are hanging around--><Action ID="ClearSnapshot"/><!--Setup the environment to its "home" configuration - move to center and open gripper--><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/><!--Run pick and place on loop--><Decorator ID="KeepRunningUntilFailure"><Control ID="Sequence"><!--Pick object from original location, put it down at a different part of the table, and go back to center pose--><Control ID="Sequence"><SubTree ID="Move to Waypoint" waypoint_name="Pick Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><SubTree ID="Move to Waypoint" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}" waypoint_name="Place Cube"/><Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Control><!--Pick object from where it was placed, put it down on the original location, and go back to center pose--><Control ID="Sequence"><SubTree ID="Move to Waypoint" waypoint_name="Place Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" 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 Cube" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/><Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/><SubTree ID="Move to Waypoint" waypoint_name="Look at Table" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" planner_interface="moveit_default" constraints="{constraints}"/></Control></Control></Decorator></Control></BehaviorTree></root> | ||
| <root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place"> | ||
| <!--//////////--> | ||
| <BehaviorTree | ||
| ID="3 Waypoints Pick and Place" | ||
| _description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position" | ||
| _favorite="false" | ||
| > | ||
| <Control ID="Sequence"> | ||
| <!--Reset the planning scene to ensure no old collision objects are hanging around--> | ||
| <Action ID="ClearSnapshot" /> | ||
| <!--Setup the environment to its "home" configuration - move to center and open gripper--> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Look at Table" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| <Action | ||
| ID="MoveGripperAction" | ||
| gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" | ||
| position="0" | ||
| /> | ||
| <!--Run pick and place on loop--> | ||
| <Decorator ID="KeepRunningUntilFailure"> | ||
| <Control ID="Sequence"> | ||
| <!--Pick object from original location, put it down at a different part of the table, and go back to center pose--> | ||
| <Control ID="Sequence"> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Pick Cube" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| <Action | ||
| ID="MoveGripperAction" | ||
| gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" | ||
| position="0.7929" | ||
| /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Look at Table" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| waypoint_name="Place Cube" | ||
| /> | ||
| <Action | ||
| ID="MoveGripperAction" | ||
| gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" | ||
| position="0" | ||
| /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Look at Table" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| </Control> | ||
| <!--Pick object from where it was placed, put it down on the original location, and go back to center pose--> | ||
| <Control ID="Sequence"> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Place Cube" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| <Action | ||
| ID="MoveGripperAction" | ||
| gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" | ||
| position="0.7929" | ||
| /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Look at Table" | ||
| 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 Cube" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| <Action | ||
| ID="MoveGripperAction" | ||
| gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" | ||
| position="0" | ||
| /> | ||
| <SubTree | ||
| ID="Move to Waypoint" | ||
| waypoint_name="Look at Table" | ||
| joint_group_name="manipulator" | ||
| controller_names="/joint_trajectory_controller /robotiq_gripper_controller" | ||
| planner_interface="moveit_default" | ||
| constraints="{constraints}" | ||
| /> | ||
| </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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,22 +1,38 @@ | ||
| <?xml version="1.0" encoding="utf-8"?> | ||
| <?xml version="1.0" encoding="utf-8" ?> | ||
| <root BTCPP_format="4" main_tree_to_execute="Add Waypoints to MTC Task"> | ||
| <!--//////////--> | ||
| <BehaviorTree ID="Add Waypoints to MTC Task"> | ||
| <Control ID="Sequence"> | ||
| <Decorator ID="ForEachString" vector_in="{waypoint_names}" out="{waypoint_name}"> | ||
| <Decorator | ||
| ID="ForEachString" | ||
| vector_in="{waypoint_names}" | ||
| out="{waypoint_name}" | ||
| > | ||
| <Control ID="Sequence"> | ||
| <Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}" joint_group_name="{joint_group_name}"/> | ||
| <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState" planning_group_name="{joint_group_name}" planner_interface="{planner_interface}" task="{mtc_task}"/> | ||
| <Action | ||
| ID="RetrieveWaypoint" | ||
| waypoint_joint_state="{target_joint_state}" | ||
| waypoint_name="{waypoint_name}" | ||
| joint_group_name="{joint_group_name}" | ||
| /> | ||
| <Action | ||
| ID="SetupMTCMoveToJointState" | ||
| joint_state="{target_joint_state}" | ||
| name="SetupMTCMoveToJointState" | ||
| planning_group_name="{joint_group_name}" | ||
| planner_interface="{planner_interface}" | ||
| task="{mtc_task}" | ||
| /> | ||
| </Control> | ||
| </Decorator> | ||
| </Control> | ||
| </BehaviorTree> | ||
| <TreeNodesModel> | ||
| <SubTree ID="Add Waypoints to MTC Task"> | ||
| <input_port name="joint_group_name" default="manipulator"/> | ||
| <input_port name="waypoint_names" default="Pick Cube;Place Cube"/> | ||
| <inout_port name="mtc_task" default="{mtc_task}"/> | ||
| <input_port name="planner_interface" default="moveit_default"/> | ||
| <input_port name="joint_group_name" default="manipulator" /> | ||
| <input_port name="waypoint_names" default="Pick Cube;Place Cube" /> | ||
| <inout_port name="mtc_task" default="{mtc_task}" /> | ||
| <input_port name="planner_interface" default="moveit_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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1 +1,71 @@ | ||
| <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object"><!--//////////--><BehaviorTree ID="Pick April Tag Labeled Object" _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="Wrist camera snapshot" _collapsed="true"/><Control ID="Sequence" name="Setup"><SubTree ID="Open Gripper"/></Control><Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/><Action ID="GetCameraInfo" topic_name="/wrist_camera/camera_info" message_out="{camera_info}"/><Action ID="GetImage" topic_name="/wrist_camera/color" message_out="{image}"/><Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/><Action ID="GetDetectionPose"/><Action ID="TransformPoseFrame" input_pose="{detection_pose}"/><Action ID="TransformPose" output_pose="{offset_pose}" input_pose="{output_pose}" quaternion_xyzw="0;0;0;1" translation_xyz="0;0;-0.01"/><Action ID="LoadObjectiveParameters" config_file_name="pick_object_config.yaml"/><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="{offset_pose}" task="{pick_object_task}" parameters="{parameters}"/><Action ID="PlanMTCTask" solution="{pick_object_solution}" task="{pick_object_task}"/><Action ID="ExecuteMTCTask" solution="{pick_object_solution}"/></Control></BehaviorTree></root> | ||
| <root BTCPP_format="4" main_tree_to_execute="Pick April Tag Labeled Object"> | ||
| <!--//////////--> | ||
| <BehaviorTree | ||
| ID="Pick April Tag Labeled Object" | ||
| _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="Wrist camera snapshot" _collapsed="true" /> | ||
| <Control ID="Sequence" name="Setup"> | ||
| <SubTree ID="Open Gripper" /> | ||
| </Control> | ||
| <Action | ||
| ID="LoadObjectiveParameters" | ||
| config_file_name="apriltag_detection_config.yaml" | ||
| parameters="{parameters}" | ||
| /> | ||
| <Action | ||
| ID="GetCameraInfo" | ||
| topic_name="/wrist_camera/camera_info" | ||
| message_out="{camera_info}" | ||
| /> | ||
| <Action | ||
| ID="GetImage" | ||
| topic_name="/wrist_camera/color" | ||
| message_out="{image}" | ||
| /> | ||
| <Action | ||
| ID="DetectAprilTags" | ||
| image="{image}" | ||
| camera_info="{camera_info}" | ||
| parameters="{parameters}" | ||
| detections="{detections}" | ||
| /> | ||
| <Action ID="GetDetectionPose" /> | ||
| <Action ID="TransformPoseFrame" input_pose="{detection_pose}" /> | ||
| <Action | ||
| ID="TransformPose" | ||
| output_pose="{offset_pose}" | ||
| input_pose="{output_pose}" | ||
| quaternion_xyzw="0;0;0;1" | ||
| translation_xyz="0;0;-0.01" | ||
| /> | ||
| <Action | ||
| ID="LoadObjectiveParameters" | ||
| config_file_name="pick_object_config.yaml" | ||
| /> | ||
| <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="{offset_pose}" | ||
| task="{pick_object_task}" | ||
| parameters="{parameters}" | ||
| /> | ||
| <Action | ||
| ID="PlanMTCTask" | ||
| solution="{pick_object_solution}" | ||
| task="{pick_object_task}" | ||
| /> | ||
| <Action ID="ExecuteMTCTask" solution="{pick_object_solution}" /> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
Oops, something went wrong.