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.
Showing
21 changed files
with
210 additions
and
228 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
25 changes: 25 additions & 0 deletions
25
src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
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,25 @@ | ||
<root> | ||
<TreeNodesModel> | ||
<SubTree ID="Sample April Tag"> | ||
<description> | ||
<p> | ||
Samples the pose of an AprilTag and returns the average measurement. | ||
</p> | ||
</description> | ||
<input_port name="num_samples" default="1">The number of times to sample the pose.</input_port> | ||
<input_port name="tag_id" default="0">The AprilTag ID to detect.</input_port> | ||
<input_port name="apriltag_config" default="apriltag_detection_config.yaml">The configuration file for detecting AprilTags.</input_port> | ||
<input_port name="max_distance" default="0.02">The maximum distance allow between the average pose and the new sample (Euclidean distance between pose positions).</input_port> | ||
<input_port name="max_rotation" default="0.2">The maximum angle allow between the average pose and the new sample (shortest angle between two pose quaternions).</input_port> | ||
<output_port name="avg_pose" default="{avg_pose}">The calculated average pose.</output_port> | ||
</SubTree> | ||
<SubTree ID="Pick Object"> | ||
<description> | ||
<p> | ||
Picks an object at the specified grasp pose. | ||
</p> | ||
</description> | ||
<input_port name="grasp_pose" default="{grasp_pose}">The desired grasp pose.</input_port> | ||
</SubTree> | ||
</TreeNodesModel> | ||
</root> |
16 changes: 16 additions & 0 deletions
16
src/picknik_ur_gazebo_config/open_door_fixed_handle_MTC_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,16 @@ | ||
SetupMTCGraspThenMoveAlongArcPull: | ||
arm_group_name: "manipulator" | ||
end_effector_group_name: "gripper" | ||
end_effector_name: "moveit_ee" | ||
ik_frame_name: "grasp_link" | ||
end_effector_closed_pose_name: "close" | ||
end_effector_open_pose_name: "open" | ||
|
||
# List of controllers to use during the stages that interact with the door. | ||
# TODO: this file currently only exists as gazebo does not have the door admittance controller. Remove if this becomes available. | ||
move_along_arc_controllers: "/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller" | ||
|
||
approach_distance: 0.10 # meters | ||
translation_distance: 0.0 # meters (0 means only move along arc here, no linear component) | ||
rotation_distance: 55.0 # Degrees | ||
use_circular_arc: true |
23 changes: 23 additions & 0 deletions
23
src/picknik_ur_gazebo_config/open_door_lever_handle_MTC_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,23 @@ | ||
SetupMTCGraspAndTwistThenMoveAlongArcPush: | ||
arm_group_name: "manipulator" | ||
end_effector_group_name: "gripper" | ||
ik_frame_name: "grasp_link" | ||
|
||
# List of controllers to use during the stages that interact with the door. | ||
# TODO: this file currently only exists as gazebo does not have the door admittance controller. Remove if this becomes available. | ||
move_along_arc_controllers: "/joint_trajectory_controller_chained_open_door /robotiq_gripper_controller" | ||
|
||
# The hand should be placed 75% of the way down the handle length when | ||
# manipulating the handle. Valid value range [0, 1.0]. | ||
handle_grasp_offset_percent: 0.75 | ||
# The distance above the handle the robot should approach with before | ||
# manipulating it. | ||
above_handle_approach_distance: 0.03 # meters | ||
# The distance the hand should move past the end of the handle to release it. | ||
release_handle_distance: 0.03 # meters | ||
# How many degrees to rotate the lever handle (use positive value for | ||
# counter-clockwise rotation and negative value for clockwise rotation). | ||
handle_twist_angle: 65.0 # degrees | ||
approach_distance: 0.10 # meters | ||
push_open_distance: 0.05 # meters | ||
use_circular_arc: true |
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
55 changes: 55 additions & 0 deletions
55
src/picknik_ur_site_config/objectives/detect_lever_handle_graspable_object.xml
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,55 @@ | ||
<?xml version="1.0" encoding="UTF-8"?> | ||
<root BTCPP_format="4" main_tree_to_execute="Detect Lever Handle Graspable Object"> | ||
<!-- ////////// --> | ||
<BehaviorTree ID="Detect Lever Handle Graspable Object" _description="Detects a lever-handle graspable object from a depth image and returns 2 PoseStampeds to represent the handle."> | ||
<!-- 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="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_tip_masks2d}" valid_classes="lever_handle_tip"/> | ||
<Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_tip_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="{handle_tip_objects}"/> | ||
<Decorator ID="ForceSuccess"> | ||
<Decorator ID="ForEachGraspableObject" vector_in="{handle_tip_objects}" out="{handle_tip_object}"> | ||
<Action ID="AlwaysFailure"/> | ||
</Decorator> | ||
</Decorator> | ||
<!-- Add object to planning scene and extract its pose --> | ||
<Action ID="ModifyObjectInPlanningScene" object="{handle_tip_object}" apply_planning_scene_service="/apply_planning_scene"/> | ||
<Action ID="ExtractGraspableObjectPose" graspable_object="{handle_tip_object}" pose="{handle_tip_pose}"/> | ||
<!-- 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_pivot_masks2d}" valid_classes="lever_handle_pivot"/> | ||
<Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_pivot_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="{handle_pivot_objects}"/> | ||
<Decorator ID="ForceSuccess"> | ||
<Decorator ID="ForEachGraspableObject" vector_in="{handle_pivot_objects}" out="{handle_pivot_object}"> | ||
<Action ID="AlwaysFailure"/> | ||
</Decorator> | ||
</Decorator> | ||
<!-- Add object to planning scene and extract its pose --> | ||
<Action ID="ModifyObjectInPlanningScene" object="{handle_pivot_object}" apply_planning_scene_service="/apply_planning_scene"/> | ||
<Action ID="ExtractGraspableObjectPose" graspable_object="{handle_pivot_object}" pose="{handle_pivot_pose}"/> | ||
<Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected lever handle." log_level="info"/> | ||
</Control> | ||
</BehaviorTree> | ||
</root> |
27 changes: 0 additions & 27 deletions
27
src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml
This file was deleted.
Oops, something went wrong.
Oops, something went wrong.