Collision Avoidance

Collision Avoidance: Grasping in Clutter

Overview

A UR5e arm with a Robotiq 3-finger gripper must take cans out of a crate full of objects (cans, a bottle, an apple, a biscuit box) and drop them into a bin behind it, without touching the crate or any object it is not picking. The camera on the wrist recognizes the objects and gives their poses, so finding the cans is easy. Deciding which can the hand can actually reach is not: the open gripper is 26 cm wide, almost half the crate, and most grasps would push a finger into a wall or a neighbour.

The lesson teaches grasp planning with a collision model. The recognized objects are copied into a PyBullet world together with a model of the crate and of the open gripper; every candidate grasp is checked there, along its whole approach, before the real arm moves. The motion layer (forward and inverse kinematics, joint and straight-line moves) is the one built in the Inverse Kinematics lab and is provided.

Theoretical Background

From recognition to world poses

Webots' camera recognition reports, for each recognized object, its model name, its pose in the camera frame and the colour of its pixels in the segmentation image. The pose becomes a world pose through the chain of homogeneous transforms

where \(T^{W}_{T}(q)\) is the tool flange pose given by forward kinematics at the joint angles \(q\), \(T^{T}_{C}\) is the fixed camera mount in the tool frame and \(T^{C}_{O}\) is the recognized object pose. Each \(T\) is a \(4\times4\) matrix \(\begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}\) with a rotation \(R\) and a position \(p\). For a can, the third column of \(R^{W}_{O}\) is the direction \(a\) of the cylinder's axis: \(a_z \approx \pm 1\) for a standing can, \(a_z \approx 0\) for a lying one.

The segmentation image tells how much of each object is visible: its mask area \(A\) is the number of pixels with the object's colour. A can with a small \(A\) is mostly hidden, and its estimated pose is the least trustworthy, so it is not a pick candidate. It still is an obstacle.

Configuration space and collision checking

A grasp is a tool pose. Write it \(g = (p, R)\): the flange position and the tool rotation. The set of poses at which the gripper overlaps something is the obstacle region; the planner must find poses outside it. Rather than describing that region explicitly, which is hard, a collision checker answers point queries: is the gripper at pose \(g\) free? PyBullet computes, for each pair of convex shapes \(A\) and \(B\), the signed distance

and getClosestPoints returns it for every pair of links closer than a query range. The clearance of the gripper at pose \(g\) is the smallest signed distance to any obstacle,

with \(\mathcal{G}(g)\) the gripper's shapes placed at \(g\) and \(\mathcal{O}\) the crate and the recognized objects. A pose is accepted when \(c(g) \ge c_{\min}\). The margin \(c_{\min}\) (1 cm here) absorbs what the model gets wrong: pose errors, simplified shapes, the arm's tracking error.

Checking a motion, not only a pose

The gripper does not appear at the grasp; it travels there. The approach is a vertical straight line from a pregrasp pose \(p_0\) above the crate to the grasp \(p_1\), at a constant rotation \(R\). The swept motion is checked by sampling it:

and the approach's clearance is \(\min_s c\big(p(s), R\big)\). With a step \(\Delta = 1\) cm, an obstacle thinner than the step could slip between two samples; the margin \(c_{\min}\) and the fact that the fingers are several centimetres long make that impossible here. The arm then follows exactly this line (a straight-line move, MoveL), so the checked motion is the executed one.

Top-down grasps on a cylinder

The tool frame has its \(y\) axis along the gripper, from the flange through the fingers, and the fingers close along its \(x\) axis. A top-down grasp has \(y_T = (0, 0, -1)\). Its remaining freedom is the yaw \(\psi\) about the vertical. The fingers must close across the can, so the tool's \(z\) axis runs along the can:

where \(\delta\) is a yaw offset. For a lying can, \(\delta \in \{0, \pi\}\): the fingers close across the can, the middle finger on one side and the two others on the other side, either way round. (Closing along the can, on its flat ends, fits in the open hand but does not hold: the fingertips slip over the rims.) A standing can is symmetric about its axis, so \(\psi = \delta\) and any yaw grasps it; the planner tries four, \(\delta \in \{0, \tfrac{\pi}{4}, \tfrac{\pi}{2}, \tfrac{3\pi}{4}\}\). The grasp height is the depth \(h\) of the flange above the can's centre: the open fingertips reach \(19.5\) cm below the flange, so \(h = 18.5\) cm puts them just below the centre and \(h = 20\) cm level with it, a few millimetres above the crate's floor for a lying can.

The fingers are position-controlled. Commanding them fully closed makes the motors push the fingertips through the can's surface with their full torque, and the contact forces then shoot the can out of the hand; commanding a closure just past the can's diameter (GRIP_CLOSURE) holds it firmly.

Search order

The planner is a generate-and-test search: candidates (can, \(\delta\), \(h\)) are produced in a fixed order and the first that passes both tests is executed. The tests are the collision check and inverse kinematics (the pose must be reachable). In a pile, the highest object is the one with free space above it, so the cans are tried from the highest down. After each pick the scene has changed, so the robot looks again and plans again: a can that was blocked by a neighbour may now be free.

flowchart LR
    A[Look from the home pose] --> B[Recognize objects, world poses]
    B --> C[Copy all objects into PyBullet]
    C --> D[Next candidate: can, yaw, depth]
    D --> E{Approach clearance >= c_min?}
    E -- no --> D
    E -- yes --> F{IK reachable?}
    F -- no --> D
    F -- yes --> G[Descend, close, lift, drop in bin]
    G --> A
    D -- none left --> H[Stop]

Task Description

Objective

Implement, in lab4_collision_avoidance.py:

1. select_cans(observations): keep the recognized cans that are visible enough (MIN_MASK_AREA) and inside the crate, highest first.
2. grasp_rotation(can, yaw_offset): the top-down tool rotation whose fingers close across the can's axis.
3. approach_positions(can, depth): the pregrasp and grasp flange positions of the vertical descent.
4. approach_clearance(world, pregrasp, grasp, rotation): the smallest clearance of the open gripper along the descent, sampled every APPROACH_STEP, and the obstacle that sets it.
5. plan_grasp(can, yaw_offset, depth, world, joints): accept a candidate only when its clearance is at least MIN_CLEARANCE and the grasp pose has an IK solution.

choose_grasp (the search over cans, yaws and depths), execute_grasp and the loop in main are given.

System Configuration

• Robot: Universal Robots UR5e on the table, base at \((-1.53, -1.04, 0.80)\) m, with a Robotiq 3F gripper.
• Sensors: joint position sensors; a 320x240 camera with recognition and segmentation, on the tool flange.
• Actuators: six joint motors (position control), the gripper's three finger motors.
• Provided modules: motion.py (forward and inverse kinematics, MoveJ, MoveL, MoveJointTargets, the home and drop poses), perception.py (recognized objects in the world frame), collision_world.py (the PyBullet world: reset_scene, gripper_clearance), scene_plot.py (the Visualization tab).
• Environment: a crate (0.60 x 0.45 x 0.15 m, floor centre at \((-1.45, -0.54, 0.80)\) m) holding five cans (three standing, two lying), a standing bottle taller than the crate, an apple and a biscuit box; a bin (0.30 x 0.30 m) behind the robot at \((-1.55, -1.45)\) m.

Performance Metrics

The supervisor watches the real world and writes metrics.json:

Touching an object is part of a grasp only if that object is lifted soon after; the run ends at the first collision.

Suggested Algorithm

One pick cycle:

1. Move to the home pose, from which the camera sees the whole crate.
2. Read the recognized objects; select the cans to try.
3. Reset the PyBullet scene with every recognized object (all of them are obstacles, the target too).
4. For each can, yaw offset and depth: compute the rotation and the approach, its clearance, and the IK of the grasp; take the first candidate that passes.
5. Move above the can at the pregrasp, descend in a straight line, close the gripper, lift straight up to CARRY_HEIGHT, high above everything in the crate (the bottle stands taller than the crate's rim).
6. Go back over the home pose, swing to the drop pose above the bin, open the gripper.
7. Repeat until three cans are in the bin or no candidate is feasible.

Validation Checklist

• The console lists the recognized objects and the cans to try, and the positions are inside the crate.
• The highest can is not always the one picked: its candidates are rejected with a clearance below 10 mm and the obstacle that caused it (a neighbour, the crate).
• In the Visualization tab, the rejected candidates are red crosses and the chosen one a green circle.
• The gripper goes down vertically with its fingers open around the can and touches nothing else.
• Each picked can ends in the bin; the run ends with "success": true and "collisions": 0.
• Setting MIN_CLEARANCE to a negative value makes the robot try grasps that hit things, and the supervisor reports them.

References

• N. Correll, B. Hayes, C. Heckman, A. Roncone, Introduction to Autonomous Robots, MIT Press, 2022: chapters on manipulation, grasping and path planning. Course labs: https://introduction-to-autonomous-robots.github.io/
• PyBullet Quickstart Guide (collision queries, getClosestPoints): https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
• Webots camera recognition: https://cyberbotics.com/doc/reference/recognition
• Webots UR5e: https://cyberbotics.com/doc/guide/ure
• S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006, chapter 5 (collision detection, sampling a path): https://lavalle.pl/planning/

Extension Ideas

• Rank the candidates by clearance instead of taking the first feasible one, and compare how often the grasps succeed.
• Check the whole arm, not only the gripper: load a UR5e model into PyBullet and test the joint-space move to the pregrasp as well.
• Allow tilted grasps (the tool's \(y\) axis off the vertical) for cans next to a wall, and add the tilt to the search.