Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Updates to ML Objectives and MTC Parameters #205

Merged
merged 25 commits into from
Apr 12, 2024
Merged
Show file tree
Hide file tree
Changes from 19 commits
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
c190ca6
Update lever handle to use line segment version. Raise timeout on pus…
Feb 8, 2024
bb104a3
Update parameters to be more accurate.
Feb 8, 2024
009bcc4
Update params following behavior updates
Feb 9, 2024
4353848
Add new parameters to cabinet door mtc
Mar 1, 2024
3203269
Fixed parameters with new changes
Mar 4, 2024
5c83298
fix typo, wrap in force success following MTC changes
Mar 8, 2024
6e99215
Fix version change rebase issue, PR comments
Mar 8, 2024
880a2f6
Merge pull request #250 from PickNikRobotics/5.0-to-main-dont-squash
orensbruli Mar 28, 2024
fa091c0
Update ros2_robotiq_gripper version
Apr 1, 2024
6241cbf
Update camera sync behavior names (#219)
sea-bass Apr 1, 2024
f434982
Update take_snapshot to work with the new userClick behaviors. Update…
Apr 1, 2024
12f685c
Merge branch 'main' into ml-mtc-updates
Apr 1, 2024
860fecd
Modified objective to use other modular SetupMTC behaviors so we don'…
Apr 2, 2024
6a8cddb
Update ML Cabinet door objective to new MTC setup
Apr 2, 2024
8cc2b5d
Update camera stream topics to have /camera prefix and fix behavior t…
Apr 2, 2024
40ec179
This is what I meant in the last commit
Apr 2, 2024
b996b8e
Merge branch 'ml-mtc-updates' of github.com:PickNikRobotics/moveit_st…
Apr 2, 2024
42d6866
Reenable takeSnapshot in OpenCabinetDoor after hardware testing
Apr 2, 2024
c9231e1
fix getDoorHandle and fitLineSegment objectives to use a vector of po…
Apr 3, 2024
4061601
try to get behavor to work
pac48 Apr 4, 2024
dcb32d5
fix open_lever_handle_door ports
pac48 Apr 4, 2024
e3238f6
fix gazebo objectives
pac48 Apr 5, 2024
99e4fb1
revert deletion of subtrees
pac48 Apr 8, 2024
3ac4073
revert camera topic change
pac48 Apr 8, 2024
2b57302
revert more camera topics
pac48 Apr 8, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
<Decorator ID="KeepRunningUntilFailure">
<Control ID="Sequence" name="GetDetection">
<Action ID="LoadObjectiveParameters" config_file_name="apriltag_detection_config.yaml" parameters="{parameters}"/>
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetCameraInfo" topic_name="/camera/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this change was reverted with the realsense downgrade

<Action ID="GetImage" topic_name="/camera/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="DetectAprilTags" image="{image}" camera_info="{camera_info}" parameters="{parameters}" detections="{detections}"/>
</Control>
</Decorator>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<BehaviorTree ID="Object Segmentation 2D" _description="Detect objects using a 2D image segmentation model." _favorite="false" _hardcoded="false">
<Control ID="Sequence" name="TopLevelSequence">
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetImage" topic_name="/camera/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" valid_classes="" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="-1" masks2d="{masks2d}"/>
</Control>
</BehaviorTree>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,9 @@
<Action ID="ClearSnapshot"/>
<Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
<SubTree ID="Move to Waypoint" waypoint_name="Extended Right" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetCameraInfo" topic_name="/camera/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetPointCloud" topic_name="/camera/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetImage" topic_name="/camera/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="0.8" timeout_sec="-1" masks2d="{masks2d}" valid_classes=""/>
<Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
<Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
Expand Down

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<BehaviorTree ID="Estimate Object Pose" _description="Estimate an object pose from a reference mesh file using ICP on live point cloud data." _favorite="false" _hardcoded="false">
<Control ID="Sequence">
<Action ID="LoadPointCloudFromFile" file_path="{object_model_file_path}" frame_id="world" num_sampled_points="10000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
<Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
<Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/camera/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/camera/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/camera/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
<Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
<Action ID="CreateStampedPose" reference_frame="world" position_xyz="{guess_position}" orientation_xyzw="{guess_orientation}" stamped_pose="{object_stamped_pose_estimate}"/>
<Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{object_stamped_pose_estimate}" output_cloud="{model_point_cloud}"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<Control ID="Sequence">
<Action ID="TransformPoseWithPose" input_pose="{trajectory_pose}" transform_pose="{model_to_real_pose}" output_pose="{target_pose}"/>
<SubTree ID="Move To Pose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
<Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
<Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/camera/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/camera/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/camera/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
<Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud_world}"/>
<Action ID="AddPointCloudToVector" point_cloud="{point_cloud_world}" point_cloud_vector="{point_cloud_vector}"/>
</Control>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
</Decorator>
<!-- Cartesian and joint jogging -->
<Control ID="Sequence" _while="teleop_mode == 2">
<Action ID="SwitchUIPrimaryView" primary_view_name="/wrist_mounted_camera/color/image_raw"/>
<Action ID="SwitchUIPrimaryView" primary_view_name="/camera/wrist_mounted_camera/color/image_raw"/>
<Action ID="TeleoperateTwist" controller_name="right_servo_controller"/>
</Control>
<Control ID="Sequence" _while="teleop_mode == 1">
Expand Down
11 changes: 9 additions & 2 deletions src/picknik_ur_site_config/objectives/close_cabinet_door.xml
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,15 @@
<Control ID="Sequence">
<Action ID="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
<Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
<Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
<Action ID="GetPointsFromUser" view_name="/camera/wrist_mounted_camera/color/image_raw" point_names="target_pose" point_prompts="Click on the door you want to close" pixel_coords="{pixel_coords}"/>
<Action ID="GetPointCloud" topic_name="/camera/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
<Decorator ID="ForEachPoseStamped" vector_in="{output_poses}" out="{pose}">
<Control ID="Sequence">
<Action ID="TransformPose" output_pose="{target_pose}" quaternion_xyzw="0.7071;0.7071;0;0" translation_xyz="0;0;0.1" input_pose="{pose}"/>
<Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
</Control>
</Decorator>
<Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
<Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />
<Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
Expand Down
Original file line number Diff line number Diff line change
@@ -1,34 +1,33 @@
<?xml version="1.0" encoding="UTF-8"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Detect Door Graspable Object">
<!-- ////////// -->
<BehaviorTree ID="Detect Door Graspable Object" _description="Detects a door grapsable object from a depth image and gets a GraspableObject.">
<BehaviorTree ID="Detect Door Graspable Object" _description="Segments out a door from an input depth image and attempts to fit an output GraspableObject.">
<!-- TODO: need to be directly in front of door for this objective to work until GetGraspableObjectsFromMasks3D calculates the collision more robustly for non cuboid objects. -->
<Control ID="Sequence" name="TopLevelSequence">
<!-- Get the point cloud from the wrist camera -->
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetCameraInfo" topic_name="/camera/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetPointCloud" topic_name="/camera/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetImage" topic_name="/camera/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
<Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="10.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
<Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="15.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
<Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
<Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
<Control ID="Sequence">
<Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
<Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
</Control>
</Decorator>
<!-- Convert 3D mask into graspable object -->
<Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
<!-- Convert 3D mask into graspable object, but as we don't need it for MTC -->
<Decorator ID="ForceSuccess">
<Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
</Decorator>
<Decorator ID="ForceSuccess">
<Decorator ID="ForEachGraspableObject" vector_in="{door_objects}" out="{door_object}">
<Action ID="AlwaysFailure"/>
</Decorator>
</Decorator>
<!-- Add object to planning scene and return success -->
<Action ID="ModifyObjectInPlanningScene" object="{door_object}" apply_planning_scene_service="/apply_planning_scene"/>
<Action ID="LogMessage" message="Found a GraspableObject for the door." log_level="info"/>
</Control>
</BehaviorTree>
</root>
Original file line number Diff line number Diff line change
@@ -1,18 +1,18 @@
<?xml version="1.0" encoding="UTF-8"?>
<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4" main_tree_to_execute="Detect Fixed Handle Graspable Object">
<!-- ////////// -->
<BehaviorTree ID="Detect Fixed Handle Graspable Object" _description="Detects a fixed-handle graspable object from a depth image and returns a PoseStamped for the handle centroid.">
<!-- TODO: need to be directly in front of handle for this objective to work until GetGraspableObjectsFromMasks3D calculates the collision more robustly for non cuboid objects or shape completion is available. -->
<Control ID="Sequence" name="TopLevelSequence">
<!-- Get the point cloud from the wrist camera -->
<Control ID="Sequence" name="GetDepthImage">
<Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="GetCameraInfo" topic_name="/camera/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
<Action ID="GetImage" topic_name="/camera/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
<Action ID="GetPointCloud" topic_name="/camera/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
<Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
</Control>
<!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
<Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
<Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="15.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
<Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
<Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
<Control ID="Sequence">
Expand All @@ -27,10 +27,9 @@
<Action ID="AlwaysFailure"/>
</Decorator>
</Decorator>
<!-- Add object to planning scene and return success -->
<Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
<Action ID="ExtractGraspableObjectPose" graspable_object="{handle_object}" pose="{grasp_pose}"/>
<Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected pull handle." log_level="info"/>
<Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
</Control>
</BehaviorTree>
</root>
Loading
Loading