Skip to content

Commit 9e8564f

Browse files
griswaldbrooksgriswaldbrooks
committed
14875 Moving waypoint in Move Flasks to Burners Objective to be more
 robust and not collide with table
1 parent 3d4f445 commit 9e8564f

File tree

5 files changed

+157
-318
lines changed

5 files changed

+157
-318
lines changed

‎src/lab_sim/objectives/constrained_pick_place.xml‎

Lines changed: 9 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -1,58 +1,20 @@
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 Flasks to Burners">
33
<!--//////////-->
4-
<BehaviorTree
5-
ID="Move Flasks to Burners"
6-
_description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
7-
_favorite="true"
8-
>
4+
<BehaviorTree ID="Move Flasks to Burners" _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion" _favorite="true">
95
<Control ID="Sequence">
10-
<SubTree
11-
ID="Move to Waypoint"
12-
_collapsed="true"
13-
waypoint_name="Flask 1a - Pre Pick"
14-
joint_group_name="manipulator"
15-
controller_names="joint_trajectory_controller"
16-
velocity_scale_factor="1.0"
17-
acceleration_scale_factor="1.0"
18-
link_padding="0.01"
19-
keep_orientation="false"
20-
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
21-
keep_orientation_tolerance="0.05"
22-
keep_orientation_link_names="grasp_link"
23-
/>
24-
<SubTree ID="Open Gripper" />
25-
<SubTree
26-
ID="Constrained Pick and Place Subtree"
27-
pre_pick="Flask 1a - Pre Pick"
28-
pick="Flask 1b - Pick"
29-
pre_place="Flask 1c - Pre Place"
30-
place="Flask 1d - Place"
31-
name="Flask 1"
32-
/>
33-
<SubTree
34-
ID="Constrained Pick and Place Subtree"
35-
pre_pick="Flask 2a - Pre Pick"
36-
pick="Flask 2b - Pick"
37-
pre_place="Flask 2c - Pre Place"
38-
place="Flask 2d - Place"
39-
name="Flask 2"
40-
/>
41-
<SubTree
42-
ID="Constrained Pick and Place Subtree"
43-
pre_pick="Flask 3a - Pre Pick"
44-
pick="Flask 3b - Pick"
45-
pre_place="Flask 3c - Pre Place"
46-
place="Flask 3d - Place"
47-
name="Flask 3"
48-
/>
6+
<SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Flask 1a - Pre Pick" joint_group_name="manipulator" controller_names="joint_trajectory_controller" velocity_scale_factor="1.0" acceleration_scale_factor="1.0" link_padding="0.01" keep_orientation="false" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation_tolerance="0.05" keep_orientation_link_names="grasp_link" seed="0"/>
7+
<SubTree ID="Open Gripper"/>
8+
<SubTree ID="Constrained Pick and Place Subtree" pre_pick="Flask 1a - Pre Pick" pick="Flask 1b - Pick" pre_place="Flask 1c - Pre Place" place="Flask 1d - Place" name="Flask 1"/>
9+
<SubTree ID="Constrained Pick and Place Subtree" pre_pick="Flask 3a - Pre Pick" pick="Flask 3b - Pick" pre_place="Flask 3c - Pre Place" place="Flask 3d - Place" name="Flask 3"/>
10+
<SubTree ID="Constrained Pick and Place Subtree" pre_pick="Flask 2a - Pre Pick" pick="Flask 2b - Pick" pre_place="Flask 2c - Pre Place" place="Flask 2d - Place" name="Flask 2" _collapsed="true"/>
4911
</Control>
5012
</BehaviorTree>
5113
<TreeNodesModel>
5214
<SubTree ID="Move Flasks to Burners">
5315
<MetadataFields>
54-
<Metadata subcategory="Application - Advanced Examples" />
55-
<Metadata runnable="true" />
16+
<Metadata subcategory="Application - Advanced Examples"/>
17+
<Metadata runnable="true"/>
5618
</MetadataFields>
5719
</SubTree>
5820
</TreeNodesModel>

‎src/lab_sim/objectives/load_and_execute_joint_trajectory.xml‎

Lines changed: 9 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -1,60 +1,22 @@
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="Load and Execute Joint Trajectory">
33
<!--//////////-->
4-
<BehaviorTree
5-
ID="Load and Execute Joint Trajectory"
6-
_description="Load a trajectory that was saved as a YAML file"
7-
_favorite="true"
8-
>
4+
<BehaviorTree ID="Load and Execute Joint Trajectory" _description="Load a trajectory that was saved as a YAML file" _favorite="false">
95
<Control ID="Sequence" name="TopLevelSequence">
106
<!--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-
<SubTree
12-
ID="Move to Waypoint"
13-
_collapsed="true"
14-
waypoint_name="Workspace Right"
15-
joint_group_name="manipulator"
16-
controller_names="joint_trajectory_controller"
17-
acceleration_scale_factor="1.0"
18-
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
19-
keep_orientation="false"
20-
keep_orientation_tolerance="0.05"
21-
link_padding="0.01"
22-
velocity_scale_factor="1.0"
23-
/>
24-
<Action
25-
ID="LoadJointTrajectoryFromYaml"
26-
file_path="joint_trajectory.yaml"
27-
output="{joint_trajectory_msg}"
28-
/>
7+
<SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="Workspace Right" joint_group_name="manipulator" controller_names="joint_trajectory_controller" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" velocity_scale_factor="1.0"/>
8+
<Action ID="LoadJointTrajectoryFromYaml" file_path="joint_trajectory.yaml" output="{joint_trajectory_msg}"/>
299
<!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
30-
<Action
31-
ID="GetCurrentPlanningScene"
32-
planning_scene_msg="{planning_scene}"
33-
/>
34-
<Action
35-
ID="ValidateTrajectory"
36-
cartesian_space_step="0.020000"
37-
joint_space_step="0.100000"
38-
joint_trajectory_msg="{joint_trajectory_msg}"
39-
planning_group_name="manipulator"
40-
planning_scene_msg="{planning_scene}"
41-
debug_solution="{debug_solution}"
42-
/>
43-
<Action
44-
ID="ExecuteFollowJointTrajectory"
45-
execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory"
46-
goal_duration_tolerance="-1.000000"
47-
goal_position_tolerance="0.000000"
48-
goal_time_tolerance="0.000000"
49-
joint_trajectory_msg="{joint_trajectory_msg}"
50-
/>
10+
<Action ID="GetCurrentPlanningScene" planning_scene_msg="{planning_scene}"/>
11+
<Action ID="ValidateTrajectory" cartesian_space_step="0.020000" joint_space_step="0.100000" joint_trajectory_msg="{joint_trajectory_msg}" planning_group_name="manipulator" planning_scene_msg="{planning_scene}" debug_solution="{debug_solution}"/>
12+
<Action ID="ExecuteFollowJointTrajectory" execute_follow_joint_trajectory_action_name="/joint_trajectory_controller/follow_joint_trajectory" goal_duration_tolerance="-1.000000" goal_position_tolerance="0.000000" goal_time_tolerance="0.000000" joint_trajectory_msg="{joint_trajectory_msg}"/>
5113
</Control>
5214
</BehaviorTree>
5315
<TreeNodesModel>
5416
<SubTree ID="Load and Execute Joint Trajectory">
5517
<MetadataFields>
56-
<Metadata subcategory="Application - Basic Examples" />
57-
<Metadata runnable="true" />
18+
<Metadata subcategory="Application - Basic Examples"/>
19+
<Metadata runnable="true"/>
5820
</MetadataFields>
5921
</SubTree>
6022
</TreeNodesModel>

‎src/lab_sim/objectives/ml_auto_grasp_object_from_clicked_point.xml‎

Lines changed: 24 additions & 128 deletions
Original file line numberDiff line numberDiff line change
@@ -1,142 +1,38 @@
1-
<?xml version="1.0" encoding="UTF-8" ?>
2-
<root
3-
BTCPP_format="4"
4-
main_tree_to_execute="ML Auto Grasp Object from Clicked Point"
5-
>
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="ML Auto Grasp Object from Clicked Point">
63
<!--//////////-->
7-
<BehaviorTree
8-
ID="ML Auto Grasp Object from Clicked Point"
9-
_description="Asks user to click on object to auto grasp and asks for approval before attempting pick."
10-
_favorite="false"
11-
>
4+
<BehaviorTree ID="ML Auto Grasp Object from Clicked Point" _description="Asks user to click on object to auto grasp and asks for approval before attempting pick." _favorite="false">
125
<Control ID="Sequence" name="TopLevelSequence">
13-
<Action
14-
ID="LogMessage"
15-
log_level="warn"
16-
message="There might be some warnings and errors when this Objective runs, as long as the Objective completes successfully, everything worked as intended."
17-
name="Caution about errors"
18-
/>
19-
<SubTree ID="Open Gripper" _collapsed="true" />
20-
<SubTree
21-
ID="Fuse Multiple Views Subtree"
22-
_collapsed="true"
23-
merged_cloud="{point_cloud}"
24-
waypoint1="Look at Objects - Right"
25-
waypoint2="Look at Objects - Top"
26-
waypoint3="Look at Objects - Left"
27-
name="Scan the scene for a good point cloud"
28-
/>
29-
<SubTree ID="Look at Table" _collapsed="true" />
30-
<SubTree
31-
ID="Segment Image from Point Subtree"
32-
_collapsed="true"
33-
encoder_model_path="models/sam2_hiera_large_encoder.onnx"
34-
image_topic_name="/wrist_camera/color"
35-
masks_visualization_topic="/masks_visualization"
36-
masks2d="{masks2d}"
37-
model_package="lab_sim"
38-
pixel_coords="{pixel_coords}"
39-
decoder_model_path="models/decoder.onnx"
40-
name="Ask the user to pick an object"
41-
/>
42-
<Action
43-
ID="GetCameraInfo"
44-
message_out="{camera_info}"
45-
timeout_sec="5.000000"
46-
topic_name="/wrist_camera/camera_info"
47-
publisher_timeout_sec="5.000000"
48-
/>
49-
<Action
50-
ID="GetMasks3DFromMasks2D"
51-
camera_info="{camera_info}"
52-
masks2d="{masks2d}"
53-
masks3d="{masks3d}"
54-
point_cloud="{point_cloud}"
55-
name="Project the 2D mask to 3D"
56-
/>
57-
<SubTree
58-
ID="Visualize Segmented Point Cloud"
59-
_collapsed="true"
60-
masks3d="{masks3d}"
61-
point_cloud="{point_cloud}"
62-
/>
63-
<Decorator
64-
ID="ForEachUntilSuccess"
65-
index="{index}"
66-
out="{mask3d}"
67-
vector_in="{masks3d}"
68-
name="Mask out the first cloud that work"
69-
>
70-
<Action
71-
ID="GetPointCloudFromMask3D"
72-
mask3d="{mask3d}"
73-
point_cloud="{point_cloud}"
74-
point_cloud_fragment="{point_cloud_fragment}"
75-
/>
6+
<Action ID="LogMessage" log_level="warn" message="There might be some warnings and errors when this Objective runs, as long as the Objective completes successfully, everything worked as intended." name="Caution about errors"/>
7+
<SubTree ID="Open Gripper" _collapsed="true"/>
8+
<SubTree ID="Fuse Multiple Views Subtree" _collapsed="true" merged_cloud="{point_cloud}" waypoint1="Look at Objects - Right" waypoint2="Look at Objects - Top" waypoint3="Look at Objects - Left" name="Scan the scene for a good point cloud"/>
9+
<SubTree ID="Look at Table" _collapsed="true"/>
10+
<SubTree ID="Segment Image from Point Subtree" _collapsed="true" encoder_model_path="models/sam2_hiera_large_encoder.onnx" image_topic_name="/wrist_camera/color" masks_visualization_topic="/masks_visualization" masks2d="{masks2d}" model_package="lab_sim" pixel_coords="{pixel_coords}" decoder_model_path="models/decoder.onnx" name="Ask the user to pick an object"/>
11+
<Action ID="GetCameraInfo" message_out="{camera_info}" timeout_sec="5.000000" topic_name="/wrist_camera/camera_info" publisher_timeout_sec="5.000000"/>
12+
<Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" masks3d="{masks3d}" point_cloud="{point_cloud}" name="Project the 2D mask to 3D"/>
13+
<SubTree ID="Visualize Segmented Point Cloud" _collapsed="true" masks3d="{masks3d}" point_cloud="{point_cloud}"/>
14+
<Decorator ID="ForEachUntilSuccess" index="{index}" out="{mask3d}" vector_in="{masks3d}" name="Mask out the first cloud that work">
15+
<Action ID="GetPointCloudFromMask3D" mask3d="{mask3d}" point_cloud="{point_cloud}" point_cloud_fragment="{point_cloud_fragment}"/>
7616
</Decorator>
77-
<Action
78-
ID="GetGraspPoseFromPointCloud"
79-
grasps="{grasps}"
80-
number_of_grasps_to_return="10"
81-
point_cloud="{point_cloud_fragment}"
82-
model_package="lab_sim"
83-
model_path="models/l2g.onnx"
84-
/>
85-
<SubTree
86-
ID="Reflect Poses Subtree"
87-
_collapsed="true"
88-
input_poses="{grasps}"
89-
output_poses="{grasps_z_modified}"
90-
name="Add z reflected poses"
91-
reflection_axis="0;0;1;0"
92-
/>
93-
<SubTree
94-
ID="Reflect Poses Subtree"
95-
_collapsed="true"
96-
input_poses="{grasps_z_modified}"
97-
output_poses="{grasps_zy_modified}"
98-
name="Add y reflected poses"
99-
reflection_axis="0;1;0;0"
100-
/>
101-
<SubTree ID="Add Table to Planning Scene" _collapsed="true" />
102-
<Decorator
103-
ID="ForEachUntilSuccess"
104-
index="{index}"
105-
vector_in="{grasps_zy_modified}"
106-
out="{grasp_pose}"
107-
name="Iterate through grasps and find a good one"
108-
>
17+
<Action ID="GetGraspPoseFromPointCloud" grasps="{grasps}" number_of_grasps_to_return="10" point_cloud="{point_cloud_fragment}" model_package="lab_sim" model_path="models/l2g.onnx"/>
18+
<SubTree ID="Reflect Poses Subtree" _collapsed="true" input_poses="{grasps}" output_poses="{grasps_z_modified}" name="Add z reflected poses" reflection_axis="0;0;1;0"/>
19+
<SubTree ID="Reflect Poses Subtree" _collapsed="true" input_poses="{grasps_z_modified}" output_poses="{grasps_zy_modified}" name="Add y reflected poses" reflection_axis="0;1;0;0"/>
20+
<SubTree ID="Add Table to Planning Scene" _collapsed="true"/>
21+
<Decorator ID="ForEachUntilSuccess" index="{index}" vector_in="{grasps_zy_modified}" out="{grasp_pose}" name="Iterate through grasps and find a good one">
10922
<Control ID="Sequence">
110-
<Action
111-
ID="VisualizePose"
112-
marker_lifetime="0.000000"
113-
marker_name="grasp pose"
114-
marker_size="0.100000"
115-
pose="{grasp_pose}"
116-
/>
117-
<Action
118-
ID="VisualizeMesh"
119-
marker_lifetime="0.000000"
120-
mesh_path="package://lab_sim/objectives/gripper_token.dae"
121-
mesh_pose="{grasp_pose}"
122-
/>
123-
<SubTree
124-
ID="Pick from Pose With Approval"
125-
_collapsed="true"
126-
approach_distance="0.1"
127-
grasp_pose="{grasp_pose}"
128-
retract_xyz="0;0;-0.1"
129-
/>
23+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="grasp pose" marker_size="0.100000" pose="{grasp_pose}"/>
24+
<Action ID="VisualizeMesh" marker_lifetime="0.000000" mesh_path="package://lab_sim/objectives/gripper_token.dae" mesh_pose="{grasp_pose}" mesh_name="mesh"/>
25+
<SubTree ID="Pick from Pose With Approval" _collapsed="true" approach_distance="0.1" grasp_pose="{grasp_pose}" retract_xyz="0;0;-0.1"/>
13026
</Control>
13127
</Decorator>
132-
<SubTree ID="Reset Planning Scene" _collapsed="true" />
28+
<SubTree ID="Reset Planning Scene" _collapsed="true"/>
13329
</Control>
13430
</BehaviorTree>
13531
<TreeNodesModel>
13632
<SubTree ID="ML Auto Grasp Object from Clicked Point">
13733
<MetadataFields>
138-
<Metadata runnable="true" />
139-
<Metadata subcategory="Application - ML (GPU Recommended)" />
34+
<Metadata runnable="true"/>
35+
<Metadata subcategory="Application - ML (GPU Recommended)"/>
14036
</MetadataFields>
14137
</SubTree>
14238
</TreeNodesModel>

‎src/lab_sim/objectives/show_grasp_link.xml‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="show grasp_link">
3+
<!--//////////-->
4+
<BehaviorTree ID="show grasp_link" _description="" _favorite="false">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Get Transform Frame Pose" _collapsed="true" requested_pose="{grasp_pose}" requested_frame="grasp_link"/>
7+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{grasp_pose}"/>
8+
<Action ID="VisualizeMesh" marker_lifetime="0.000000" mesh_name="mesh" mesh_path="package://lab_sim/objectives/gripper_token.dae" mesh_pose="{grasp_pose}"/>
9+
</Control>
10+
</BehaviorTree>
11+
<TreeNodesModel>
12+
<SubTree ID="show grasp_link">
13+
<MetadataFields>
14+
<Metadata runnable="true"/>
15+
<Metadata subcategory="Application - Advanced Examples"/>
16+
</MetadataFields>
17+
</SubTree>
18+
</TreeNodesModel>
19+
</root>

0 commit comments

Comments
 (0)