Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

wip #208

Draft
wants to merge 3 commits into
base: main
Choose a base branch
from
Draft

wip #208

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d4bde9e
wip
tusharsangam Aug 31, 2024
46f40fc
seems to work, plane filtering
tusharsangam Sep 5, 2024
e8a1ad9
testing in apt
tusharsangam Sep 7, 2024
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
4 changes: 4 additions & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,3 +26,7 @@ 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
*.pt
2 changes: 1 addition & 1 deletion spot_rl_experiments/configs/config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
42 changes: 21 additions & 21 deletions spot_rl_experiments/spot_rl/envs/skill_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 30,
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(
Expand Down
71 changes: 54 additions & 17 deletions spot_rl_experiments/spot_rl/utils/plane_detection.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down Expand Up @@ -158,20 +159,63 @@ def DetectMultiPlanes(points, min_ratio=0.05, threshold=0.01, iterations=1000):
return plane_list


def plane_detect(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):
Expand All @@ -185,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 = [planes[high_i]]
colors = [colors[high_i]]
planes = np.concatenate(planes, axis=0)
colors = np.concatenate(colors, axis=0)
return DrawResult(planes, colors)

# if visualize:
# o3d.visualization.draw_geometries(planes)

return planes
Loading