Skip to content

Commit 1cc1696

Browse files
dyackzandyackzan
committed
Don't use MTC in Plan and Save Trajectory objective
The MTC plan's computational complexity increases exponentially when several MoveToPose stages, each with multiple solutions are stacked on top of each other. Using PlanCartesianPath with a validation step to ensure no collisions is a much less computationally complex way to navigate through poses. Former-commit-id: 87262df
1 parent 4baa292 commit 1cc1696

File tree

4 files changed

+106
-36
lines changed

4 files changed

+106
-36
lines changed

‎src/lab_sim/objectives/create_pose_vector.xml‎

Lines changed: 56 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,20 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
12
<root BTCPP_format="4" main_tree_to_execute="Create Pose Vector">
23
<!--//////////-->
34
<BehaviorTree
45
ID="Create Pose Vector"
56
_favorite="false"
67
_subtreeOnly="true"
78
_description=""
9+
target_poses="{target_poses}"
810
>
911
<Control ID="Sequence">
1012
<Action
1113
ID="CreateStampedPose"
1214
orientation_xyzw="0;-1;0;0"
13-
position_xyz="1;0.5;1"
15+
position_xyz="0.75;0.5;0.9"
1416
stamped_pose="{stamped_pose}"
17+
reference_frame="world"
1518
/>
1619
<Action
1720
ID="AddPoseStampedToVector"
@@ -23,6 +26,31 @@
2326
orientation_xyzw="0;-1;0;0"
2427
position_xyz="0.5;0.5;0.8"
2528
stamped_pose="{stamped_pose}"
29+
reference_frame="world"
30+
/>
31+
<Action
32+
ID="AddPoseStampedToVector"
33+
vector="{target_poses}"
34+
input="{stamped_pose}"
35+
/>
36+
<Action
37+
ID="CreateStampedPose"
38+
orientation_xyzw="0;-1;0;0"
39+
position_xyz="0.25;0.5;0.7"
40+
stamped_pose="{stamped_pose}"
41+
reference_frame="world"
42+
/>
43+
<Action
44+
ID="AddPoseStampedToVector"
45+
vector="{target_poses}"
46+
input="{stamped_pose}"
47+
/>
48+
<Action
49+
ID="CreateStampedPose"
50+
orientation_xyzw="0;-1;0;0"
51+
position_xyz="0;0.5;0.6"
52+
stamped_pose="{stamped_pose}"
53+
reference_frame="world"
2654
/>
2755
<Action
2856
ID="AddPoseStampedToVector"
@@ -34,6 +62,31 @@
3462
orientation_xyzw="0;-1;0;0"
3563
position_xyz="-0.25;0.5;0.7"
3664
stamped_pose="{stamped_pose}"
65+
reference_frame="world"
66+
/>
67+
<Action
68+
ID="AddPoseStampedToVector"
69+
vector="{target_poses}"
70+
input="{stamped_pose}"
71+
/>
72+
<Action
73+
ID="CreateStampedPose"
74+
orientation_xyzw="0;-1;0;0"
75+
position_xyz="-0.5;0.5;0.8"
76+
stamped_pose="{stamped_pose}"
77+
reference_frame="world"
78+
/>
79+
<Action
80+
ID="AddPoseStampedToVector"
81+
vector="{target_poses}"
82+
input="{stamped_pose}"
83+
/>
84+
<Action
85+
ID="CreateStampedPose"
86+
orientation_xyzw="0;-1;0;0"
87+
position_xyz="-0.75;0.5;0.9"
88+
stamped_pose="{stamped_pose}"
89+
reference_frame="world"
3790
/>
3891
<Action
3992
ID="AddPoseStampedToVector"
@@ -45,6 +98,7 @@
4598
orientation_xyzw="0;-1;0;0"
4699
position_xyz="-1;0.5;1"
47100
stamped_pose="{stamped_pose}"
101+
reference_frame="world"
48102
/>
49103
<Action
50104
ID="AddPoseStampedToVector"
@@ -58,6 +112,7 @@
58112
<output_port name="target_poses" default="{target_poses}" />
59113
<MetadataFields>
60114
<Metadata subcategory="Application - Basic Examples" />
115+
<Metadata runnable="false" />
61116
</MetadataFields>
62117
</SubTree>
63118
</TreeNodesModel>

‎src/lab_sim/objectives/load_and_execute_joint_trajectory.xml‎

Lines changed: 32 additions & 2 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="Load and Execute Joint Trajectory">
23
<!--//////////-->
34
<BehaviorTree
@@ -7,26 +8,55 @@
78
_subtreeOnly="false"
89
>
910
<Control ID="Sequence" name="TopLevelSequence">
10-
<!-- Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path -->
11+
<!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->
1112
<SubTree
1213
ID="Move to Waypoint"
1314
_collapsed="true"
1415
waypoint_name="Workspace Right"
1516
joint_group_name="manipulator"
1617
planner_interface="moveit_default"
1718
controller_names="/joint_trajectory_controller"
19+
acceleration_scale_factor="1.0"
20+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
21+
keep_orientation="false"
22+
keep_orientation_tolerance="0.05"
23+
link_padding="0.01"
24+
velocity_scale_factor="1.0"
1825
/>
1926
<Action
2027
ID="LoadJointTrajectoryFromYaml"
2128
file_path="joint_trajectory.yaml"
29+
output="{joint_trajectory_msg}"
30+
/>
31+
<!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
32+
<Action
33+
ID="GetCurrentPlanningScene"
34+
planning_scene_msg="{planning_scene}"
35+
/>
36+
<Action
37+
ID="ValidateTrajectory"
38+
cartesian_space_step="0.020000"
39+
joint_space_step="0.100000"
40+
joint_trajectory_msg="{joint_trajectory_msg}"
41+
planning_group_name="manipulator"
42+
planning_scene_msg="{planning_scene}"
43+
debug_solution="{debug_solution}"
44+
/>
45+
<Action
46+
ID="ExecuteFollowJointTrajectory"
47+
execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
48+
goal_duration_tolerance="-1.000000"
49+
goal_position_tolerance="0.000000"
50+
goal_time_tolerance="0.000000"
51+
joint_trajectory_msg="{joint_trajectory_msg}"
2252
/>
23-
<Action ID="ExecuteFollowJointTrajectory" />
2453
</Control>
2554
</BehaviorTree>
2655
<TreeNodesModel>
2756
<SubTree ID="Load and Execute Joint Trajectory">
2857
<MetadataFields>
2958
<Metadata subcategory="Application - Basic Examples" />
59+
<Metadata runnable="true" />
3060
</MetadataFields>
3161
</SubTree>
3262
</TreeNodesModel>

‎src/lab_sim/objectives/plan_and_save_trajectory.xml‎

Lines changed: 18 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
_subtreeOnly="false"
99
>
1010
<Control ID="Sequence" name="TopLevelSequence">
11-
<!-- Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position -->
11+
<!--Before we plan we want to make sure we are at a known position, so when we execute the plan it can be done from the same position-->
1212
<SubTree
1313
ID="Move to Waypoint"
1414
_collapsed="true"
@@ -23,45 +23,30 @@
2323
link_padding="0.01"
2424
velocity_scale_factor="1.0"
2525
/>
26-
<!-- Create a vector of poses so we can add them all to the MTC task at once -->
26+
<!--Create a vector of poses we want to move through-->
2727
<SubTree
2828
ID="Create Pose Vector"
2929
_collapsed="false"
3030
target_poses="{target_poses}"
3131
/>
32-
<!-- Add the cartesian poses we created in the previous step -->
33-
<SubTree
34-
ID="Add Poses to MTC Task"
35-
_collapsed="true"
36-
target_poses="{target_poses}"
37-
controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
38-
planning_group_name="manipulator"
39-
ik_frame="grasp_link"
40-
mtc_task="{mtc_task}"
41-
/>
42-
<!-- Add joint-state waypoints to the MTC task as well -->
43-
<SubTree
44-
ID="Add Waypoints to MTC Task"
45-
_collapsed="false"
46-
mtc_task="{mtc_task}"
47-
waypoint_names="Pick Cube;Place Cube"
48-
joint_group_name="manipulator"
49-
planner_interface="moveit_default"
50-
/>
51-
<!-- Perform the computationally intensive calculation now -->
52-
<Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
53-
<!-- Preview the trajectory for approval -->
54-
<Action ID="WaitForUserTrajectoryApproval" solution="{mtc_solution}" />
55-
<!-- The MTC solution does not contain time parameterization, so we can use TOTG to convert it to a JointTrajectory. The source code for the following behavior can be found in the example_behaviors package -->
32+
<!--Plan a cartesian path through the desired poses-->
5633
<Action
57-
ID="ConvertMtcSolutionToJointTrajectory"
58-
joint_trajectory="{joint_trajectory_msg}"
59-
acceleration_scaling_factor="1.000000"
60-
joint_group="manipulator"
61-
sampling_rate="100"
62-
solution="{mtc_solution}"
63-
velocity_scaling_factor="1.000000"
34+
ID="PlanCartesianPath"
35+
acceleration_scale_factor="1.000000"
36+
blending_radius="0.020000"
37+
ik_cartesian_space_density="0.010000"
38+
ik_joint_space_density="0.100000"
39+
path="{target_poses}"
40+
planning_group_name="manipulator"
41+
position_only="false"
42+
tip_link="grasp_link"
43+
trajectory_sampling_rate="100"
44+
velocity_scale_factor="1.000000"
45+
debug_solution="{debug_solution}"
46+
joint_trajectory_msg="{joint_trajectory_msg}"
6447
/>
48+
<!--Preview the trajectory for approval and save if approved-->
49+
<Action ID="WaitForUserTrajectoryApproval" solution="{debug_solution}" />
6550
<Action
6651
ID="SaveJointTrajectoryToYaml"
6752
yaml_filename="~/user_ws/src/lab_sim/objectives/joint_trajectory"

src/lab_sim/objectives/save_and_execute_a_trajectory_from_disk.xml renamed to src/lab_sim/objectives/record_and_replay_scanning_motion.xml

File renamed without changes.

0 commit comments

Comments
 (0)