Simple Grasping

CSCI 4302 Lab 2 - Point Clouds and Can Detection

Overview

Cans ride along a conveyor belt toward a UR5e arm. The arm waits with its open Robotiq gripper hanging over the belt; a depth sensor (range finder) in its palm looks down at the cans coming. Your job is the perception: turn the depth image into a 3D point cloud, remove the conveyor surface, and find the cans as clusters of points with the size of a can. The provided controller uses your detections to line the gripper up with the next can, closes the gripper when the palm distance sensor sees the can arrive, and drops the can into the crate behind the robot.

The lesson covers the pinhole camera model, point clouds, plane segmentation with RANSAC, down-sampling and outlier removal, clustering with DBSCAN, and bounding boxes, with Open3D. The motion primitives (MoveJ, MoveL) are the ones from Lab 1, provided in motion.py.

Theoretical Background

From a Depth Image to a Point Cloud

A depth image stores, for each pixel \((u, v)\), the distance \(Z\) along the sensor's optical axis. The pinhole model relates a 3D point \((X, Y, Z)\) in the sensor frame to its pixel:

so each valid pixel gives back a 3D point:

The intrinsics are the focal lengths \(f_x, f_y\) (in pixels) and the principal point \((c_x, c_y)\). For a sensor \(W \times H\) pixels wide with horizontal and vertical fields of view \(\alpha_h\) and \(\alpha_v\):

Webots gives only \(\alpha_h\); for square pixels \(\tan(\alpha_v / 2) = \tan(\alpha_h / 2)\, H / W\). Using only every \(s\)-th pixel (a stride \(s\)) makes the image \(s\) times smaller in each direction, and \(f_x, f_y, c_x, c_y\) shrink by the same factor. Pixels with no return or out of range are skipped. The cloud is finally flipped (PCD_VISUAL_TRANSFORM) so that it is easier to look at.

Plane Removal with RANSAC

The conveyor belt is the largest plane in view. RANSAC finds it robustly despite the cans on it:

1. pick 3 random points and compute the plane \(ax + by + cz + d = 0\) through them,
2. count the inliers, the points closer to the plane than a threshold,
3. repeat many times and keep the plane with the most inliers.

The plane is accepted as the conveyor only if it is large and thin (is_large_thin_plane); its inliers are then removed. The belt does not move relative to the waiting gripper, so the plane found once is reused: later frames just drop the points close to it, which is much cheaper than running RANSAC again.

Down-Sampling and Outlier Removal

Voxel down-sampling replaces all the points inside each small cube (voxel) by their centroid: the cloud keeps its shape with far fewer points. Statistical outlier removal computes, for each point, the mean distance to its \(k\) nearest neighbours and drops the points whose mean distance is more than a few standard deviations above the average: isolated noise points.

Clustering with DBSCAN

DBSCAN groups points that are density-connected: a point with at least min_points neighbours within a radius \(\varepsilon\) is a core point, and clusters grow from core points through their neighbours. Points that belong to no cluster are noise (label \(-1\)). With \(\varepsilon\) a little larger than the point spacing, each can becomes one cluster, and cans further apart than \(\varepsilon\) stay separate.

Bounding Boxes as a Shape Check

An axis-aligned bounding box around each cluster gives its extent. A soda can is about 6 cm wide and 12 cm tall, so a cluster is accepted only if its extents fall in the ranges matches_can_geometry allows for a can: two similar small sides and a longer one, with plausible sizes. The box center is the can's position in the sensor frame; its \(x\) coordinate is how far the gripper must move sideways to line up with the can.

From Detections to a Grasp (provided)

By the time the arm reacts, a detection is already stale: the belt moves at 0.2 m/s. tracking.py estimates the can's velocity from successive box centers and predicts its position a moment ahead; the gripper moves sideways (MoveL) to that predicted position. The palm distance sensor reads 1000 when nothing is within 10 cm and less as a can comes closer; the gripper closes when the reading drops below a threshold, or is about to (from its trend).

Task Description

Objective

Implement in perception.py:

1. make_intrinsic_from_robot(robot, pixel_stride): the range finder's intrinsics as an Open3D PinholeCameraIntrinsic, for the image sampled with the given stride.
2. depth_to_point_cloud(robot, depth): the point cloud of the depth image (stride POINT_CLOUD_PIXEL_STRIDE, depths between DEPTH_MIN_METERS and DEPTH_MAX_METERS).
3. find_conveyor_plane(pcd): the conveyor plane with RANSAC and its inliers, or None.
4. preprocess_object_points(pcd): voxel down-sampling and statistical outlier removal.
5. cluster_object_candidates(pcd): one point cloud per DBSCAN cluster.
6. The end of detect_can_candidates(pcd): a bounding box per cluster, keeping the clusters that look like cans.

The rest is provided: remove_conveyor_plane (with the plane cache), lab2_grasping.py (the main loop and the grasp cycle), motion.py (Lab 1's motion primitives), tracking.py (can velocity and grasp timing), robot.py (the devices) and point_cloud_view.py (the visualization).

System Configuration

• Robot: Universal Robots UR5e with a Robotiq 3F gripper; base at \((0, -0.1, 0.61)\) m
• Sensors in the palm: a range finder (320 x 240 pixels, get_depth_image() in meters, range 0.02 to 3.5 m, noise 1 cm), an RGB camera (get_image(), for looking only: detection must use depth), and a distance sensor (get_distance_sensor_value())
• Actuators: six joint motors in position control and the three gripper fingers
• Environment: a conveyor belt moving the cans at 0.2 m/s past the robot; a crate behind the robot for the cans
• Control period: 32 ms; perception runs every PERCEPTION_INTERVAL_STEPS control steps
• Visualization: a Matplotlib window shows the depth image and the detected can points with their boxes (DEBUG_POINT_CLOUD_VISUALIZATION in perception.py turns it off)

Performance Metrics

A supervisor counts the cans that really lie in the crate. Cans that fall off the end of the belt or onto the floor go back to the start of the belt. The run ends when 3 cans are in the crate, when no can has been picked up for 30 s, or after 90 s. It writes:

Suggested Algorithm

One perception cycle:

1. Read the depth image and build the point cloud with the strided intrinsics.
2. Remove the conveyor plane (RANSAC the first time, then the cached plane).
3. Voxel down-sample and remove outliers.
4. Cluster with DBSCAN.
5. Fit a bounding box to each cluster and keep the can-sized ones.
6. Report the can boxes; the tracker picks the next can and predicts where it will be.

Diagnostic prints that help:

depth = robot.get_depth_image()
print("depth shape:", depth.shape)
print("depth range:", np.nanmin(depth), np.nanmax(depth))

pcd = depth_to_point_cloud(robot, depth)
filtered_pcd, can_boxes = detect_can_candidates(pcd)
print("points:", len(pcd.points), "can points:", len(filtered_pcd.points), "candidates:", len(can_boxes))
for box in can_boxes:
    print("center:", np.round(box.get_center(), 3), "extent:", np.round(box.get_extent(), 3))

When detection fails, check the pipeline in this order: finite depths in the expected range; a plausible number of points; most of the belt gone after plane removal while the cans remain; compact clusters, one per can; box extents close to a can's.

Validation Checklist

• The depth image has finite values; the cloud has thousands of points before filtering.
• After plane removal, most belt points are gone and the cans remain.
• Each can gives one cluster rather than many fragments.
• The boxes in the visualization sit on the cans and move with them.
• The gripper moves sideways to meet each can before it arrives, and closes around it.
• Cans end up in the crate.

References

• Open3D: point clouds (voxel down-sampling, plane segmentation, DBSCAN, bounding boxes) and RGBD images
• Pinhole camera model (Wikipedia)
• RANSAC (Wikipedia) and DBSCAN (Wikipedia)
• CSCI 4302 Lab 2: point clouds

Extension Ideas

• Fit a cylinder to each cluster instead of a box, and use its axis for the grasp.
• Measure how the detection rate changes with POINT_CLOUD_PIXEL_STRIDE, VOXEL_SIZE and CLUSTER_EPS, and how long one perception cycle takes.
• Raise the belt speed and tune tracking.py until the arm still catches every can.