Skip to content

Commit 26e509a

Browse files
griswaldbrooksgriswaldbrooks
committed
14875 Fix precommit
1 parent 9e8564f commit 26e509a

File tree

4 files changed

+248
-48
lines changed

4 files changed

+248
-48
lines changed

‎src/lab_sim/objectives/constrained_pick_place.xml‎

Lines changed: 49 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,60 @@
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 ID="Move Flasks to Burners" _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion" _favorite="true">
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+
>
59
<Control ID="Sequence">
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"/>
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+
seed="0"
24+
/>
25+
<SubTree ID="Open Gripper" />
26+
<SubTree
27+
ID="Constrained Pick and Place Subtree"
28+
pre_pick="Flask 1a - Pre Pick"
29+
pick="Flask 1b - Pick"
30+
pre_place="Flask 1c - Pre Place"
31+
place="Flask 1d - Place"
32+
name="Flask 1"
33+
/>
34+
<SubTree
35+
ID="Constrained Pick and Place Subtree"
36+
pre_pick="Flask 3a - Pre Pick"
37+
pick="Flask 3b - Pick"
38+
pre_place="Flask 3c - Pre Place"
39+
place="Flask 3d - Place"
40+
name="Flask 3"
41+
/>
42+
<SubTree
43+
ID="Constrained Pick and Place Subtree"
44+
pre_pick="Flask 2a - Pre Pick"
45+
pick="Flask 2b - Pick"
46+
pre_place="Flask 2c - Pre Place"
47+
place="Flask 2d - Place"
48+
name="Flask 2"
49+
_collapsed="true"
50+
/>
1151
</Control>
1252
</BehaviorTree>
1353
<TreeNodesModel>
1454
<SubTree ID="Move Flasks to Burners">
1555
<MetadataFields>
16-
<Metadata subcategory="Application - Advanced Examples"/>
17-
<Metadata runnable="true"/>
56+
<Metadata subcategory="Application - Advanced Examples" />
57+
<Metadata runnable="true" />
1858
</MetadataFields>
1959
</SubTree>
2060
</TreeNodesModel>

‎src/lab_sim/objectives/load_and_execute_joint_trajectory.xml‎

Lines changed: 47 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,60 @@
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 ID="Load and Execute Joint Trajectory" _description="Load a trajectory that was saved as a YAML file" _favorite="false">
4+
<BehaviorTree
5+
ID="Load and Execute Joint Trajectory"
6+
_description="Load a trajectory that was saved as a YAML file"
7+
_favorite="false"
8+
>
59
<Control ID="Sequence" name="TopLevelSequence">
610
<!--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-->
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}"/>
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+
/>
929
<!--Validate the loaded trajectory with the current planning scene to check for collisions before executing-->
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}"/>
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+
/>
1351
</Control>
1452
</BehaviorTree>
1553
<TreeNodesModel>
1654
<SubTree ID="Load and Execute Joint Trajectory">
1755
<MetadataFields>
18-
<Metadata subcategory="Application - Basic Examples"/>
19-
<Metadata runnable="true"/>
56+
<Metadata subcategory="Application - Basic Examples" />
57+
<Metadata runnable="true" />
2058
</MetadataFields>
2159
</SubTree>
2260
</TreeNodesModel>

‎src/lab_sim/objectives/ml_auto_grasp_object_from_clicked_point.xml‎

Lines changed: 129 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -1,38 +1,143 @@
1-
<?xml version='1.0' encoding='UTF-8'?>
2-
<root BTCPP_format="4" main_tree_to_execute="ML Auto Grasp Object from Clicked Point">
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+
>
36
<!--//////////-->
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">
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+
>
512
<Control ID="Sequence" name="TopLevelSequence">
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}"/>
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+
/>
1676
</Decorator>
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">
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+
>
22109
<Control ID="Sequence">
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"/>
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+
mesh_name="mesh"
123+
/>
124+
<SubTree
125+
ID="Pick from Pose With Approval"
126+
_collapsed="true"
127+
approach_distance="0.1"
128+
grasp_pose="{grasp_pose}"
129+
retract_xyz="0;0;-0.1"
130+
/>
26131
</Control>
27132
</Decorator>
28-
<SubTree ID="Reset Planning Scene" _collapsed="true"/>
133+
<SubTree ID="Reset Planning Scene" _collapsed="true" />
29134
</Control>
30135
</BehaviorTree>
31136
<TreeNodesModel>
32137
<SubTree ID="ML Auto Grasp Object from Clicked Point">
33138
<MetadataFields>
34-
<Metadata runnable="true"/>
35-
<Metadata subcategory="Application - ML (GPU Recommended)"/>
139+
<Metadata runnable="true" />
140+
<Metadata subcategory="Application - ML (GPU Recommended)" />
36141
</MetadataFields>
37142
</SubTree>
38143
</TreeNodesModel>

‎src/lab_sim/objectives/show_grasp_link.xml‎

Lines changed: 23 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,35 @@
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="show grasp_link">
33
<!--//////////-->
44
<BehaviorTree ID="show grasp_link" _description="" _favorite="false">
55
<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}"/>
6+
<SubTree
7+
ID="Get Transform Frame Pose"
8+
_collapsed="true"
9+
requested_pose="{grasp_pose}"
10+
requested_frame="grasp_link"
11+
/>
12+
<Action
13+
ID="VisualizePose"
14+
marker_lifetime="0.000000"
15+
marker_name="pose"
16+
marker_size="0.100000"
17+
pose="{grasp_pose}"
18+
/>
19+
<Action
20+
ID="VisualizeMesh"
21+
marker_lifetime="0.000000"
22+
mesh_name="mesh"
23+
mesh_path="package://lab_sim/objectives/gripper_token.dae"
24+
mesh_pose="{grasp_pose}"
25+
/>
926
</Control>
1027
</BehaviorTree>
1128
<TreeNodesModel>
1229
<SubTree ID="show grasp_link">
1330
<MetadataFields>
14-
<Metadata runnable="true"/>
15-
<Metadata subcategory="Application - Advanced Examples"/>
31+
<Metadata runnable="true" />
32+
<Metadata subcategory="Application - Advanced Examples" />
1633
</MetadataFields>
1734
</SubTree>
1835
</TreeNodesModel>

0 commit comments

Comments
 (0)