From d4bde9ec8bbc01fcf53bf4f8597c3b844ea0709d Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Fri, 30 Aug 2024 21:03:48 -0700 Subject: [PATCH 1/3] wip --- .gitignore | 3 + spot_rl_experiments/configs/config.yaml | 2 +- .../spot_rl/envs/skill_manager.py | 42 ++-- .../spot_rl/utils/plane_detection.py | 49 +++- .../spot_rl/utils/search_table_location.py | 230 ++++++++++++++---- 5 files changed, 256 insertions(+), 70 deletions(-) diff --git a/.gitignore b/.gitignore index 002b5af4..c53cd5c9 100644 --- a/.gitignore +++ b/.gitignore @@ -26,3 +26,6 @@ spot_rl_experiments/experiments/skill_test/logs/* spot_rl_experiments/experiments/skill_test/table_detections/* ros_tcp/ros_communication_client.egg-info/** *.npy +*.pkl* +*.json +*.csv \ No newline at end of file diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index ba8eaf46..f5c88803 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -136,7 +136,7 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # T INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] -SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -125, 80, 0, 85, 0] +SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -150, 80, 0, 75, 0] #[0, -125, 80, 0, 85, 0] PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index ebf92282..46c803af 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -509,28 +509,28 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray conditional_print(message=message, verbose=self.verbose) is_local = True # estimate waypoint - try: - ( - place_target_location, - place_target_in_gripper_camera, - _, - ) = detect_place_point_by_pcd_method( - self.spot, - self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, - percentile=0 if visualize else 70, - visualize=visualize, - height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, + #try: + ( + place_target_location, + place_target_in_gripper_camera, + _, + ) = detect_place_point_by_pcd_method( + self.spot, + self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, + percentile=0 if visualize else 70, + visualize=visualize, + height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, + ) + print(f"Estimate Place xyz: {place_target_location}") + if visualize: + plot_place_point_in_gripper_image( + self.spot, place_target_in_gripper_camera ) - print(f"Estimate Place xyz: {place_target_location}") - if visualize: - plot_place_point_in_gripper_image( - self.spot, place_target_in_gripper_camera - ) - except Exception as e: - message = f"Failed to estimate place way point due to {str(e)}" - conditional_print(message=message, verbose=self.verbose) - print(message) - return False, message + # except Exception as e: + # message = f"Failed to estimate place way point due to {str(e)}" + # conditional_print(message=message, verbose=self.verbose) + # print(message) + #return False, message place_x, place_y, place_z = place_target_location.astype(np.float64).tolist() status, message = self.place( diff --git a/spot_rl_experiments/spot_rl/utils/plane_detection.py b/spot_rl_experiments/spot_rl/utils/plane_detection.py index c9e2865f..c45575fc 100644 --- a/spot_rl_experiments/spot_rl/utils/plane_detection.py +++ b/spot_rl_experiments/spot_rl/utils/plane_detection.py @@ -157,16 +157,55 @@ def DetectMultiPlanes(points, min_ratio=0.05, threshold=0.01, iterations=1000): return plane_list +def trying_new_segment_plane(pcd): + + #assert (pcd.has_normals()) + # pcd = NumpyToPCD(points) + if not pcd.has_normals(): + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) + ) + # using all defaults + oboxes = pcd.detect_planar_patches( + normal_variance_threshold_deg=70, + coplanarity_deg=80, + outlier_ratio=0.75, + min_plane_edge_length=0, + min_num_points=0, + search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30)) + + print("Detected {} patches".format(len(oboxes))) + + planes = [ pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) for obox in oboxes] + #geometries = [] + # for obox in oboxes: + # plane_pcd = pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) + # planes.append(plane_pcd) + # mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(obox, scale=[1, 1, 0.0001]) + # mesh.paint_uniform_color(obox.color) + # geometries.append(plane) + # geometries.append(obox) + #geometries.append(pcd) + o3d.visualization.draw_geometries(planes) + # o3d.visualization.draw_geometries(geometries, + # zoom=0.62, + # front=[0.4361, -0.2632, -0.8605], + # lookat=[2.4947, 1.7728, 1.5541], + # up=[-0.1726, -0.9630, 0.2071]) + return planes def plane_detect(pcd): + return trying_new_segment_plane(pcd) points = PCDToNumpy(pcd) points = RemoveNoiseStatistical(points, nb_neighbors=50, std_ratio=0.5) # DrawPointCloud(points, color=(0.4, 0.4, 0.4)) t0 = time.time() + results = DetectMultiPlanes( points, min_ratio=0.05, threshold=0.005, iterations=2000 ) + print("Time:", time.time() - t0) planes = [] colors = [] @@ -195,8 +234,8 @@ def plane_detect(pcd): highest_pts = dist high_i = i - planes = [planes[high_i]] - colors = [colors[high_i]] - planes = np.concatenate(planes, axis=0) - colors = np.concatenate(colors, axis=0) - return DrawResult(planes, colors) + planes_selected = [planes[high_i]] + colors_selected = [colors[high_i]] + planes_selected = np.concatenate(planes_selected, axis=0) + colors_selected = np.concatenate(colors_selected, axis=0) + return DrawResult(planes_selected, colors_selected), planes, colors diff --git a/spot_rl_experiments/spot_rl/utils/search_table_location.py b/spot_rl_experiments/spot_rl/utils/search_table_location.py index 646e5b21..ce34e614 100644 --- a/spot_rl_experiments/spot_rl/utils/search_table_location.py +++ b/spot_rl_experiments/spot_rl/utils/search_table_location.py @@ -10,11 +10,11 @@ from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME from spot_rl.envs.base_env import SpotBaseEnv from spot_rl.utils.gripper_t_intel_path import GRIPPER_T_INTEL_PATH -from spot_rl.utils.heuristic_nav import get_3d_point, get_best_uvz_from_detection -from spot_rl.utils.plane_detection import plane_detect +from spot_rl.utils.plane_detection import plane_detect, NumpyToPCD from spot_rl.utils.utils import ros_topics as rt from spot_wrapper.spot import Spot, image_response_to_cv2 from std_msgs.msg import String +from spot_rl.utils.pixel_to_3d_conversion_utils import project_3d_to_pixel_uv, get_best_uvz_from_detection, get_3d_point class DetectionSubscriber: @@ -87,19 +87,6 @@ def farthest_point_sampling(points, num_samples): ) return farthest_pts - -def project_3d_to_pixel_uv(points_3d, fx, fy, cx, cy): - """ - Back projects given xyz 3d point to pixel location u,v using camera intrinsics - """ - Z = points_3d[:, -1] - X_Z = points_3d[:, 0] / Z - Y_Z = points_3d[:, 1] / Z - u = (fx * X_Z) + cx - v = (fy * Y_Z) + cy - return np.stack([u.flatten(), v.flatten()], axis=1).reshape(-1, 2) - - def generate_point_cloud( image, depth, mask, fx=383.883, fy=383.883, cx=324.092, cy=238.042 ): @@ -197,7 +184,7 @@ def plot_intel_point_in_gripper_image( cx = gripper_intrinsics.principal_point.x cy = gripper_intrinsics.principal_point.y point2d_in_gripper = project_3d_to_pixel_uv( - point3d_in_gripper.reshape(1, 3), fx, fy, cx, cy + point3d_in_gripper.reshape(1, 3), gripper_intrinsics )[0] img_with_point = cv2.circle( gripper_image.copy(), @@ -217,10 +204,7 @@ def plot_place_point_in_gripper_image(spot: Spot, point_in_gripper_camera: np.nd intrinsics_gripper = gripper_resps[0].source.pinhole.intrinsics pixel = project_3d_to_pixel_uv( point_in_gripper_camera.reshape(1, 3), - intrinsics_gripper.focal_length.x, - intrinsics_gripper.focal_length.y, - intrinsics_gripper.principal_point.x, - intrinsics_gripper.principal_point.y, + intrinsics_gripper )[0] print(f"Pixel in gripper image {pixel}") gripper_rgb = cv2.circle( @@ -304,10 +288,158 @@ def filter_pointcloud_by_normals_in_the_given_direction( pcd_dir_filtered = pcd_with_normals.select_by_index( np.where(cosines > cosine_thresh)[0] ) - if visualize: - o3d.visualization.draw_geometries([pcd_dir_filtered]) + #if visualize: + #o3d.visualization.draw_geometries([pcd_dir_filtered]) return pcd_dir_filtered +def filter_planes_by_normals_in_the_given_direction( + planes:np.ndarray, + plane_normals:np.ndarray, + direction_vector: np.ndarray, + body_T_hand: np.ndarray = np.eye(4), + gripper_T_intel: np.ndarray = np.eye(4), +): + """Filter point clouds based on the normal""" + direction_vector = direction_vector.reshape(3) + normals = plane_normals.reshape(-1, 3) + body_T_intel = np.array(body_T_hand @ gripper_T_intel) + normals_in_body = np.dot(body_T_intel[:3, :3], normals.T).T.reshape(-1, 3) + # Compute the dot product to get the cosines + # Calculate the angle using the dot product formula + cosines = (np.dot(normals_in_body, direction_vector) / (np.linalg.norm(normals_in_body) * np.linalg.norm(direction_vector))) + + + euclidean_dist, height_of_plane, number_of_pts = [], [], [] + for plane in planes: + center_point = plane.get_center().reshape(1, 3) + norm = np.linalg.norm(center_point[0]) + euclidean_dist.append(norm) + center_point_in_body = np.dot(body_T_intel[:3, :3], center_point.T).T.reshape(1, 3)[0] + height_of_plane.append(center_point_in_body[-1]) + number_of_pts.append(np.array(plane.points).shape[0]) + + euclidean_dist = np.array(euclidean_dist) / np.max(euclidean_dist) + height_of_plane = np.array(height_of_plane)/ max(height_of_plane) + number_of_pts = np.array(number_of_pts)/max(number_of_pts) + + #cost = -0.0*euclidean_dist + 0.05*height_of_plane + 0.05*number_of_pts + 0.9*cosines + cost = cosines + 0.3*height_of_plane + # Filter out the point clouds + argmax = int(np.argmax(cost)) + + return argmax, normals_in_body + +# def is_collinear(p1, p2, p3): +# """ +# Check if three points are collinear by computing the cross product. + +# Parameters: +# p1, p2, p3 (numpy.ndarray): Three points in 3D space. + +# Returns: +# bool: True if the points are collinear, False otherwise. +# """ +# v1 = p2 - p1 +# v2 = p3 - p1 +# cross_product = np.cross(v1, v2) +# return np.linalg.norm(cross_product) < 1e-6 + +# def compute_plane_normal_by_cross_product(points): +# n = points.shape[0] + +# if n < 3: +# raise ValueError("The point cloud must have at least 3 points.") + +# # Initial selection: first, middle, and last points +# p1 = points[0] +# p2 = points[n // 2] +# p3 = points[-1] + +# # Check if the initially selected points are collinear +# if not is_collinear(p1, p2, p3): +# return p1, p2, p3 + +# # If collinear, randomly sample points until a non-collinear triplet is found +# t1 = time.time() +# while True: +# if time.time() - t1 > 30 : +# raise Exception(f"couldnt find non-colinear points in the given plane for calculating the plane normal, been more than 30 secs") +# p1, p2, p3 = points[np.random.choice(n, 3, replace=False)] +# if not is_collinear(p1, p2, p3): +# # Compute vectors v1 and v2 +# v1 = p2 - p1 +# v2 = p3 - p1 + +# # Compute the cross product +# normal = np.cross(v1, v2) + +# # Normalize the normal vector +# normal_normalized = normal / np.linalg.norm(normal) + +# return normal_normalized, np.mean(points, axis=0) + + +def compute_plane_normal_by_average(pcd): + #pcd = NumpyToPCD(points) + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) + ) + normals = np.array(pcd.normals) + return normals.mean(axis=0), pcd.get_center().reshape(-1) + +# def compute_plane_normal(points): +# """ +# Compute the normal of a plane given a point cloud on the plane. + +# Parameters: +# points (numpy.ndarray): An array of shape (m, 3) representing points on the plane. + +# Returns: +# numpy.ndarray: The normal vector of the plane. +# """ +# # Step 1: Center the points (subtract the mean) +# centroid = np.mean(points, axis=0) +# centered_points = points - centroid + +# # Step 2: Compute the covariance matrix +# cov_matrix = np.cov(centered_points, rowvar=False) + +# # Step 3: Perform eigenvalue decomposition +# eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix) + +# # The normal vector is the eigenvector corresponding to the smallest eigenvalue +# normal_vector = eigenvectors[:, np.argmin(eigenvalues)] + +# return normal_vector, centroid + +def visualize_all_planes(planes, image_rgb_orig, camera_intrinsics, all_planes_normals=None, normal_in_body=None): + #project all 3D plane points back to image_rgb + #fx, fy, cx, cy = camera_intrinsics + np.random.seed(42) # Optional: set a seed for reproducibility + image_rgb = image_rgb_orig.copy() + for p_i, plane in enumerate(planes): + if all_planes_normals is not None: + plane_normal = all_planes_normals[p_i] + plane_origin = plane.get_center().reshape(-1) #np.mean(plane, axis=0) + else: + plane_normal, plane_origin = compute_plane_normal_by_average(plane) + plane_extend_origin = plane_origin + 0.1*plane_normal + plane_normals_2d = project_3d_to_pixel_uv(np.vstack((plane_origin, plane_extend_origin)), camera_intrinsics) + plane_normals_2d = plane_normals_2d.astype(int).tolist() + image_rgb = cv2.arrowedLine(image_rgb, plane_normals_2d[0], plane_normals_2d[1], (0, 0, 255), 2, tipLength=0.2) + print(f"Plane id {p_i} plane normal {plane_normal}") + plane_pts_2d = project_3d_to_pixel_uv(np.array(plane.points), camera_intrinsics) + color = tuple(np.random.randint(0, 256, size=3).astype(int).tolist()) + for point in plane_pts_2d: + x, y = list(map(int, point)) + image_rgb = cv2.circle(image_rgb, (x, y), 1, color, 1) + if len(planes) == 1: + cv2.putText(image_rgb, f"N:{np.round(normal_in_body, 2)}", (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1) + + cv2.namedWindow("plane detections", cv2.WINDOW_NORMAL) + cv2.imshow("plane detections", image_rgb) + cv2.waitKey(0) + cv2.destroyAllWindows() def detect_place_point_by_pcd_method( spot, @@ -350,25 +482,37 @@ def detect_place_point_by_pcd_method( cy = camera_intrinsics_intel.principal_point.y # u,v in pixel -> depth at u,v, intriniscs -> xyz in 3D pcd = generate_point_cloud(img, depth_raw, mask, fx, fy, cx, cy) - - pcd.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) - ) - # 0.25 - 0 < angle < 75 - pcd = filter_pointcloud_by_normals_in_the_given_direction( - pcd, - np.array([0.0, 0.0, 1.0]), - 0.5, - body_T_hand, - gripper_T_intel, - visualize=visualize, - ) + #pcd = pcd.uniform_down_sample(every_k_points=2) + # pcd.estimate_normals( + # search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) + # ) + # # 0.25 - 0 < angle < 75 + # pcd = filter_pointcloud_by_normals_in_the_given_direction( + # pcd, + # np.array([0.0, 0.0, 1.0]), + # 0.25, + # body_T_hand, + # gripper_T_intel, + # visualize=visualize, + # ) # Down-sample by using voxel # pcd = pcd.voxel_down_sample(voxel_size=0.01) # print(f"After Downsampling {np.array(pcd.points).shape}") - plane_pcd = plane_detect(pcd) + all_planes = plane_detect(pcd) + + all_plane_normals = np.array([compute_plane_normal_by_average(plane)[0] for plane in all_planes], dtype=np.float32) + + plane_index, normals_in_body = filter_planes_by_normals_in_the_given_direction(all_planes, all_plane_normals.copy(), np.array([0.0, 0.0, 1.0]), body_T_hand, gripper_T_intel) + + plane_pcd = all_planes[plane_index] + + if visualize: + #for plane_index in range(len(all_planes)): + #visualize_all_planes(all_planes, img, camera_intrinsics_intel, all_plane_normals) + visualize_all_planes([all_planes[plane_index]], img, camera_intrinsics_intel, [all_plane_normals[plane_index]], normals_in_body[plane_index]) + # Down-sample plane_pcd.points = o3d.utility.Vector3dVector( farthest_point_sampling(np.array(plane_pcd.points), 1024) @@ -381,7 +525,7 @@ def detect_place_point_by_pcd_method( target_points, selected_point = get_target_points_by_heuristic( np.array(plane_pcd.points) ) - corners_xys = project_3d_to_pixel_uv(target_points, fx, fy, cx, cy) + corners_xys = project_3d_to_pixel_uv(target_points, camera_intrinsics_intel) y_threshold = np.percentile(corners_xys[:, -1], percentile) @@ -420,10 +564,10 @@ def detect_place_point_by_pcd_method( img_with_bbox, (int(selected_xy[0]), int(selected_xy[1])), 2, (0, 0, 255) ) # For debug - # cv2.namedWindow("table_detection", cv2.WINDOW_NORMAL) - # cv2.imshow("table_detection", img_with_bbox) - # cv2.waitKey(0) - # cv2.destroyAllWindows() + cv2.namedWindow("table_detection", cv2.WINDOW_NORMAL) + cv2.imshow("table_detection", img_with_bbox) + cv2.waitKey(0) + cv2.destroyAllWindows() cv2.imwrite("table_detection.png", img_with_bbox) point_in_body = body_T_hand.transform_point(mn.Vector3(*selected_point_in_gripper)) @@ -542,7 +686,7 @@ def contrained_place_point_estimation( target_points, selected_point = get_target_points_by_heuristic( np.array(plane_pcd.points) ) - corners_xys = project_3d_to_pixel_uv(target_points, fx, fy, cx, cy) + corners_xys = project_3d_to_pixel_uv(target_points, camera_intrinsics_intel) y_threshold = np.percentile(corners_xys[:, -1], percentile) @@ -584,7 +728,7 @@ def contrained_place_point_estimation( cx = camera_intrinsics_gripper.principal_point.x cy = camera_intrinsics_gripper.principal_point.y selected_xy_in_gripper = project_3d_to_pixel_uv( - selected_point_in_gripper.reshape(1, 3), fx, fy, cx, cy + selected_point_in_gripper.reshape(1, 3), camera_intrinsics_gripper )[0] depth_at_selected_xy_in_gripper = ( depth_at_selected_xy + 0.02 From 46f40fc189122891ea6f0b958b697c8db53aa6ae Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Wed, 4 Sep 2024 19:04:01 -0700 Subject: [PATCH 2/3] seems to work, plane filtering --- .../spot_rl/utils/plane_detection.py | 110 +++-- .../spot_rl/utils/search_table_location.py | 380 ++++++++---------- 2 files changed, 232 insertions(+), 258 deletions(-) diff --git a/spot_rl_experiments/spot_rl/utils/plane_detection.py b/spot_rl_experiments/spot_rl/utils/plane_detection.py index c45575fc..76228b4f 100644 --- a/spot_rl_experiments/spot_rl/utils/plane_detection.py +++ b/spot_rl_experiments/spot_rl/utils/plane_detection.py @@ -126,6 +126,7 @@ def DrawResult(points, colors): pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(points) pcd.colors = o3d.utility.Vector3dVector(colors) + o3d.visualization.draw_geometries([pcd]) return pcd @@ -157,60 +158,64 @@ def DetectMultiPlanes(points, min_ratio=0.05, threshold=0.01, iterations=1000): return plane_list -def trying_new_segment_plane(pcd): - - #assert (pcd.has_normals()) - # pcd = NumpyToPCD(points) - if not pcd.has_normals(): - pcd.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) - ) - # using all defaults - oboxes = pcd.detect_planar_patches( - normal_variance_threshold_deg=70, - coplanarity_deg=80, - outlier_ratio=0.75, - min_plane_edge_length=0, - min_num_points=0, - search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30)) - - print("Detected {} patches".format(len(oboxes))) - - planes = [ pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) for obox in oboxes] - #geometries = [] - # for obox in oboxes: - # plane_pcd = pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) - # planes.append(plane_pcd) - # mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(obox, scale=[1, 1, 0.0001]) - # mesh.paint_uniform_color(obox.color) - # geometries.append(plane) - # geometries.append(obox) - #geometries.append(pcd) - o3d.visualization.draw_geometries(planes) - # o3d.visualization.draw_geometries(geometries, - # zoom=0.62, - # front=[0.4361, -0.2632, -0.8605], - # lookat=[2.4947, 1.7728, 1.5541], - # up=[-0.1726, -0.9630, 0.2071]) - return planes -def plane_detect(pcd): - return trying_new_segment_plane(pcd) +# def trying_new_segment_plane(pcd): + +# #assert (pcd.has_normals()) +# # pcd = NumpyToPCD(points) +# if not pcd.has_normals(): +# pcd.estimate_normals( +# search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) +# ) +# # using all defaults +# oboxes = pcd.detect_planar_patches( +# normal_variance_threshold_deg=70, +# coplanarity_deg=80, +# outlier_ratio=0.75, +# min_plane_edge_length=0, +# min_num_points=0, +# search_param=o3d.geometry.KDTreeSearchParamKNN(knn=30)) + +# print("Detected {} patches".format(len(oboxes))) + +# planes = [ pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) for obox in oboxes] +# geometries = [] +# for obox in oboxes: +# plane_pcd = pcd.select_by_index(obox.get_point_indices_within_bounding_box(pcd.points)) +# planes.append(plane_pcd) +# mesh = o3d.geometry.TriangleMesh.create_from_oriented_bounding_box(obox, scale=[1, 1, 0.0001]) +# mesh.paint_uniform_color(obox.color) +# #geometries.append(plane) +# geometries.append(obox) + +# geometries.append(pcd) +# o3d.visualization.draw_geometries([pcd]) +# o3d.visualization.draw_geometries(geometries, +# zoom=0.62, +# front=[0.4361, -0.2632, -0.8605], +# lookat=[2.4947, 1.7728, 1.5541], +# up=[-0.1726, -0.9630, 0.2071]) +# #o3d.visualization.draw_geometries(planes) +# return planes + + +def plane_detect(pcd, visualize=False): + if visualize: + o3d.visualization.draw_geometries([pcd]) + points = PCDToNumpy(pcd) points = RemoveNoiseStatistical(points, nb_neighbors=50, std_ratio=0.5) # DrawPointCloud(points, color=(0.4, 0.4, 0.4)) t0 = time.time() - + results = DetectMultiPlanes( points, min_ratio=0.05, threshold=0.005, iterations=2000 ) - + print("Time:", time.time() - t0) planes = [] - colors = [] - highest_pts, high_i = -np.inf, 0 # lowest_dist_to_camera, low_i = np.inf, 0 print(f"{len(results)} plane are detected") for i, (_, plane) in enumerate(results): @@ -224,18 +229,11 @@ def plane_detect(pcd): color[:, 1] = g color[:, 2] = b + plane = NumpyToPCD(plane) + plane.colors = o3d.utility.Vector3dVector(color) planes.append(plane) - colors.append(color) - - # check depth at centroid - plane_pcd = NumpyToPCD(plane) - dist = -plane_pcd.get_center()[-1] + plane.shape[0] - if dist >= highest_pts: - highest_pts = dist - high_i = i - - planes_selected = [planes[high_i]] - colors_selected = [colors[high_i]] - planes_selected = np.concatenate(planes_selected, axis=0) - colors_selected = np.concatenate(colors_selected, axis=0) - return DrawResult(planes_selected, colors_selected), planes, colors + + if visualize: + o3d.visualization.draw_geometries(planes) + + return planes diff --git a/spot_rl_experiments/spot_rl/utils/search_table_location.py b/spot_rl_experiments/spot_rl/utils/search_table_location.py index ce34e614..354ceeb1 100644 --- a/spot_rl_experiments/spot_rl/utils/search_table_location.py +++ b/spot_rl_experiments/spot_rl/utils/search_table_location.py @@ -10,11 +10,20 @@ from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME from spot_rl.envs.base_env import SpotBaseEnv from spot_rl.utils.gripper_t_intel_path import GRIPPER_T_INTEL_PATH -from spot_rl.utils.plane_detection import plane_detect, NumpyToPCD +from spot_rl.utils.pixel_to_3d_conversion_utils import ( + get_3d_point, + get_best_uvz_from_detection, + project_3d_to_pixel_uv, +) +from spot_rl.utils.plane_detection import NumpyToPCD, plane_detect from spot_rl.utils.utils import ros_topics as rt from spot_wrapper.spot import Spot, image_response_to_cv2 from std_msgs.msg import String -from spot_rl.utils.pixel_to_3d_conversion_utils import project_3d_to_pixel_uv, get_best_uvz_from_detection, get_3d_point + +try: + import sophuspy as sp +except Exception: + import sophus as sp class DetectionSubscriber: @@ -87,6 +96,7 @@ def farthest_point_sampling(points, num_samples): ) return farthest_pts + def generate_point_cloud( image, depth, mask, fx=383.883, fy=383.883, cx=324.092, cy=238.042 ): @@ -179,10 +189,6 @@ def plot_intel_point_in_gripper_image( [point3d_in_gripper.x, point3d_in_gripper.y, point3d_in_gripper.z] ) - fx = gripper_intrinsics.focal_length.x - fy = gripper_intrinsics.focal_length.y - cx = gripper_intrinsics.principal_point.x - cy = gripper_intrinsics.principal_point.y point2d_in_gripper = project_3d_to_pixel_uv( point3d_in_gripper.reshape(1, 3), gripper_intrinsics )[0] @@ -203,8 +209,7 @@ def plot_place_point_in_gripper_image(spot: Spot, point_in_gripper_camera: np.nd gripper_rgb = image_response_to_cv2(gripper_resps[0]) intrinsics_gripper = gripper_resps[0].source.pinhole.intrinsics pixel = project_3d_to_pixel_uv( - point_in_gripper_camera.reshape(1, 3), - intrinsics_gripper + point_in_gripper_camera.reshape(1, 3), intrinsics_gripper )[0] print(f"Pixel in gripper image {pixel}") gripper_rgb = cv2.circle( @@ -244,19 +249,20 @@ def get_arguments(spot: Spot, gripper_T_intel: np.ndarray): hand_T_intel = gripper_T_intel if place_point_generation_src else np.identity(4) # hand_T_intel[:3, :3] = np.identity(3) - hand_T_intel = mn.Matrix4.from_(hand_T_intel[:3, :3], hand_T_intel[:3, 3]) + hand_T_intel = sp.SE3(hand_T_intel[:3, :3], hand_T_intel[:3, 3]) # Load hand_T_intel from caliberation image_resps = [image_response_to_cv2(image_resp) for image_resp in image_resps] - body_T_hand: mn.Matrix4 = spot.get_magnum_Matrix4_spot_a_T_b( + body_T_hand: sp.SE3 = spot.get_sophus_SE3_spot_a_T_b( + None, GRAV_ALIGNED_BODY_FRAME_NAME, # "body", "link_wr1", ) - hand_T_gripper: mn.Matrix4 = spot.get_magnum_Matrix4_spot_a_T_b( + hand_T_gripper: sp.SE3 = spot.get_sophus_SE3_spot_a_T_b( + snapshot_tree, "arm0.link_wr1", "hand_color_image_sensor", - snapshot_tree, ) - body_T_hand = body_T_hand @ hand_T_gripper + body_T_hand = body_T_hand * hand_T_gripper # load body_T_hand # body_T_hand = body_T_hand.__matmul__(hand_T_intel) # body_T_intel return ( @@ -273,160 +279,135 @@ def filter_pointcloud_by_normals_in_the_given_direction( pcd_with_normals: o3d.geometry.PointCloud, direction_vector: np.ndarray, cosine_thresh: float = 0.25, - body_T_hand: np.ndarray = np.eye(4), - gripper_T_intel: np.ndarray = np.eye(4), + body_T_hand: sp.SE3 = sp.SE3(np.eye(4)), + gripper_T_intel: sp.SE3 = sp.SE3(np.eye(4)), visualize: bool = False, ): """Filter point clouds based on the normal""" direction_vector = direction_vector.reshape(3) normals = np.asarray(pcd_with_normals.normals).reshape(-1, 3) - body_T_intel = np.array(body_T_hand @ gripper_T_intel) - normals_in_body = np.dot(body_T_intel[:3, :3], normals.T).T.reshape(-1, 3) + body_T_intel = sp.SO3((body_T_hand * gripper_T_intel).rotationMatrix()) + normals_in_body = body_T_intel * normals # Compute the dot product to get the cosines cosines = (normals_in_body @ direction_vector).reshape(-1) # Filter out the point clouds pcd_dir_filtered = pcd_with_normals.select_by_index( np.where(cosines > cosine_thresh)[0] ) - #if visualize: - #o3d.visualization.draw_geometries([pcd_dir_filtered]) + # if visualize: + # o3d.visualization.draw_geometries([pcd_dir_filtered]) return pcd_dir_filtered -def filter_planes_by_normals_in_the_given_direction( - planes:np.ndarray, - plane_normals:np.ndarray, - direction_vector: np.ndarray, - body_T_hand: np.ndarray = np.eye(4), - gripper_T_intel: np.ndarray = np.eye(4), + +def rank_planes( + planes: np.ndarray, + all_plane_normals: np.ndarray, + body_T_hand: sp.SE3 = sp.SE3(np.eye(4)), + gripper_T_intel: sp.SE3 = sp.SE3(np.eye(4)), + max_number_of_points: float = 1e8, ): """Filter point clouds based on the normal""" - direction_vector = direction_vector.reshape(3) - normals = plane_normals.reshape(-1, 3) - body_T_intel = np.array(body_T_hand @ gripper_T_intel) - normals_in_body = np.dot(body_T_intel[:3, :3], normals.T).T.reshape(-1, 3) + # breakpoint() + body_T_intel: sp.SE3 = body_T_hand * gripper_T_intel + # Compute the dot product to get the cosines # Calculate the angle using the dot product formula - cosines = (np.dot(normals_in_body, direction_vector) / (np.linalg.norm(normals_in_body) * np.linalg.norm(direction_vector))) - - - euclidean_dist, height_of_plane, number_of_pts = [], [], [] - for plane in planes: - center_point = plane.get_center().reshape(1, 3) - norm = np.linalg.norm(center_point[0]) - euclidean_dist.append(norm) - center_point_in_body = np.dot(body_T_intel[:3, :3], center_point.T).T.reshape(1, 3)[0] - height_of_plane.append(center_point_in_body[-1]) - number_of_pts.append(np.array(plane.points).shape[0]) - - euclidean_dist = np.array(euclidean_dist) / np.max(euclidean_dist) - height_of_plane = np.array(height_of_plane)/ max(height_of_plane) - number_of_pts = np.array(number_of_pts)/max(number_of_pts) - - #cost = -0.0*euclidean_dist + 0.05*height_of_plane + 0.05*number_of_pts + 0.9*cosines - cost = cosines + 0.3*height_of_plane + euclidean_dist, number_of_pts = [], [] + indices_of_min_dist = [] + planes_points = [np.array(plane.points).reshape(-1, 3) for plane in planes] + + for plane_points in planes_points: + plane_points_in_body = body_T_intel * plane_points + norms_of_plane_points = np.linalg.norm(plane_points_in_body, axis=1) + argmin_dist = int(np.argmin(norms_of_plane_points)) + + indices_of_min_dist.append(argmin_dist) + euclidean_dist.append(norms_of_plane_points[argmin_dist] / 0.5) + number_of_pts.append(plane_points.shape[0] / max_number_of_points) + + # euclidean_dist = np.array(euclidean_dist) / 0.5 + + cost = -0.8 * np.array(euclidean_dist) + 0.2 * np.array( + number_of_pts + ) # + 0.9*cosines + # cost = cosines + 0.3*height_of_plane # Filter out the point clouds + # breakpoint() argmax = int(np.argmax(cost)) - - return argmax, normals_in_body - -# def is_collinear(p1, p2, p3): -# """ -# Check if three points are collinear by computing the cross product. - -# Parameters: -# p1, p2, p3 (numpy.ndarray): Three points in 3D space. - -# Returns: -# bool: True if the points are collinear, False otherwise. -# """ -# v1 = p2 - p1 -# v2 = p3 - p1 -# cross_product = np.cross(v1, v2) -# return np.linalg.norm(cross_product) < 1e-6 - -# def compute_plane_normal_by_cross_product(points): -# n = points.shape[0] - -# if n < 3: -# raise ValueError("The point cloud must have at least 3 points.") - -# # Initial selection: first, middle, and last points -# p1 = points[0] -# p2 = points[n // 2] -# p3 = points[-1] - -# # Check if the initially selected points are collinear -# if not is_collinear(p1, p2, p3): -# return p1, p2, p3 - -# # If collinear, randomly sample points until a non-collinear triplet is found -# t1 = time.time() -# while True: -# if time.time() - t1 > 30 : -# raise Exception(f"couldnt find non-colinear points in the given plane for calculating the plane normal, been more than 30 secs") -# p1, p2, p3 = points[np.random.choice(n, 3, replace=False)] -# if not is_collinear(p1, p2, p3): -# # Compute vectors v1 and v2 -# v1 = p2 - p1 -# v2 = p3 - p1 - -# # Compute the cross product -# normal = np.cross(v1, v2) - -# # Normalize the normal vector -# normal_normalized = normal / np.linalg.norm(normal) - -# return normal_normalized, np.mean(points, axis=0) + + return ( + argmax, + planes_points[argmax][indices_of_min_dist[argmax]], + sp.SO3(body_T_intel.rotationMatrix()) * all_plane_normals, + ) def compute_plane_normal_by_average(pcd): - #pcd = NumpyToPCD(points) + # pcd = NumpyToPCD(points) pcd.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) ) normals = np.array(pcd.normals) return normals.mean(axis=0), pcd.get_center().reshape(-1) -# def compute_plane_normal(points): -# """ -# Compute the normal of a plane given a point cloud on the plane. - -# Parameters: -# points (numpy.ndarray): An array of shape (m, 3) representing points on the plane. - -# Returns: -# numpy.ndarray: The normal vector of the plane. -# """ -# # Step 1: Center the points (subtract the mean) -# centroid = np.mean(points, axis=0) -# centered_points = points - centroid - -# # Step 2: Compute the covariance matrix -# cov_matrix = np.cov(centered_points, rowvar=False) - -# # Step 3: Perform eigenvalue decomposition -# eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix) - -# # The normal vector is the eigenvector corresponding to the smallest eigenvalue -# normal_vector = eigenvectors[:, np.argmin(eigenvalues)] - -# return normal_vector, centroid - -def visualize_all_planes(planes, image_rgb_orig, camera_intrinsics, all_planes_normals=None, normal_in_body=None): - #project all 3D plane points back to image_rgb - #fx, fy, cx, cy = camera_intrinsics + +def compute_plane_normal(pcd): + """ + Compute the normal of a plane given a point cloud on the plane. + + Parameters: + points (numpy.ndarray): An array of shape (m, 3) representing points on the plane. + + Returns: + numpy.ndarray: The normal vector of the plane. + """ + points = np.array(pcd.points) + # Step 1: Center the points (subtract the mean) + centroid = np.mean(points, axis=0) + centered_points = points - centroid + + # Step 2: Compute the covariance matrix + cov_matrix = np.cov(centered_points, rowvar=False) + + # Step 3: Perform eigenvalue decomposition + eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix) + + # The normal vector is the eigenvector corresponding to the smallest eigenvalue + normal_vector = eigenvectors[:, np.argmin(eigenvalues)] + + return -1 * normal_vector, centroid + + +def visualize_all_planes( + planes, + image_rgb_orig, + camera_intrinsics, + all_planes_normals=None, + normal_in_body=None, +): + # project all 3D plane points back to image_rgb + # fx, fy, cx, cy = camera_intrinsics np.random.seed(42) # Optional: set a seed for reproducibility image_rgb = image_rgb_orig.copy() for p_i, plane in enumerate(planes): if all_planes_normals is not None: - plane_normal = all_planes_normals[p_i] - plane_origin = plane.get_center().reshape(-1) #np.mean(plane, axis=0) + plane_normal = all_planes_normals[p_i] + plane_origin = plane.get_center().reshape(-1) # np.mean(plane, axis=0) else: plane_normal, plane_origin = compute_plane_normal_by_average(plane) - plane_extend_origin = plane_origin + 0.1*plane_normal - plane_normals_2d = project_3d_to_pixel_uv(np.vstack((plane_origin, plane_extend_origin)), camera_intrinsics) + plane_extend_origin = plane_origin + 0.1 * plane_normal + plane_normals_2d = project_3d_to_pixel_uv( + np.vstack((plane_origin, plane_extend_origin)), camera_intrinsics + ) plane_normals_2d = plane_normals_2d.astype(int).tolist() - image_rgb = cv2.arrowedLine(image_rgb, plane_normals_2d[0], plane_normals_2d[1], (0, 0, 255), 2, tipLength=0.2) + image_rgb = cv2.arrowedLine( + image_rgb, + plane_normals_2d[0], + plane_normals_2d[1], + (0, 0, 255), + 2, + tipLength=0.2, + ) print(f"Plane id {p_i} plane normal {plane_normal}") plane_pts_2d = project_3d_to_pixel_uv(np.array(plane.points), camera_intrinsics) color = tuple(np.random.randint(0, 256, size=3).astype(int).tolist()) @@ -434,13 +415,22 @@ def visualize_all_planes(planes, image_rgb_orig, camera_intrinsics, all_planes_n x, y = list(map(int, point)) image_rgb = cv2.circle(image_rgb, (x, y), 1, color, 1) if len(planes) == 1: - cv2.putText(image_rgb, f"N:{np.round(normal_in_body, 2)}", (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1) - + cv2.putText( + image_rgb, + f"N:{np.round(normal_in_body, 2)}", + (10, 50), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (0, 0, 255), + 1, + ) + cv2.namedWindow("plane detections", cv2.WINDOW_NORMAL) cv2.imshow("plane detections", image_rgb) cv2.waitKey(0) cv2.destroyAllWindows() + def detect_place_point_by_pcd_method( spot, GAZE_ARM_JOINT_ANGLES, @@ -482,95 +472,81 @@ def detect_place_point_by_pcd_method( cy = camera_intrinsics_intel.principal_point.y # u,v in pixel -> depth at u,v, intriniscs -> xyz in 3D pcd = generate_point_cloud(img, depth_raw, mask, fx, fy, cx, cy) - #pcd = pcd.uniform_down_sample(every_k_points=2) - # pcd.estimate_normals( - # search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) - # ) - # # 0.25 - 0 < angle < 75 - # pcd = filter_pointcloud_by_normals_in_the_given_direction( - # pcd, - # np.array([0.0, 0.0, 1.0]), - # 0.25, - # body_T_hand, - # gripper_T_intel, - # visualize=visualize, - # ) + # pcd = pcd.uniform_down_sample(every_k_points=2) + pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30) + ) + # 0.25 - 0 < angle < 75 + pcd = filter_pointcloud_by_normals_in_the_given_direction( + pcd, + np.array([0.0, 0.0, 1.0]), + 0.86, + body_T_hand, + gripper_T_intel, + visualize=visualize, + ) + max_number_of_points = len(pcd.points) # Down-sample by using voxel # pcd = pcd.voxel_down_sample(voxel_size=0.01) # print(f"After Downsampling {np.array(pcd.points).shape}") - all_planes = plane_detect(pcd) - - all_plane_normals = np.array([compute_plane_normal_by_average(plane)[0] for plane in all_planes], dtype=np.float32) - - plane_index, normals_in_body = filter_planes_by_normals_in_the_given_direction(all_planes, all_plane_normals.copy(), np.array([0.0, 0.0, 1.0]), body_T_hand, gripper_T_intel) - - plane_pcd = all_planes[plane_index] - - if visualize: - #for plane_index in range(len(all_planes)): - #visualize_all_planes(all_planes, img, camera_intrinsics_intel, all_plane_normals) - visualize_all_planes([all_planes[plane_index]], img, camera_intrinsics_intel, [all_plane_normals[plane_index]], normals_in_body[plane_index]) - - # Down-sample - plane_pcd.points = o3d.utility.Vector3dVector( - farthest_point_sampling(np.array(plane_pcd.points), 1024) + all_planes = plane_detect(pcd, visualize=visualize) # returns list of open3d pcd + + all_plane_normals = np.array( + [compute_plane_normal(plane)[0] for plane in all_planes], dtype=np.float32 ) - color = np.zeros(np.array(plane_pcd.points).shape) - color[:, 0] = 1 - color[:, 1] = 0 - color[:, 2] = 0 - plane_pcd.colors = o3d.utility.Vector3dVector(color) - target_points, selected_point = get_target_points_by_heuristic( - np.array(plane_pcd.points) + + plane_index, selected_point, normals_in_body = rank_planes( + all_planes, + all_plane_normals, + body_T_hand, + gripper_T_intel, + max_number_of_points, ) - corners_xys = project_3d_to_pixel_uv(target_points, camera_intrinsics_intel) - y_threshold = np.percentile(corners_xys[:, -1], percentile) + plane_pcd = all_planes[plane_index] - indices_gtr_thn_y_thresh = corners_xys[:, 1] >= y_threshold - corners_gtr_than_y_thresh = corners_xys[indices_gtr_thn_y_thresh] - indices = np.argsort(corners_gtr_than_y_thresh[:, 1]) - sorted_corners_gtr_than_y_thresh = corners_gtr_than_y_thresh[indices] - selected_xy = sorted_corners_gtr_than_y_thresh[0] - corners_xys = corners_gtr_than_y_thresh[indices] - print(f"Selected XY {selected_xy}") - depth_at_selected_xy = ( - sample_patch_around_point(int(selected_xy[0]), int(selected_xy[1]), depth_raw) - / 1000.0 - ) - print(f"Depth in Intel {depth_at_selected_xy}") - assert ( - depth_at_selected_xy != 0.0 - ), f"Non zero depth required found {depth_at_selected_xy}" - selected_point = get_3d_point( - camera_intrinsics_intel, selected_xy, depth_at_selected_xy - ) + if visualize: + # for plane_index in range(len(all_planes)): + # visualize_all_planes(all_planes, img, camera_intrinsics_intel, all_plane_normals) + visualize_all_planes( + [all_planes[plane_index]], + img, + camera_intrinsics_intel, + [all_plane_normals[plane_index]], + normals_in_body[plane_index], + ) - selected_point_in_gripper = np.array( - gripper_T_intel.transform_point(mn.Vector3(*selected_point)) + corners_xys = project_3d_to_pixel_uv( + np.array(plane_pcd.points), camera_intrinsics_intel ) + selected_xy = project_3d_to_pixel_uv( + selected_point.reshape(1, 3), camera_intrinsics_intel + )[0] + + selected_point_in_gripper = np.array(gripper_T_intel * selected_point) print(f"Intel point {selected_point}, Gripper Point {selected_point_in_gripper}") img_with_bbox = None - if visualize: - img_with_bbox = img.copy() - for xy in corners_xys: - img_with_bbox = cv2.circle( - img_with_bbox, (int(xy[0]), int(xy[1])), 1, (255, 0, 0) - ) + + img_with_bbox = img.copy() + for xy in corners_xys: img_with_bbox = cv2.circle( - img_with_bbox, (int(selected_xy[0]), int(selected_xy[1])), 2, (0, 0, 255) + img_with_bbox, (int(xy[0]), int(xy[1])), 1, (255, 0, 0) ) + img_with_bbox = cv2.circle( + img_with_bbox, (int(selected_xy[0]), int(selected_xy[1])), 2, (0, 0, 255) + ) + if visualize: # For debug cv2.namedWindow("table_detection", cv2.WINDOW_NORMAL) cv2.imshow("table_detection", img_with_bbox) cv2.waitKey(0) cv2.destroyAllWindows() - cv2.imwrite("table_detection.png", img_with_bbox) + cv2.imwrite("table_detection.png", img_with_bbox) - point_in_body = body_T_hand.transform_point(mn.Vector3(*selected_point_in_gripper)) + point_in_body = body_T_hand * selected_point_in_gripper placexyz = np.array(point_in_body) # This is useful if we want the place target to be in global frame: # convert_point_in_body_to_place_waypoint(point_in_body, spot) From e8a1ad901b8c481e00eb3539112a3bc10065fbc2 Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Fri, 6 Sep 2024 18:16:11 -0700 Subject: [PATCH 3/3] testing in apt --- .gitignore | 3 +- .../spot_rl/envs/skill_manager.py | 6 +-- .../spot_rl/utils/plane_detection.py | 8 ++-- .../spot_rl/utils/search_table_location.py | 43 +++++++++++++------ 4 files changed, 39 insertions(+), 21 deletions(-) diff --git a/.gitignore b/.gitignore index c53cd5c9..6e52d85c 100644 --- a/.gitignore +++ b/.gitignore @@ -28,4 +28,5 @@ ros_tcp/ros_communication_client.egg-info/** *.npy *.pkl* *.json -*.csv \ No newline at end of file +*.csv +*.pt \ No newline at end of file diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 46c803af..95d7adc4 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -509,7 +509,7 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray conditional_print(message=message, verbose=self.verbose) is_local = True # estimate waypoint - #try: + # try: ( place_target_location, place_target_in_gripper_camera, @@ -517,7 +517,7 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray ) = detect_place_point_by_pcd_method( self.spot, self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, - percentile=0 if visualize else 70, + percentile=0 if visualize else 30, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, ) @@ -530,7 +530,7 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray # message = f"Failed to estimate place way point due to {str(e)}" # conditional_print(message=message, verbose=self.verbose) # print(message) - #return False, message + # return False, message place_x, place_y, place_z = place_target_location.astype(np.float64).tolist() status, message = self.place( diff --git a/spot_rl_experiments/spot_rl/utils/plane_detection.py b/spot_rl_experiments/spot_rl/utils/plane_detection.py index 76228b4f..18737e71 100644 --- a/spot_rl_experiments/spot_rl/utils/plane_detection.py +++ b/spot_rl_experiments/spot_rl/utils/plane_detection.py @@ -200,8 +200,8 @@ def DetectMultiPlanes(points, min_ratio=0.05, threshold=0.01, iterations=1000): def plane_detect(pcd, visualize=False): - if visualize: - o3d.visualization.draw_geometries([pcd]) + # if visualize: + # o3d.visualization.draw_geometries([pcd]) points = PCDToNumpy(pcd) points = RemoveNoiseStatistical(points, nb_neighbors=50, std_ratio=0.5) @@ -233,7 +233,7 @@ def plane_detect(pcd, visualize=False): plane.colors = o3d.utility.Vector3dVector(color) planes.append(plane) - if visualize: - o3d.visualization.draw_geometries(planes) + # if visualize: + # o3d.visualization.draw_geometries(planes) return planes diff --git a/spot_rl_experiments/spot_rl/utils/search_table_location.py b/spot_rl_experiments/spot_rl/utils/search_table_location.py index 354ceeb1..96bb9c83 100644 --- a/spot_rl_experiments/spot_rl/utils/search_table_location.py +++ b/spot_rl_experiments/spot_rl/utils/search_table_location.py @@ -305,6 +305,7 @@ def rank_planes( body_T_hand: sp.SE3 = sp.SE3(np.eye(4)), gripper_T_intel: sp.SE3 = sp.SE3(np.eye(4)), max_number_of_points: float = 1e8, + percentile_thresh=30, ): """Filter point clouds based on the normal""" # breakpoint() @@ -313,15 +314,16 @@ def rank_planes( # Compute the dot product to get the cosines # Calculate the angle using the dot product formula euclidean_dist, number_of_pts = [], [] - indices_of_min_dist = [] + # indices_of_min_dist = [] + all_distances = [] planes_points = [np.array(plane.points).reshape(-1, 3) for plane in planes] for plane_points in planes_points: plane_points_in_body = body_T_intel * plane_points norms_of_plane_points = np.linalg.norm(plane_points_in_body, axis=1) argmin_dist = int(np.argmin(norms_of_plane_points)) - - indices_of_min_dist.append(argmin_dist) + all_distances.append(np.linalg.norm(plane_points_in_body[:, :3], axis=1)) + # indices_of_min_dist.append(argmin_dist) euclidean_dist.append(norms_of_plane_points[argmin_dist] / 0.5) number_of_pts.append(plane_points.shape[0] / max_number_of_points) @@ -334,10 +336,24 @@ def rank_planes( # Filter out the point clouds # breakpoint() argmax = int(np.argmax(cost)) - + distances_of_points_for_selected_plane = all_distances[argmax] + sorted_distances_of_points = np.sort(distances_of_points_for_selected_plane.copy()) + selected_distance_based_on_percentile = np.percentile( + sorted_distances_of_points, percentile_thresh + ) + index_in_og_plane = int( + np.argmin( + np.abs( + distances_of_points_for_selected_plane + - selected_distance_based_on_percentile + ) + ) + ) return ( argmax, - planes_points[argmax][indices_of_min_dist[argmax]], + planes_points[argmax][ + index_in_og_plane + ], # if not visualize else planes_points[argmax][indices_of_max_dist[argmax]], sp.SO3(body_T_intel.rotationMatrix()) * all_plane_normals, ) @@ -434,7 +450,7 @@ def visualize_all_planes( def detect_place_point_by_pcd_method( spot, GAZE_ARM_JOINT_ANGLES, - percentile: float = 70, + percentile: float = 30, visualize=True, height_adjustment_offset: float = 0.10, ): @@ -480,7 +496,7 @@ def detect_place_point_by_pcd_method( pcd = filter_pointcloud_by_normals_in_the_given_direction( pcd, np.array([0.0, 0.0, 1.0]), - 0.86, + float(np.cos(np.deg2rad(45))), body_T_hand, gripper_T_intel, visualize=visualize, @@ -503,6 +519,7 @@ def detect_place_point_by_pcd_method( body_T_hand, gripper_T_intel, max_number_of_points, + percentile, ) plane_pcd = all_planes[plane_index] @@ -538,12 +555,12 @@ def detect_place_point_by_pcd_method( img_with_bbox = cv2.circle( img_with_bbox, (int(selected_xy[0]), int(selected_xy[1])), 2, (0, 0, 255) ) - if visualize: - # For debug - cv2.namedWindow("table_detection", cv2.WINDOW_NORMAL) - cv2.imshow("table_detection", img_with_bbox) - cv2.waitKey(0) - cv2.destroyAllWindows() + # if visualize: + # # For debug + # cv2.namedWindow("table_detection", cv2.WINDOW_NORMAL) + # cv2.imshow("table_detection", img_with_bbox) + # cv2.waitKey(0) + # cv2.destroyAllWindows() cv2.imwrite("table_detection.png", img_with_bbox) point_in_body = body_T_hand * selected_point_in_gripper