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

SORRY SQUASH MERGED Revert "Merge v5.0 to main" #260

Merged
merged 1 commit into from
Apr 17, 2024
Merged
Changes from all commits
Commits
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
1 change: 0 additions & 1 deletion src/picknik_ur_base_config/config/config.yaml
Original file line number Diff line number Diff line change
@@ -196,7 +196,6 @@ ros2_control:
- "joint_trajectory_controller"
- "admittance_controller_open_door"
- "joint_trajectory_controller_chained_open_door"
- "joint_trajectory_admittance_controller"
# Any controllers here will not be spawned by MoveIt Pro.
# [Optional, default=[]]
controllers_not_managed: []
Original file line number Diff line number Diff line change
@@ -19,8 +19,6 @@ controller_manager:
type: admittance_controller/AdmittanceController
joint_trajectory_controller_chained_open_door:
type: joint_trajectory_controller/JointTrajectoryController
joint_trajectory_admittance_controller:
type: moveit_pro_controllers/JointTrajectoryAdmittanceController

io_and_status_controller:
ros__parameters:
@@ -260,18 +258,3 @@ admittance_controller_open_door:

# general settings
enable_parameter_update_without_reactivation: true

joint_trajectory_admittance_controller:
ros__parameters:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
base_frame: base_link
tip_frame: grasp_link
sensor_frame: tool0
ee_frame: grasp_link
ft_sensor_name: tcp_fts_sensor
1 change: 0 additions & 1 deletion src/picknik_ur_base_config/package.xml
Original file line number Diff line number Diff line change
@@ -14,7 +14,6 @@
<exec_depend>admittance_controller</exec_depend>
<exec_depend>kinematics_interface_kdl</exec_depend>
<exec_depend>moveit_planners_stomp</exec_depend>
<exec_depend>moveit_pro_controllers</exec_depend>
<exec_depend>moveit_ros_perception</exec_depend>
<exec_depend>moveit_studio_agent</exec_depend>
<exec_depend>moveit_studio_behavior</exec_depend>
25 changes: 25 additions & 0 deletions src/picknik_ur_gazebo_config/objectives/tree_nodes_model.xml
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>
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
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
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?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="Segments out a door from an input depth image and attempts to fit an output GraspableObject.">
<BehaviorTree ID="Detect Door Graspable Object" _description="Detects a door grapsable object from a depth image and gets a 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 -->
@@ -11,23 +11,24 @@
<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}"/>
<!-- 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="15.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="10.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, 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>
<!-- 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}"/>
<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,4 +1,4 @@
<?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.">
@@ -12,7 +12,7 @@
<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="15.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="10.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">
@@ -27,9 +27,10 @@
<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>
Original file line number Diff line number Diff line change
@@ -17,7 +17,7 @@
</Decorator>
</Decorator>
<!-- fit 3D mask to a line segment and return the two end poses. -->
<Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_poses="{line_segment_poses}"/>
<Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_start_point="{line_segment_start_point}" line_segment_end_point="{line_segment_end_point}"/>
<Action ID="LogMessage" message="Fit a line segment to the detected hinge successfully." log_level="info"/>
</Control>
</BehaviorTree>
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>

This file was deleted.

Loading
Loading