Skip to content

Commit 790eb27

Browse files
jeremydbronsonjeremydbronson
authored
Merge pull request #49 from PickNikRobotics/improve_lab_sim_examples
Improve lab sim example objectives with story telling
2 parents 3370096 + c22b920 commit 790eb27

17 files changed

+784
-210
lines changed

‎src/lab_sim/config/moveit/picknik_ur.srdf‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
<disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>
5353
<disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
5454
<disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
55+
<disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Adjacent"/>
5556
<disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
5657
<disable_collisions link1="forearm_pinch_link" link2="forearm_link" reason="Adjacent"/>
5758
<disable_collisions link1="forearm_pinch_link" link2="tool_changer_link" reason="Never"/>

‎src/lab_sim/description/picknik_ur.xacro‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1202,6 +1202,7 @@
12021202
<param name="render_publish_rate">10</param>
12031203
<param name="tf_publish_rate">60</param>
12041204
<param name="lidar_publish_rate">10</param>
1205+
<param name="mujoco_viewer">false</param>
12051206
</hardware>
12061207
</ros2_control>
12071208
</robot>

‎src/lab_sim/objectives/3_waypoint_pick_and_place.xml‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,12 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="3 Waypoints Pick and Place">
23
<!--//////////-->
34
<BehaviorTree
45
ID="3 Waypoints Pick and Place"
56
_description="Basic example of repeatedly grabbing a small (invisible) object, placing it at desired destination, and then moving back to a home position"
6-
_favorite="false"
7+
_favorite="true"
78
>
89
<Control ID="Sequence">
9-
<!--Reset the planning scene to ensure no old collision objects are hanging around-->
10-
<Action ID="ClearSnapshot" />
1110
<!--Setup the environment to its "home" configuration - move to center and open gripper-->
1211
<SubTree
1312
ID="Move to Waypoint"

‎src/lab_sim/objectives/_scan_scene.xml‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="_Scan Scene">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="_Scan Scene"
6+
_description="Move robot our of scene to enable full snapshot of scene."
7+
_favorite="true"
8+
>
9+
<Control ID="Sequence" name="TopLevelSequence">
10+
<Action ID="ClearSnapshot" />
11+
<SubTree
12+
ID="Move to Waypoint"
13+
_collapsed="true"
14+
joint_group_name="manipulator"
15+
planner_interface="moveit_default"
16+
controller_names="/joint_trajectory_controller"
17+
waypoint_name="Park Far Right"
18+
/>
19+
<SubTree ID="Take scene camera snapshot" _collapsed="true" />
20+
<SubTree
21+
ID="Move to Waypoint"
22+
_collapsed="true"
23+
joint_group_name="manipulator"
24+
planner_interface="moveit_default"
25+
controller_names="/joint_trajectory_controller"
26+
waypoint_name="Look at Table"
27+
/>
28+
</Control>
29+
</BehaviorTree>
30+
<TreeNodesModel>
31+
<SubTree ID="_Scan Scene" />
32+
</TreeNodesModel>
33+
</root>

‎src/lab_sim/objectives/constrained_pick_and_place_subtree.xml‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="Constrained Pick and Place Subtree"
5+
>
6+
<!--//////////-->
7+
<BehaviorTree
8+
ID="Constrained Pick and Place Subtree"
9+
_subtreeOnly="true"
10+
_description=""
11+
>
12+
<Control ID="Sequence">
13+
<!--Create a special type of motion planning configuration that includes an upwards orientation requirement-->
14+
<Action
15+
ID="InitializeMotionConstraints"
16+
constraints_name="gripper pointing down"
17+
constraints="{constraints}"
18+
/>
19+
<Action
20+
ID="AppendOrientationConstraint"
21+
config_file_name="my_constraints.yaml"
22+
constraints="{constraints}"
23+
/>
24+
<!--Move to pick location-->
25+
<!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
26+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
27+
<SubTree
28+
ID="Move to Waypoint"
29+
waypoint_name="{pre_pick}"
30+
joint_group_name="manipulator"
31+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
32+
planner_interface="moveit_default"
33+
constraints="{constraints}"
34+
/>
35+
</Decorator>
36+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
37+
<SubTree
38+
ID="Move to Waypoint"
39+
waypoint_name="{pick}"
40+
joint_group_name="manipulator"
41+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
42+
planner_interface="moveit_default"
43+
constraints="{constraints}"
44+
/>
45+
</Decorator>
46+
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
47+
<Decorator ID="ForceSuccess">
48+
<SubTree ID="Close Gripper" />
49+
</Decorator>
50+
<!--Move to place (drop) location-->
51+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
52+
<SubTree
53+
ID="Move to Waypoint"
54+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
55+
joint_group_name="manipulator"
56+
planner_interface="moveit_default"
57+
constraints="{constraints}"
58+
_collapsed="true"
59+
waypoint_name="{pre_pick}"
60+
/>
61+
</Decorator>
62+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
63+
<SubTree
64+
ID="Move to Waypoint"
65+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
66+
joint_group_name="manipulator"
67+
planner_interface="moveit_default"
68+
constraints="{constraints}"
69+
_collapsed="true"
70+
waypoint_name="{pre_place}"
71+
/>
72+
</Decorator>
73+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
74+
<SubTree
75+
ID="Move to Waypoint"
76+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
77+
joint_group_name="manipulator"
78+
planner_interface="moveit_default"
79+
constraints="{constraints}"
80+
_collapsed="true"
81+
waypoint_name="{place}"
82+
/>
83+
</Decorator>
84+
<SubTree ID="Open Gripper" />
85+
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
86+
<SubTree
87+
ID="Move to Waypoint"
88+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
89+
joint_group_name="manipulator"
90+
planner_interface="moveit_default"
91+
constraints="{constraints}"
92+
_collapsed="true"
93+
waypoint_name="{pre_place}"
94+
/>
95+
</Decorator>
96+
</Control>
97+
</BehaviorTree>
98+
<TreeNodesModel>
99+
<SubTree ID="Constrained Pick and Place Subtree">
100+
<inout_port name="pre_pick" default="" />
101+
<inout_port name="pick" default="" />
102+
<inout_port name="pre_place" default="" />
103+
<inout_port name="place" default="" />
104+
</SubTree>
105+
</TreeNodesModel>
106+
</root>

‎src/lab_sim/objectives/constrained_pick_place.xml‎

Lines changed: 32 additions & 136 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,18 @@
11
<?xml version="1.0" encoding="utf-8" ?>
2-
<root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
2+
<root BTCPP_format="4" main_tree_to_execute="Move Beakers to Burners">
33
<!--//////////-->
44
<BehaviorTree
5-
ID="Constrained Pick and Place"
6-
_description="More advanced example of picking and placing an (invisible) object with orientation constraints enforced on the gripper's motion"
7-
_favorite="false"
5+
ID="Move Beakers to Burners"
6+
_description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
7+
_favorite="true"
8+
_subtreeOnly="false"
89
>
910
<Control ID="Sequence">
1011
<SubTree
1112
ID="Move to Waypoint"
1213
_collapsed="true"
13-
waypoint_name="Look at Table"
14+
constraints="{constraints}"
15+
waypoint_name="Beaker 1a - Pre Pick"
1416
joint_group_name="manipulator"
1517
controller_names="/joint_trajectory_controller"
1618
velocity_scale_factor="1.0"
@@ -20,139 +22,33 @@
2022
keep_orientation="false"
2123
/>
2224
<SubTree ID="Open Gripper" />
23-
<Decorator ID="KeepRunningUntilFailure">
24-
<Control ID="Sequence">
25-
<!--Move to pick location-->
26-
<SubTree
27-
ID="Move to Waypoint"
28-
waypoint_name="Above Pick Cube"
29-
joint_group_name="manipulator"
30-
controller_names="/joint_trajectory_controller"
31-
velocity_scale_factor="1.0"
32-
acceleration_scale_factor="1.0"
33-
link_padding="0.01"
34-
keep_orientation_tolerance="0.05"
35-
keep_orientation="true"
36-
/>
37-
<SubTree
38-
ID="Move to Waypoint"
39-
waypoint_name="Pick Cube"
40-
joint_group_name="manipulator"
41-
controller_names="/joint_trajectory_controller"
42-
velocity_scale_factor="1.0"
43-
acceleration_scale_factor="1.0"
44-
link_padding="0.01"
45-
keep_orientation_tolerance="0.05"
46-
keep_orientation="true"
47-
/>
48-
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
49-
<Decorator ID="ForceSuccess">
50-
<SubTree ID="Close Gripper" />
51-
</Decorator>
52-
<!--Move to place (drop) location-->
53-
<SubTree
54-
ID="Move to Waypoint"
55-
controller_names="/joint_trajectory_controller"
56-
joint_group_name="manipulator"
57-
_collapsed="true"
58-
waypoint_name="Above Pick Cube"
59-
velocity_scale_factor="1.0"
60-
acceleration_scale_factor="1.0"
61-
link_padding="0.01"
62-
keep_orientation_tolerance="0.05"
63-
keep_orientation="true"
64-
/>
65-
<SubTree
66-
ID="Move to Waypoint"
67-
controller_names="/joint_trajectory_controller"
68-
joint_group_name="manipulator"
69-
_collapsed="true"
70-
waypoint_name="Above Place Cube"
71-
velocity_scale_factor="1.0"
72-
acceleration_scale_factor="1.0"
73-
link_padding="0.01"
74-
keep_orientation_tolerance="0.05"
75-
keep_orientation="true"
76-
/>
77-
<SubTree
78-
ID="Move to Waypoint"
79-
controller_names="/joint_trajectory_controller"
80-
joint_group_name="manipulator"
81-
_collapsed="true"
82-
waypoint_name="Place Cube"
83-
velocity_scale_factor="1.0"
84-
acceleration_scale_factor="1.0"
85-
link_padding="0.01"
86-
keep_orientation_tolerance="0.05"
87-
keep_orientation="true"
88-
/>
89-
<SubTree ID="Open Gripper" />
90-
<SubTree
91-
ID="Move to Waypoint"
92-
controller_names="/joint_trajectory_controller"
93-
joint_group_name="manipulator"
94-
_collapsed="true"
95-
waypoint_name="Above Place Cube"
96-
velocity_scale_factor="1.0"
97-
acceleration_scale_factor="1.0"
98-
link_padding="0.01"
99-
keep_orientation_tolerance="0.05"
100-
keep_orientation="true"
101-
/>
102-
<SubTree
103-
ID="Move to Waypoint"
104-
controller_names="/joint_trajectory_controller"
105-
joint_group_name="manipulator"
106-
_collapsed="true"
107-
waypoint_name="Place Cube"
108-
velocity_scale_factor="1.0"
109-
acceleration_scale_factor="1.0"
110-
link_padding="0.01"
111-
keep_orientation_tolerance="0.05"
112-
keep_orientation="true"
113-
/>
114-
<SubTree ID="Close Gripper" />
115-
<SubTree
116-
ID="Move to Waypoint"
117-
controller_names="/joint_trajectory_controller"
118-
joint_group_name="manipulator"
119-
_collapsed="true"
120-
waypoint_name="Above Place Cube"
121-
velocity_scale_factor="1.0"
122-
acceleration_scale_factor="1.0"
123-
link_padding="0.01"
124-
keep_orientation_tolerance="0.05"
125-
keep_orientation="true"
126-
/>
127-
<SubTree
128-
ID="Move to Waypoint"
129-
controller_names="/joint_trajectory_controller"
130-
joint_group_name="manipulator"
131-
_collapsed="true"
132-
waypoint_name="Above Pick Cube"
133-
velocity_scale_factor="1.0"
134-
acceleration_scale_factor="1.0"
135-
link_padding="0.01"
136-
keep_orientation_tolerance="0.05"
137-
keep_orientation="true"
138-
/>
139-
<SubTree
140-
ID="Move to Waypoint"
141-
waypoint_name="Pick Cube"
142-
joint_group_name="manipulator"
143-
controller_names="/joint_trajectory_controller"
144-
velocity_scale_factor="1.0"
145-
acceleration_scale_factor="1.0"
146-
link_padding="0.01"
147-
keep_orientation_tolerance="0.05"
148-
keep_orientation="true"
149-
/>
150-
<SubTree ID="Open Gripper" />
151-
</Control>
152-
</Decorator>
25+
<SubTree
26+
ID="Constrained Pick and Place Subtree"
27+
pre_pick="Beaker 1a - Pre Pick"
28+
pick="Beaker 1b - Pick"
29+
pre_place="Beaker 1c - Pre Place"
30+
place="Beaker 1d - Place"
31+
name="Beaker 1"
32+
/>
33+
<SubTree
34+
ID="Constrained Pick and Place Subtree"
35+
pre_pick="Beaker 2a - Pre Pick"
36+
pick="Beaker 2b - Pick"
37+
pre_place="Beaker 2c - Pre Place"
38+
place="Beaker 2d - Place"
39+
name="Beaker 2"
40+
/>
41+
<SubTree
42+
ID="Constrained Pick and Place Subtree"
43+
pre_pick="Beaker 3a - Pre Pick"
44+
pick="Beaker 3b - Pick"
45+
pre_place="Beaker 3c - Pre Place"
46+
place="Beaker 3d - Place"
47+
name="Beaker 3"
48+
/>
15349
</Control>
15450
</BehaviorTree>
15551
<TreeNodesModel>
156-
<SubTree ID="Constrained Pick and Place" />
52+
<SubTree ID="Move Beakers to Burners" />
15753
</TreeNodesModel>
15854
</root>

‎src/lab_sim/objectives/joint_diagnostic.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<BehaviorTree
44
ID="Joint Diagnostic"
55
_description="Example of cycling through the min and max limits of a single joint, for hardware testing"
6-
_favorite="false"
6+
_favorite="true"
77
>
88
<Control ID="Sequence" name="TopLevelSequence">
99
<!--Setup by moving home-->

‎src/lab_sim/objectives/load_and_execute_joint_trajectory.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<BehaviorTree
44
ID="Load and Execute Joint Trajectory"
55
_description="Load a trajectory that was saved as a YAML file"
6-
_favorite="false"
6+
_favorite="true"
77
_subtreeOnly="false"
88
>
99
<Control ID="Sequence" name="TopLevelSequence">

‎src/lab_sim/objectives/look_at_table.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<BehaviorTree
44
ID="Look at table"
55
_description="Move the arm to a position that faces the table surface"
6-
_favorite="true"
6+
_favorite="false"
77
_subtreeOnly="false"
88
>
99
<Control ID="Sequence" name="TopLevelSequence">

0 commit comments

Comments
 (0)