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

Commit

Permalink
Revert "Merge v5.0 to main (#259)"
Browse files Browse the repository at this point in the history
This reverts commit f04ccbb.
  • Loading branch information
chancecardona authored Apr 17, 2024
1 parent f04ccbb commit c291434
Show file tree
Hide file tree
Showing 21 changed files with 210 additions and 228 deletions.
1 change: 0 additions & 1 deletion src/picknik_ur_base_config/config/config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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: []
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Up @@ -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>
Expand Down
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 -->
Expand All @@ -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.">
Expand All @@ -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">
Expand All @@ -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
Expand Up @@ -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>
Expand Down
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

0 comments on commit c291434

Please sign in to comment.