Skip to content

Commit 17eb85f

Browse files
dyackzandyackzan
authored
Merge pull request #63 from PickNikRobotics/pro-rrt-default
Use Pro-RRT directly under `MoveToJointState` and `MoveToWaypoint`
2 parents c20d519 + 3084b0e commit 17eb85f

24 files changed

+491
-500
lines changed

‎src/fanuc_sim/objectives/move_to_joint_state.xml‎

Lines changed: 26 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,37 +1,45 @@
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="Move to Joint State">
33
<BehaviorTree
44
ID="Move to Joint State"
55
_description="Move to a specified joint state"
66
_subtreeOnly="true"
77
>
88
<Control ID="Sequence">
9+
<Action ID="ActivateControllers" controller_names="{controller_names}" />
910
<Action
10-
ID="InitializeMTCTask"
11-
task_id="move_to_joint_state"
12-
controller_names="/joint_trajectory_controller"
13-
task="{move_to_waypoint_task}"
11+
ID="PlanToJointGoal"
12+
joint_goal="{target_joint_state}"
13+
keep_orientation_tolerance="{keep_orientation_tolerance}"
14+
link_padding="{link_padding}"
15+
keep_orientation="{keep_orientation}"
16+
acceleration_scale_factor="{acceleration_scale_factor}"
17+
velocity_scale_factor="{velocity_scale_factor}"
18+
planning_group_name="{joint_group_name}"
1419
/>
15-
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
1620
<Action
17-
ID="SetupMTCMoveToJointState"
18-
joint_state="{target_joint_state}"
19-
name="SetupMTCMoveToJointState_First"
20-
planning_group_name="manipulator"
21-
task="{move_to_waypoint_task}"
22-
planner_interface="moveit_default"
21+
ID="ExecuteFollowJointTrajectory"
22+
execute_follow_joint_trajectory_action_name="{controller_action_server}"
2323
/>
24-
<Action
25-
ID="PlanMTCTask"
26-
solution="{move_to_waypoint_solution}"
27-
task="{move_to_waypoint_task}"
28-
/>
29-
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}" />
3024
</Control>
3125
</BehaviorTree>
3226
<TreeNodesModel>
3327
<SubTree ID="Move to Joint State">
3428
<input_port name="target_joint_state" default="{target_joint_state}" />
29+
<input_port name="joint_group_name" default="manipulator" />
30+
<input_port name="link_padding" default="0.01" />
31+
<input_port name="keep_orientation_tolerance" default="0.05" />
32+
<input_port name="keep_orientation" default="false" />
33+
<input_port name="velocity_scale_factor" default="1.0" />
34+
<input_port name="acceleration_scale_factor" default="1.0" />
35+
<input_port
36+
name="controller_names"
37+
default="joint_trajectory_controller"
38+
/>
39+
<input_port
40+
name="controller_action_server"
41+
default="/joint_trajectory_controller/follow_joint_trajectory"
42+
/>
3543
<MetadataFields>
3644
<Metadata subcategory="Motion - Execute" />
3745
</MetadataFields>

‎src/fanuc_sim/objectives/move_to_waypoint.xml‎

Lines changed: 22 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Move to Waypoint">
23
<BehaviorTree
34
ID="Move to Waypoint"
@@ -11,42 +12,38 @@
1112
waypoint_name="{waypoint_name}"
1213
joint_group_name="{joint_group_name}"
1314
/>
14-
<Action
15-
ID="InitializeMTCTask"
16-
task_id="move_to_named_pose"
15+
<SubTree
16+
ID="Move to Joint State"
17+
_collapsed="true"
18+
velocity_scale_factor="{velocity_scale_factor}"
19+
acceleration_scale_factor="{acceleration_scale_factor}"
20+
link_padding="{link_padding}"
21+
keep_orientation_tolerance="{keep_orientation_tolerance}"
22+
keep_orientation="{keep_orientation}"
23+
target_joint_state="{target_joint_state}"
1724
controller_names="{controller_names}"
18-
task="{move_to_waypoint_task}"
19-
/>
20-
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}" />
21-
<Action
22-
ID="SetupMTCMoveToJointState"
23-
joint_state="{target_joint_state}"
24-
name="SetupMTCMoveToJointState"
25-
planning_group_name="{joint_group_name}"
26-
planner_interface="{planner_interface}"
27-
constraints="{constraints}"
28-
task="{move_to_waypoint_task}"
29-
/>
30-
<Action
31-
ID="PlanMTCTask"
32-
solution="{move_to_waypoint_solution}"
33-
task="{move_to_waypoint_task}"
34-
/>
35-
<Action
36-
ID="BlockingExecuteMTCTask"
37-
solution="{move_to_waypoint_solution}"
25+
joint_group_name="{joint_group_name}"
26+
controller_action_server="{controller_action_server}"
3827
/>
3928
</Control>
4029
</BehaviorTree>
4130
<TreeNodesModel>
4231
<SubTree ID="Move to Waypoint">
43-
<input_port name="waypoint_name" default="Point A" />
32+
<input_port name="waypoint_name" default="" />
4433
<input_port name="joint_group_name" default="manipulator" />
45-
<input_port name="planner_interface" default="moveit_default" />
34+
<input_port name="link_padding" default="0.01" />
35+
<input_port name="keep_orientation_tolerance" default="0.05" />
36+
<input_port name="keep_orientation" default="false" />
37+
<input_port name="velocity_scale_factor" default="1.0" />
38+
<input_port name="acceleration_scale_factor" default="1.0" />
4639
<input_port
4740
name="controller_names"
4841
default="/joint_trajectory_controller"
4942
/>
43+
<input_port
44+
name="controller_action_server"
45+
default="/joint_trajectory_controller/follow_joint_trajectory"
46+
/>
5047
<MetadataFields>
5148
<Metadata subcategory="Motion - Execute" />
5249
</MetadataFields>

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -195,4 +195,7 @@
195195
<disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
196196
<disable_collisions link1="wrist_2_link" link2="wrist_mounted_camera_link" reason="Never"/>
197197
<disable_collisions link1="wrist_3_link" link2="wrist_mounted_camera_link" reason="Never"/>
198+
<disable_collisions link1="robotiq_85_right_finger_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
199+
<disable_collisions link1="robotiq_85_left_finger_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
200+
<disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Never"/>
198201
</robot>

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

Lines changed: 9 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -13,9 +13,7 @@
1313
ID="Move to Waypoint"
1414
waypoint_name="Look at Table"
1515
joint_group_name="manipulator"
16-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
17-
planner_interface="moveit_default"
18-
constraints="{constraints}"
16+
controller_names="/joint_trajectory_controller"
1917
/>
2018
<Action
2119
ID="MoveGripperAction"
@@ -31,9 +29,7 @@
3129
ID="Move to Waypoint"
3230
waypoint_name="Pick Cube"
3331
joint_group_name="manipulator"
34-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
35-
planner_interface="moveit_default"
36-
constraints="{constraints}"
32+
controller_names="/joint_trajectory_controller"
3733
/>
3834
<Action
3935
ID="MoveGripperAction"
@@ -44,16 +40,12 @@
4440
ID="Move to Waypoint"
4541
waypoint_name="Look at Table"
4642
joint_group_name="manipulator"
47-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
48-
planner_interface="moveit_default"
49-
constraints="{constraints}"
43+
controller_names="/joint_trajectory_controller"
5044
/>
5145
<SubTree
5246
ID="Move to Waypoint"
5347
joint_group_name="manipulator"
54-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
55-
planner_interface="moveit_default"
56-
constraints="{constraints}"
48+
controller_names="/joint_trajectory_controller"
5749
waypoint_name="Place Cube"
5850
/>
5951
<Action
@@ -65,9 +57,7 @@
6557
ID="Move to Waypoint"
6658
waypoint_name="Look at Table"
6759
joint_group_name="manipulator"
68-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
69-
planner_interface="moveit_default"
70-
constraints="{constraints}"
60+
controller_names="/joint_trajectory_controller"
7161
/>
7262
</Control>
7363
<!--Pick object from where it was placed, put it down on the original location, and go back to center pose-->
@@ -76,9 +66,7 @@
7666
ID="Move to Waypoint"
7767
waypoint_name="Place Cube"
7868
joint_group_name="manipulator"
79-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
80-
planner_interface="moveit_default"
81-
constraints="{constraints}"
69+
controller_names="/joint_trajectory_controller"
8270
/>
8371
<Action
8472
ID="MoveGripperAction"
@@ -89,17 +77,13 @@
8977
ID="Move to Waypoint"
9078
waypoint_name="Look at Table"
9179
joint_group_name="manipulator"
92-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
93-
planner_interface="moveit_default"
94-
constraints="{constraints}"
80+
controller_names="/joint_trajectory_controller"
9581
/>
9682
<SubTree
9783
ID="Move to Waypoint"
9884
waypoint_name="Pick Cube"
9985
joint_group_name="manipulator"
100-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
101-
planner_interface="moveit_default"
102-
constraints="{constraints}"
86+
controller_names="/joint_trajectory_controller"
10387
/>
10488
<Action
10589
ID="MoveGripperAction"
@@ -110,9 +94,7 @@
11094
ID="Move to Waypoint"
11195
waypoint_name="Look at Table"
11296
joint_group_name="manipulator"
113-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
114-
planner_interface="moveit_default"
115-
constraints="{constraints}"
97+
controller_names="/joint_trajectory_controller"
11698
/>
11799
</Control>
118100
</Control>

0 commit comments

Comments
 (0)