Skip to content

Commit 391f932

Browse files
kineticsystemkineticsystem
committed
ML behaviors.
1 parent 80cb594 commit 391f932

17 files changed

+302
-212
lines changed

‎src/moveit_pro_franka_configs/franka_arm_sim/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ install(
77
config
88
description
99
launch
10+
models
1011
objectives
1112
waypoints
1213
DESTINATION

‎src/moveit_pro_franka_configs/franka_arm_sim/description/mujoco/scene.xml‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,21 @@
6767
xyaxes="0.510 -0.860 0.000 0.666 0.395 0.633"
6868
/>
6969

70+
<!-- Back scene Camera -->
71+
<site
72+
name="back_camera_optical_frame"
73+
pos="-0.661 0.324 1.325"
74+
xyaxes="0.026 -1.000 0.000 -0.062 -0.002 -0.998"
75+
/>
76+
77+
<camera
78+
name="back_camera"
79+
fovy="48"
80+
resolution="640 480"
81+
pos="-0.661 0.324 1.325"
82+
xyaxes="0.026 -1.000 0.000 0.062 0.002 0.998"
83+
/>
84+
7085
<!-- Cylindrical pedestal: radius=0.2, length=0.9 (size takes half-length=0.45) -->
7186
<body name="pedestal" pos="0 0 0">
7287
<!-- place geom so the cylinder stands from z=0 to z=0.9 -->
@@ -112,7 +127,7 @@
112127
</body>
113128
</body>
114129

115-
<body name="kettle" pos="0.06 0.42 0.96" euler="0 0 -3.14">
130+
<body name="kettle" pos="0.2 0.6 0.96" euler="0 0 -3.14">
116131
<freejoint name="kettle" />
117132
<include file="assets/kitchen_assets/item_assets/kettle_chain.xml" />
118133
</body>
@@ -143,7 +158,7 @@
143158
-->
144159
<key
145160
name="home"
146-
qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035 0.035 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.06 0.42 0.96 0 0 0 0"
161+
qpos="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035 0.035 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.2 0.6 0.96 0 0 0 0"
147162
ctrl="0 -0.7854 0 -2.3562 0 1.5707 0.7853 0.035"
148163
/>
149164
</keyframe>

‎src/moveit_pro_franka_configs/franka_arm_sim/models/decoder.onnx‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
version https://git-lfs.github.com/spec/v1
2+
oid sha256:1f448cdb479e6ec14e61c4756138eb4081ce7f8a11ca43a0a24856d5e8b61b6f
3+
size 20665365

‎src/moveit_pro_franka_configs/franka_arm_sim/models/l2g.onnx‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
version https://git-lfs.github.com/spec/v1
2+
oid sha256:fc1fd4a7cf185225ba29940da16cf052cc9801a4883aecffd82845627e0965b5
3+
size 15618125

‎src/moveit_pro_franka_configs/franka_arm_sim/models/sam2_hiera_large_encoder.onnx‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
version https://git-lfs.github.com/spec/v1
2+
oid sha256:c99ab89a38385753aff7ea9155f0808ad5535bc55ea2a49320254e39e4011630
3+
size 889364590

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/generate_graspable_object.xml‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Generate Graspable Object">
3+
<!--//////////-->
4+
<BehaviorTree ID="Generate Graspable Object" _description="Captures a point cloud and requests the user to click on one object in the image to be segmented. The point cloud is then filtered only to include the selected objects, and grasp objects are generated." _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Take Scene Camera Snapshot" _collapsed="true"/>
7+
<SubTree ID="Open Gripper" _collapsed="true"/>
8+
<Action ID="GetImage" message_out="{image}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/color"/>
9+
<Action ID="GetPointsFromUser" pixel_coords="{pixel_coords}" view_name="/scene_camera/color"/>
10+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Home"/>
11+
<Action ID="GetMasks2DFromPointQuery" image="{image}" masks2d="{masks2d}" pixel_coords="{pixel_coords}" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" model_package="franka_arm_sim"/>
12+
<Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="/masks_visualization" opacity="0.500000" bounding_box_labels="detection"/>
13+
<Action ID="SwitchUIPrimaryView" primary_view_name="/masks_visualization"/>
14+
<Action ID="GetCameraInfo" message_out="{camera_info}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/camera_info"/>
15+
<Action ID="GetPointCloud" message_out="{point_cloud}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/points"/>
16+
<Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" masks3d="{masks3d}" point_cloud="{point_cloud}"/>
17+
<Decorator ID="ForEach" index="{index}" out="{mask3d}" vector_in="{masks3d}">
18+
<Control ID="Sequence">
19+
<Action ID="GetPointCloudFromMask3D" mask3d="{mask3d}" point_cloud="{point_cloud}" point_cloud_fragment="{point_cloud_fragment}"/>
20+
<Action ID="SendPointCloudToUI" pcd_topic="/pcd_pointcloud_captures" point_cloud="{point_cloud_fragment}"/>
21+
</Control>
22+
</Decorator>
23+
<Action ID="GetGraspableObjectsFromMasks3D" base_frame="world" face_separation_threshold="0.010000" graspable_objects="{graspable_objects}" masks3d="{masks3d}" minimum_face_area="0.000625" plane_inlier_threshold="0.010000" point_cloud="{point_cloud}"/>
24+
<Decorator ID="ForEach" index="{index}" out="{graspable_object}" vector_in="{graspable_objects}">
25+
<Action ID="ModifyObjectInPlanningScene" apply_planning_scene_service="/apply_planning_scene" object="{graspable_object}"/>
26+
</Decorator>
27+
</Control>
28+
</BehaviorTree>
29+
<TreeNodesModel>
30+
<SubTree ID="Generate Graspable Object">
31+
<MetadataFields>
32+
<Metadata runnable="true"/>
33+
<Metadata subcategory="Application - ML (GPU Recommended)"/>
34+
</MetadataFields>
35+
</SubTree>
36+
</TreeNodesModel>
37+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/grasp_object_from_point.xml‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Grasp Object from Point">
3+
<!--//////////-->
4+
<BehaviorTree ID="Grasp Object from Point" _description="Captures a point cloud and requests the user to click on one object in the image to be segmented. The point cloud is then filtered only to include the selected objects, and a grasp pose is generated." _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Take Scene Camera Snapshot" _collapsed="true"/>
7+
<Action ID="GetImage" message_out="{image}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/color"/>
8+
<Action ID="GetPointsFromUser" pixel_coords="{pixel_coords}" view_name="/scene_camera/color"/>
9+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Ready"/>
10+
<Action ID="GetMasks2DFromPointQuery" image="{image}" masks2d="{masks2d}" pixel_coords="{pixel_coords}" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" model_package="franka_arm_sim"/>
11+
<Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="/masks_visualization" opacity="0.500000" bounding_box_labels="detection"/>
12+
<Action ID="SwitchUIPrimaryView" primary_view_name="/masks_visualization"/>
13+
<Action ID="GetCameraInfo" message_out="{camera_info}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/camera_info"/>
14+
<Action ID="GetPointCloud" message_out="{point_cloud}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/points"/>
15+
<Action ID="GetMasks3DFromMasks2D" camera_info="{camera_info}" masks2d="{masks2d}" masks3d="{masks3d}" point_cloud="{point_cloud}"/>
16+
<Decorator ID="ForEach" index="{index}" out="{mask3d}" vector_in="{masks3d}">
17+
<Action ID="GetPointCloudFromMask3D" mask3d="{mask3d}" point_cloud="{point_cloud}" point_cloud_fragment="{point_cloud_fragment}"/>
18+
</Decorator>
19+
<Action ID="GetCentroidFromPointCloud" output_pose="{output_pose}" point_cloud="{point_cloud_fragment}"/>
20+
<Action ID="VisualizePose" marker_lifetime="0.000000" marker_name="pose" marker_size="0.100000" pose="{output_pose}"/>
21+
<SubTree ID="Move to Pose" _collapsed="true" controller_names="joint_trajectory_controller;franka_gripper_controller" ik_frame="grasp_link" link_padding="0.01" planning_group_name="manipulator" require_user_approval="true" target_pose="{output_pose}"/>
22+
</Control>
23+
</BehaviorTree>
24+
<TreeNodesModel>
25+
<SubTree ID="Grasp Object from Point">
26+
<MetadataFields>
27+
<Metadata runnable="true"/>
28+
<Metadata subcategory="Application - ML (GPU Recommended)"/>
29+
</MetadataFields>
30+
</SubTree>
31+
</TreeNodesModel>
32+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/move_away.xml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Move Away">
3+
<!--//////////-->
4+
<BehaviorTree ID="Move Away" _description="Move the robot away from the main scene camera." _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Out of the way"/>
7+
</Control>
8+
</BehaviorTree>
9+
<TreeNodesModel>
10+
<SubTree ID="Move Away">
11+
<MetadataFields>
12+
<Metadata runnable="true"/>
13+
<Metadata subcategory="Motion - Execute"/>
14+
</MetadataFields>
15+
</SubTree>
16+
</TreeNodesModel>
17+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/move_home.xml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Move Home">
3+
<!--//////////-->
4+
<BehaviorTree ID="Move Home" _description="Move the Robot to Home Position" _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<SubTree ID="Move to Waypoint" _collapsed="true" acceleration_scale_factor="1.0" controller_action_server="/joint_trajectory_controller/follow_joint_trajectory" controller_names="joint_trajectory_controller" joint_group_name="manipulator" keep_orientation="false" keep_orientation_tolerance="0.05" link_padding="0.01" seed="0" velocity_scale_factor="1.0" waypoint_name="Home"/>
7+
</Control>
8+
</BehaviorTree>
9+
<TreeNodesModel>
10+
<SubTree ID="Move Home">
11+
<MetadataFields>
12+
<Metadata runnable="true"/>
13+
<Metadata subcategory="Motion - Execute"/>
14+
</MetadataFields>
15+
</SubTree>
16+
</TreeNodesModel>
17+
</root>

‎src/moveit_pro_franka_configs/franka_arm_sim/objectives/segment_image_from_point.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version='1.0' encoding='UTF-8'?>
2+
<root BTCPP_format="4" main_tree_to_execute="Segment Image from Point">
3+
<!--//////////-->
4+
<BehaviorTree ID="Segment Image from Point" _description="Run an ML model to find an area around a clicked point and visualize the mask with bounding boxes." _favorite="true">
5+
<Control ID="Sequence" name="TopLevelSequence">
6+
<Action ID="GetImage" message_out="{image}" publisher_timeout_sec="5.000000" timeout_sec="5.000000" topic_name="/scene_camera/color"/>
7+
<Action ID="GetPointsFromUser" pixel_coords="{pixel_coords}" view_name="/scene_camera/color"/>
8+
<Action ID="GetMasks2DFromPointQuery" image="{image}" masks2d="{masks2d}" pixel_coords="{pixel_coords}" decoder_model_path="models/decoder.onnx" encoder_model_path="models/sam2_hiera_large_encoder.onnx" model_package="franka_arm_sim"/>
9+
<Action ID="PublishMask2D" image="{image}" masks="{masks2d}" masks_visualization_topic="/masks_visualization" opacity="0.500000" bounding_box_labels="detection"/>
10+
<Action ID="SwitchUIPrimaryView" primary_view_name="/masks_visualization"/>
11+
</Control>
12+
</BehaviorTree>
13+
<TreeNodesModel>
14+
<SubTree ID="Segment Image from Point">
15+
<MetadataFields>
16+
<Metadata runnable="true"/>
17+
<Metadata subcategory="Application - ML (GPU Recommended)"/>
18+
</MetadataFields>
19+
</SubTree>
20+
</TreeNodesModel>
21+
</root>

0 commit comments

Comments
 (0)