Add minimal pick-and-place case with world embedded
This commit is contained in:
parent
209e99a4b3
commit
a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
19
rbs_bt_executor/bt_trees/example_repeat.xml
Normal file
19
rbs_bt_executor/bt_trees/example_repeat.xml
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="BehaviorTree">
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="BehaviorTree">
|
||||
<Sequence>
|
||||
<SetBlackboard output_key="" value=""/>
|
||||
<Repeat num_cycles="3">
|
||||
<Sequence>
|
||||
<AlwaysSuccess/>
|
||||
<AlwaysSuccess/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel/>
|
||||
<!-- ////////// -->
|
||||
</root>
|
||||
30
rbs_bt_executor/bt_trees/find_object.xml
Normal file
30
rbs_bt_executor/bt_trees/find_object.xml
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="Untitled">
|
||||
<Sequence>
|
||||
<SetBlackboard name="InitBT"
|
||||
value="param_name"
|
||||
output_key="param_values"/>
|
||||
<Fallback>
|
||||
<ReactiveSequence>
|
||||
<GoToPose PoseArray="{param}"
|
||||
robot_name="{robot_name}"/>
|
||||
<AproachObject/>
|
||||
</ReactiveSequence>
|
||||
<AlwaysSuccess name="DoSomethingElse"/>
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Description of Node Models (used by Groot) -->
|
||||
<TreeNodesModel>
|
||||
<Condition ID="AproachObject"
|
||||
editable="true"/>
|
||||
<Action ID="GoToPose"
|
||||
editable="true">
|
||||
<input_port name="PoseArray">Load from outside</input_port>
|
||||
<input_port name="robot_name"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
|
||||
</root>
|
||||
19
rbs_bt_executor/bt_trees/gripper_tree.xml
Normal file
19
rbs_bt_executor/bt_trees/gripper_tree.xml
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="GripperTree">
|
||||
<BehaviorTree ID="GripperTree">
|
||||
<Sequence>
|
||||
<Action ID = "GripperControl"
|
||||
pose = "open"
|
||||
server_name="/gripper_control"
|
||||
server_timeout="10"
|
||||
cancel_timeout="5" />
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<TreeNodesModel>
|
||||
<Action ID = "GripperControl">
|
||||
<input_port name = "pose"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
</root>
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="BehaviorTree">
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="BehaviorTree">
|
||||
<Sequence>
|
||||
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
|
||||
<ReactiveSequence>
|
||||
<Sequence>
|
||||
<SubTree ID="Pick"/>
|
||||
<SubTree ID="Place"/>
|
||||
</Sequence>
|
||||
<Action ID="SetNextObject" new_object="ObjectName"/>
|
||||
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
|
||||
</ReactiveSequence>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="Pick">
|
||||
<Sequence>
|
||||
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
|
||||
<Action ID="GripperCommand" command="open"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
|
||||
<Action ID="GripperCommand" command="close"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="Place">
|
||||
<Sequence>
|
||||
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
|
||||
<Action ID="GripperCommand" command="open"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="GetPickPlacePoses">
|
||||
<output_port default="GraspPose" name="GraspPose"/>
|
||||
<inout_port default="ObjectPose" name="ObjectName"/>
|
||||
<output_port default="PlacePose" name="PlacePose"/>
|
||||
<output_port default="PostGraspPose" name="PostGraspPose"/>
|
||||
<output_port default="PostPlacePose" name="PostPlacePose"/>
|
||||
<output_port default="PreGraspPose" name="PreGraspPose"/>
|
||||
<output_port default="PrePlacePose" name="PrePlacePose"/>
|
||||
</Action>
|
||||
<Action ID="GripperCommand">
|
||||
<input_port default="open" name="command"/>
|
||||
</Action>
|
||||
<Action ID="MoveToPose">
|
||||
<input_port default="cartesian" name="motion_type"/>
|
||||
<input_port default="PreGraspPose" name="pose"/>
|
||||
</Action>
|
||||
<SubTree ID="Pick"/>
|
||||
<SubTree ID="Place"/>
|
||||
<Action ID="SetNextObject">
|
||||
<inout_port name="new_object"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
<!-- ////////// -->
|
||||
</root>
|
||||
50
rbs_bt_executor/bt_trees/pick_and_place_test.xml
Normal file
50
rbs_bt_executor/bt_trees/pick_and_place_test.xml
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="BehaviorTree">
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="BehaviorTree">
|
||||
<ReactiveSequence>
|
||||
<SubTree ID="Pick"/>
|
||||
<SubTree ID="Place"/>
|
||||
</ReactiveSequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="Pick">
|
||||
<Sequence>
|
||||
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="{GraspPose}" PostPose="{PostGraspPose}" PrePose="{PreGraspPose}" name="box1"/>
|
||||
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
|
||||
<Action ID="GripperCommand" command="open"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
|
||||
<Action ID="GripperCommand" command="close"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="Place">
|
||||
<Sequence>
|
||||
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="PlacePose" PostPose="PostPlacePose" PrePose="PrePlacePose" name="GetPlacePoses"/>
|
||||
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
|
||||
<Action ID="GripperCommand" command="open"/>
|
||||
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="GetPickPlacePoses">
|
||||
<input_port default="ObjectPose" name="ObjectPose"/>
|
||||
<output_port default="GraspPose" name="Pose"/>
|
||||
<output_port default="PostGraspPose" name="PostPose"/>
|
||||
<output_port default="PreGraspPose" name="PrePose"/>
|
||||
</Action>
|
||||
<Action ID="GripperCommand">
|
||||
<input_port default="open" name="command"/>
|
||||
</Action>
|
||||
<Action ID="MoveToPose">
|
||||
<input_port default="cartesian" name="motion_type"/>
|
||||
<input_port default="PreGraspPose" name="pose"/>
|
||||
</Action>
|
||||
<SubTree ID="Pick"/>
|
||||
<SubTree ID="Place"/>
|
||||
</TreeNodesModel>
|
||||
<!-- ////////// -->
|
||||
</root>
|
||||
|
|
@ -2,36 +2,68 @@
|
|||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence>
|
||||
<Action ID="MoveToPose"
|
||||
robot_name="ur_manipulator"
|
||||
pose="pose1"
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box1"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostGraspPose2="{PostGraspPose2}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
PrePlaceJointState="{PrePlaceJointState}"
|
||||
NextObject = "{NextObject}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="AddPlanningSceneObject" ObjectName="{NextObject}" pose="{PlacePose}" server_name="/add_planing_scene_object" server_timeout="1000"/> -->
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Repeat num_cycles="5">
|
||||
<Sequence>
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="{NextObject}"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostGraspPose2="{PostGraspPose2}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
PrePlaceJointState="{PrePlaceJointState}"
|
||||
NextObject = "{NextObject}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
|
||||
server_name="/move_topose"
|
||||
server_timeout="1000"
|
||||
cancel_timeout="500" />
|
||||
<Action ID="MoveToPose"
|
||||
robot_name="ur_manipulator"
|
||||
pose="pose2"
|
||||
|
||||
|
||||
server_name="/move_topose"
|
||||
server_timeout="1000"
|
||||
cancel_timeout="500" />
|
||||
<Action ID="MoveToPose"
|
||||
robot_name="ur_manipulator"
|
||||
pose="pose3"
|
||||
|
||||
server_name="/move_topose"
|
||||
server_timeout="1000"
|
||||
cancel_timeout="500" />
|
||||
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<TreeNodesModel>
|
||||
<Action ID="MoveToPose">
|
||||
<input_port name="robot_name"/>
|
||||
<input_port name="pose"/>
|
||||
<input_port name="pose_name"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
</root>
|
||||
Loading…
Add table
Add a link
Reference in a new issue