Skip to content

Commit b348491

Browse files
dyackzandyackzan
authored
Merge pull request #82 from PickNikRobotics/remove-mtc-record-and-replay-motion
Don't use MTC in Plan and Save Trajectory objective Former-commit-id: 8aab45e
2 parents 790b5ed + 1cc1696 commit b348491

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)