This repository has been archived by the owner on Dec 13, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #333 from PickNikRobotics/v6.0
Merge v6.0 changes into main
- Loading branch information
Showing
17 changed files
with
132 additions
and
22 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,40 @@ | ||
| <?xml version='1.0' encoding='UTF-8'?> | ||
| <root BTCPP_format="4" main_tree_to_execute="SAM Pick Object"> | ||
| <!--//////////--> | ||
| <BehaviorTree ID="SAM Pick Object" _description="Uses SAM to segment out an arbitrary object and pick it."> | ||
| <Control ID="Sequence" name="TopLevelSequence"> | ||
| <Action ID="LoadObjectiveParameters" parameters="{pick_parameters}" config_file_name="sam_pick_object_config.yaml"/> | ||
| <SubTree ID="Move to Waypoint" waypoint_name="Hinge View" joint_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" _collapsed="true"/> | ||
| <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.0"/> | ||
| <!--Get the point cloud from the wrist camera--> | ||
| <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}" timeout_sec="5.000000"/> | ||
| <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}" timeout_sec="5.000000"/> | ||
| <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" pcd_topic="/pcd_pointcloud_captures"/> | ||
| <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}" timeout_sec="5.000000"/> | ||
| <!--Segment out 2D masks and then convert to 3D masks and visualize in UI--> | ||
| <Action ID="GetPointsFromUser" point_prompts="Click on the object you want to pick." point_names="clicked_object" pixel_coords="{pixel_coords}" view_name="/wrist_mounted_camera/color/image_raw"/> | ||
| <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_superseg" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.0002" max_relative_area="0.25" timeout_sec="15.0" masks2d="{masks2d}" valid_points="{pixel_coords}" 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}"> | ||
| <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}" pcd_topic="/pcd_pointcloud_captures"/> | ||
| </Control> | ||
| </Decorator> | ||
| <!--Convert 3D masks into graspable objects--> | ||
| <Action ID="GetGraspableObjectsFromMasks3D" graspable_objects="{objects}" plane_inlier_threshold="0.020000" base_frame="world" minimum_face_area="0" masks3d="{masks3d}" face_separation_threshold="0.025000" point_cloud="{point_cloud}"/> | ||
| <Decorator ID="ForEachGraspableObject" vector_in="{objects}" out="{object}"> | ||
| <Control ID="Sequence"> | ||
| <Action ID="InitializeMTCTask" task="{mtc_task}" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task_id=""/> | ||
| <Action ID="SetupMTCCurrentState" task="{mtc_task}"/> | ||
| <Action ID="SetupMTCApproachGrasp" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/> | ||
| <Action ID="SetupMTCGenerateCuboidGrasps" parameters="{pick_parameters}" target_object="{object}" monitored_stage="{monitored_stage}" task="{mtc_task}"/> | ||
| <Action ID="SetupMTCRetractFromGrasp" parameters="{pick_parameters}" target_object="{object}" task="{mtc_task}"/> | ||
| <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}"/> | ||
| <SubTree ID="Wait for Trajectory Approval if User Available" _collapsed="false" solution="{mtc_solution}"/> | ||
| <Action ID="ExecuteMTCTask" solution="{mtc_solution}"/> | ||
| </Control> | ||
| </Decorator> | ||
| </Control> | ||
| </BehaviorTree> | ||
| </root> |
71 changes: 71 additions & 0 deletions
71
src/picknik_ur_site_config/objectives/sam_pick_object_config.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,71 @@ | ||
| SetupMTCApproachGrasp: | ||
| # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame. | ||
| world_frame_name: "world" | ||
| arm_group_name: "manipulator" | ||
| end_effector_group_name: "gripper" | ||
| end_effector_name: "moveit_ee" | ||
| hand_frame_name: "grasp_link" | ||
|
|
||
| approach_distance: 0.15 | ||
|
|
||
| # Set to true to make the robot keep the object within view of its camera while reaching to grasp it | ||
| enforce_visibility_constraint: true | ||
| camera_optical_frame_id: "wrist_mounted_camera_color_optical_frame" | ||
| camera_field_of_view_angle: 1.0472 # radians: approx. 60 degrees, which is narrower than the 70 degree diagonal FOV of the D415 camera | ||
| sensor_z_offset: 0.02 | ||
| target_diameter: 0.0 # Set to 0 to disable the visibility cone constraint -- it's not useful with a single-arm camera-in-hand configuration. | ||
|
|
||
|
|
||
| SetupMTCGenerateCuboidGrasps: | ||
| end_effector_group_name: "gripper" | ||
| end_effector_name: "moveit_ee" | ||
| hand_frame_name: "grasp_link" | ||
| end_effector_closed_pose_name: "close" | ||
| ui_grasp_link: "grasp_link" | ||
|
|
||
| # Grasp candidate configs | ||
| grasp_candidate_config: | ||
| generate_x_axis_grasps: true | ||
| generate_y_axis_grasps: true | ||
| generate_z_axis_grasps: true | ||
|
|
||
| # Number of samples to generate for each quadrant of the cuboid. | ||
| samples_per_quadrant: 3 | ||
|
|
||
| # Grasp data configs | ||
| grasp_data: | ||
| # See https://ros-planning.github.io/moveit_tutorials/_images/finger_gripper_explanation.jpg for description of the following parameters | ||
| # Note: these parameters are copied from Robotiq gripper without tweaking | ||
| # min/max values of the grasp depth range in meter. | ||
| # This should fit to the distance from finger tip to inner palm or the suction cup stroke | ||
| grasp_min_depth: 0.025 # minimum amount fingers must overlap object | ||
| grasp_max_depth: 0.065 # Maximum distance from tip of end effector inwards that an object can be for a grasp | ||
|
|
||
| # Maximum allowed finger width for a grasp. | ||
| # This value should be considerably smaller than max_finger_width | ||
| # to allow padded collision checks | ||
| max_grasp_width: 0.4 | ||
|
|
||
| # The transform from the arm IK link to the grasp point to align it with the following convention. | ||
| # | ||
| # z-axis pointing toward object to grasp | ||
| # x-axis perpendicular to movement of grippers | ||
| # y-axis parallel to movement of grippers | ||
| eef_mount_to_tcp_transform: | ||
| x: 0.0 | ||
| y: 0.0 | ||
| z: -0.02 | ||
| roll: 0.0 | ||
| pitch: 0.0 | ||
| yaw: 1.57 | ||
|
|
||
| SetupMTCRetractFromGrasp: | ||
| world_frame_name: "world" | ||
| arm_group_name: "manipulator" | ||
| end_effector_group_name: "gripper" | ||
| hand_frame_name: "grasp_link" | ||
| end_effector_closed_pose_name: "close" | ||
|
|
||
| # The lift vector points to the direction of the positive z-axis of the frame marked as the world frame. | ||
| approach_distance: 0.15 | ||
| lift_distance: 0.1 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters