Skip to content

Commit 6d911c1

Browse files
mariopratsmarioprats
authored
Merge pull request #94 from PickNikRobotics/remove-planning-retries
remove RetryUntilSuccessful decorators around planning Behaviors Former-commit-id: fd812ac
2 parents c19f90a + 99d215e commit 6d911c1

File tree

4 files changed

+221
-220
lines changed

4 files changed

+221
-220
lines changed

‎src/lab_sim/objectives/constrained_pick_and_place_subtree.xml‎

Lines changed: 90 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -11,91 +11,108 @@
1111
>
1212
<Control ID="Sequence">
1313
<!--Move to pick location-->
14-
<!--Note: Sampling based planners can be non-deterministic. The retry decorator improves the likelihood of success-->
15-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
16-
<SubTree
17-
ID="Move to Waypoint"
18-
waypoint_name="{pre_pick}"
19-
joint_group_name="manipulator"
20-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
21-
planner_interface="moveit_default"
22-
keep_orientation="true"
23-
keep_orientation_tolerance="0.2"
24-
/>
25-
</Decorator>
26-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
27-
<SubTree
28-
ID="Move to Waypoint"
29-
waypoint_name="{pick}"
30-
joint_group_name="manipulator"
31-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
32-
planner_interface="moveit_default"
33-
keep_orientation="true"
34-
keep_orientation_tolerance="0.2"
35-
/>
36-
</Decorator>
14+
<SubTree
15+
ID="Move to Waypoint"
16+
waypoint_name="{pre_pick}"
17+
joint_group_name="manipulator"
18+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
19+
planner_interface="moveit_default"
20+
keep_orientation="true"
21+
keep_orientation_tolerance="0.2"
22+
acceleration_scale_factor="1.0"
23+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
24+
link_padding="0.01"
25+
velocity_scale_factor="1.0"
26+
/>
27+
<SubTree
28+
ID="Move to Waypoint"
29+
waypoint_name="{pick}"
30+
joint_group_name="manipulator"
31+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
32+
planner_interface="moveit_default"
33+
keep_orientation="true"
34+
keep_orientation_tolerance="0.2"
35+
acceleration_scale_factor="1.0"
36+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
37+
link_padding="0.01"
38+
velocity_scale_factor="1.0"
39+
/>
3740
<!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
3841
<Decorator ID="ForceSuccess">
3942
<SubTree ID="Close Gripper" />
4043
</Decorator>
4144
<!--Move to place (drop) location-->
42-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
43-
<SubTree
44-
ID="Move to Waypoint"
45-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
46-
joint_group_name="manipulator"
47-
planner_interface="moveit_default"
48-
keep_orientation="true"
49-
keep_orientation_tolerance="0.2"
50-
_collapsed="true"
51-
waypoint_name="{pre_pick}"
52-
/>
53-
</Decorator>
54-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
55-
<SubTree
56-
ID="Move to Waypoint"
57-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
58-
joint_group_name="manipulator"
59-
planner_interface="moveit_default"
60-
keep_orientation="true"
61-
keep_orientation_tolerance="0.2"
62-
_collapsed="true"
63-
waypoint_name="{pre_place}"
64-
/>
65-
</Decorator>
66-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
67-
<SubTree
68-
ID="Move to Waypoint"
69-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
70-
joint_group_name="manipulator"
71-
planner_interface="moveit_default"
72-
keep_orientation="true"
73-
keep_orientation_tolerance="0.2"
74-
_collapsed="true"
75-
waypoint_name="{place}"
76-
/>
77-
</Decorator>
45+
<SubTree
46+
ID="Move to Waypoint"
47+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
48+
joint_group_name="manipulator"
49+
planner_interface="moveit_default"
50+
keep_orientation="true"
51+
keep_orientation_tolerance="0.2"
52+
_collapsed="true"
53+
waypoint_name="{pre_pick}"
54+
acceleration_scale_factor="1.0"
55+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
56+
link_padding="0.01"
57+
velocity_scale_factor="1.0"
58+
/>
59+
<SubTree
60+
ID="Move to Waypoint"
61+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
62+
joint_group_name="manipulator"
63+
planner_interface="moveit_default"
64+
keep_orientation="true"
65+
keep_orientation_tolerance="0.2"
66+
_collapsed="true"
67+
waypoint_name="{pre_place}"
68+
acceleration_scale_factor="1.0"
69+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
70+
link_padding="0.01"
71+
velocity_scale_factor="1.0"
72+
/>
73+
<SubTree
74+
ID="Move to Waypoint"
75+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
76+
joint_group_name="manipulator"
77+
planner_interface="moveit_default"
78+
keep_orientation="true"
79+
keep_orientation_tolerance="0.2"
80+
_collapsed="true"
81+
waypoint_name="{place}"
82+
acceleration_scale_factor="1.0"
83+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
84+
link_padding="0.01"
85+
velocity_scale_factor="1.0"
86+
/>
7887
<SubTree ID="Open Gripper" />
79-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
80-
<SubTree
81-
ID="Move to Waypoint"
82-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
83-
joint_group_name="manipulator"
84-
planner_interface="moveit_default"
85-
keep_orientation="true"
86-
keep_orientation_tolerance="0.2"
87-
_collapsed="true"
88-
waypoint_name="{pre_place}"
89-
/>
90-
</Decorator>
88+
<SubTree
89+
ID="Move to Waypoint"
90+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
91+
joint_group_name="manipulator"
92+
planner_interface="moveit_default"
93+
keep_orientation="true"
94+
keep_orientation_tolerance="0.2"
95+
_collapsed="true"
96+
waypoint_name="{pre_place}"
97+
acceleration_scale_factor="1.0"
98+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
99+
link_padding="0.01"
100+
velocity_scale_factor="1.0"
101+
/>
91102
</Control>
92103
</BehaviorTree>
93104
<TreeNodesModel>
94105
<SubTree ID="Constrained Pick and Place Subtree">
95-
<inout_port name="pre_pick" default="" />
96106
<inout_port name="pick" default="" />
97-
<inout_port name="pre_place" default="" />
98107
<inout_port name="place" default="" />
108+
<inout_port name="pre_pick" default="" />
109+
<inout_port name="pre_place" default="" />
110+
<MetadataFields>
111+
<Metadata runnable="false" />
112+
</MetadataFields>
113+
<MetadataFields>
114+
<Metadata runnable="false" />
115+
</MetadataFields>
99116
</SubTree>
100117
</TreeNodesModel>
101118
</root>

‎src/moveit_pro_kinova_configs/kinova_gen3_mujoco_config/objectives/constrained_pick_place.xml‎

Lines changed: 29 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
1+
<?xml version="1.0" encoding="utf-8" ?>
22
<root BTCPP_format="4" main_tree_to_execute="Constrained Pick and Place">
3-
<!-- ////////// -->
3+
<!--//////////-->
44
<BehaviorTree
55
ID="Constrained Pick and Place"
66
_description="Pick and place an object with constraints on the gripper's range of motion"
@@ -10,27 +10,36 @@
1010
<Control ID="Sequence">
1111
<Control ID="Sequence" name="TopLevelSequence">
1212
<SubTree ID="Open Gripper" />
13-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
14-
<SubTree
15-
ID="Move to Waypoint"
16-
waypoint_name="Grasp Right"
17-
joint_group_name="manipulator"
18-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
19-
/>
20-
</Decorator>
13+
<SubTree
14+
ID="Move to Waypoint"
15+
waypoint_name="Pick"
16+
joint_group_name="manipulator"
17+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
18+
acceleration_scale_factor="1.0"
19+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
20+
keep_orientation="false"
21+
keep_orientation_tolerance="0.05"
22+
link_padding="0.01"
23+
velocity_scale_factor="1.0"
24+
/>
2125
<SubTree ID="Close Gripper" />
22-
<Decorator ID="RetryUntilSuccessful" num_attempts="3">
23-
<SubTree
24-
ID="Move to Waypoint"
25-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
26-
joint_group_name="manipulator"
27-
waypoint_name="Grasp Left"
28-
keep_orientation="true"
29-
keep_orientation_tolerance="0.2"
30-
/>
31-
</Decorator>
26+
<SubTree
27+
ID="Move to Waypoint"
28+
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
29+
joint_group_name="manipulator"
30+
waypoint_name="Place"
31+
keep_orientation="true"
32+
keep_orientation_tolerance="0.2"
33+
acceleration_scale_factor="1.0"
34+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
35+
link_padding="0.01"
36+
velocity_scale_factor="1.0"
37+
/>
3238
</Control>
3339
</Control>
3440
</Decorator>
3541
</BehaviorTree>
42+
<TreeNodesModel>
43+
<SubTree ID="Constrained Pick and Place" />
44+
</TreeNodesModel>
3645
</root>

0 commit comments

Comments
 (0)