Bin Picking

Overview

A UR5e arm with a Robotiq three-finger gripper stands next to a bin of clutter: ten soda cans, most of them lying, a few on top of others and two standing, an apple, an orange, a biscuit box and a water bottle. The robot must take the cans out one by one and drop them into the flat output tray behind it, until no can is left in the bin, without hitting the bin and without throwing anything on the floor. The other objects stay where they are.

The lesson puts the pieces of the earlier labs together: inverse kinematics and motion primitives (Lab 1), object recognition (Lab 2), oriented grasps (Lab 3) and collision checking with PyBullet (Lab 4). What is new is the glue: a behaviour tree that decides what to do next, retries when a grasp fails and stops when the job is done.

Theoretical Background

One pick, step by step

Every pick is a small sense–plan–act cycle. The camera looks into the bin from a fixed view pose, a planner chooses a can and a gripper pose that reaches it without collisions, and the arm executes the grasp. Then the robot checks whether it really holds the can, because a grasp can always fail: the can rolls, a neighbour is in the way, the fingers close on air.

flowchart LR
    A[View pose:<br/>recognize objects] --> B[Choose a can<br/>and a grasp]
    B --> C[Collision check<br/>in PyBullet]
    C --> D[Pre-grasp, descend,<br/>close, lift]
    D --> E{Can in hand?}
    E -- yes --> F[Drop in the<br/>output tray]
    E -- no --> G[Open, block that can,<br/>try another]
    F --> A
    G --> A

Behaviour trees

A behaviour tree organises the decisions of a robot as a tree of nodes. The robot ticks the root; a ticked node does its work and returns a status: SUCCESS or FAILURE (in general also RUNNING, for actions that take several ticks; here every action blocks until it is done, so it is never needed). The leaves are conditions (questions about the world: "is the bin empty?", "do I hold a can?") and actions ("move to the pre-grasp pose"). The inner nodes, the composites, decide which children run:

With children \(c_1, \dots, c_n\) and their results \(s_i \in \{0, 1\}\) (1 for SUCCESS):

evaluated from left to right with short-circuiting: a sequence never ticks the children after a failure, a selector never ticks the children after a success. That is what makes the order of the children meaningful: in a selector the preferred option comes first and the fallback last; in a sequence the cheap checks come before the expensive actions.

The tree of this lesson:

flowchart TD
    R["? Clear bin"] --> E(["Bin empty?"])
    R --> P["→ Pick and place one can"]
    P --> P1["? Plan grasp"]
    P1 --> P1a["Plan clean grasp"]
    P1 --> P1b["Plan nudging grasp"]
    P --> P2["Open gripper"]
    P --> P3["Move to pre-grasp"]
    P --> P4["Descend to grasp"]
    P --> P5["Close gripper"]
    P --> P6["Lift"]
    P --> S["? Place or recover"]
    S --> Q["→ Place"]
    Q --> Q1(["Holding a can?"])
    Q --> Q2["Place in output tray"]
    S --> F["Recover after miss"]

Plan grasp is a selector too: it prefers a clean grasp and falls back to one whose fingers push a neighbour aside (see Collision checking) only when no clean grasp exists.

One tick of the root either finds the bin empty (SUCCESS, the job is done), or picks one can (SUCCESS), or fails somewhere (FAILURE): no acceptable grasp, or the can was not held, in which case Recover after miss opens the gripper, remembers the can so the next tick tries another one, and fails on purpose so that the main loop can count failures and give up after a few in a row. The main loop simply ticks the root again and again.

Compared with a finite state machine, the same logic needs no explicit transitions: adding a recovery behaviour means adding a child to a selector, not rewiring states. That modularity is why behaviour trees are common in robotics (ROS 2 Navigation, game AI) and why Prof. Correll's course uses them for bin picking.

From the camera to the world

The camera on the tool flange recognizes objects and reports each one's pose relative to the camera, \({}^{C}T_{O}\). The flange pose follows from the joint angles \(q\) by forward kinematics, \({}^{W}T_{F}(q)\), and the camera is mounted at a fixed offset \({}^{F}T_{C}\) (9 cm along the approach axis). Chaining the homogeneous transforms gives the object's pose in the world:

For a can, \(p\) is its centre and the third column of \(R\) is its axis of symmetry \(a = (a_x, a_y, a_z)\). The camera also gives a segmentation image; the number of pixels of an object, its mask area, tells how much of it is visible. A can hidden under others is a poor candidate: its estimated pose is unreliable and the gripper cannot reach it.

A top-down grasp across a lying can

The gripper comes straight down: the tool's \(y\) axis points to \(-z\) of the world. Its fingers close along the tool's \(x\) axis (the middle finger on one side, the finger pair on the other), and that direction is set by the wrist's yaw \(\psi\):

To hold a lying can the fingers must close across it, perpendicular to its axis. With the axis' horizontal angle \(\psi_a = \operatorname{atan2}(a_y, a_x)\):

the second with the middle finger on the other side, which may be the one that fits between the neighbours. A can standing upright (\(\sqrt{a_x^2 + a_y^2}\) small) can be taken at any yaw. The flange goes to

where \(s\) shifts the grasp a little along the can (0, \(\pm 2\) cm) and a second shift \(t \in \{0, \pm 2.5\}\) cm moves it across the can, along \(\hat n_{xy} \perp \hat a_{xy}\) (the open fingers leave about 3 cm on each side of the can), to find a gap between the neighbours and the walls; \(h\) puts the fingertips, which reach about 0.17 m below the flange, around the can's equator: higher and the fingers squeeze the can out upwards, lower and they hit the bin floor. The pre-grasp pose lies 0.12 m straight above; the arm reaches it in joint space and then descends along a straight line (MoveL), so that the fingers come down vertically between the objects.

The fingers are driven to 60 % of their closing range, a little past the can's surface (they touch it at about 45 %): they stop on the can and squeeze it. Closing them fully would drag the can up between the finger bases, where it wedges and does not fall out when the gripper opens again.

Collision checking

Before the arm moves, every grasp candidate is tested in a PyBullet copy of the scene: the bin, one simple shape per recognized object (a cylinder per can, along its axis; a box for anything else) and a model of the open gripper placed at the candidate's flange pose \(T\). PyBullet returns the signed distance between the gripper and each obstacle (negative when they overlap); the smallest is the candidate's clearance:

The two tolerances differ because the obstacles do. The bin is rigid and fixed: the gripper may not touch it (\(\varepsilon_{\text{bin}} = 1\) cm, the margin of the approximate gripper model). The objects are light and loose: a finger that brushes a neighbour pushes it aside, so a small overlap is accepted (\(\varepsilon_{\text{obj}} = 3.5\) cm). When no candidate passes, the fallback planner accepts up to 8 cm of overlap with the neighbours and 3 cm with a wall (a finger then scrapes down along it): in a crowded bin a grasp that shoves its way in is better than no grasp, and every attempt loosens the pile. The target can itself is excluded: the fingers are meant to touch it. The open Robotiq hand is wide (its fingertips span about 22 cm), so a can lying against a wall can only be taken once its neighbours are gone or it has been pushed away from the wall. Among the feasible candidates the planner prefers a large clearance and cans high up in the pile, which are the least buried:

Guarded moves and grasp verification

The camera estimates the can's height to a few millimetres, and a can may have rolled since the picture. A palm distance sensor between the fingers measures the distance \(d\) to the first object below the gripper (0.1 m when there is nothing within range). The descent is guarded: it moves a centimetre at a time and stops early when \(d\) drops below 5 cm, which means something lies higher than the camera thought. After closing and lifting, the same sensor tells whether the grasp worked: a held can sits right under the palm (\(d \approx 3\) to 8 cm), while an empty gripper lifted 0.4 m above the bin sees nothing within range (\(d = 0.1\) m). The robot trusts that measurement, not its plan.

Inverse kinematics (recap)

Every pose is turned into joint angles by the damped-least-squares iteration of Lab 1. With the pose error \(e\) (position and rotation vector) and the \(6 \times 6\) Jacobian \(J(q)\):

with damping \(\lambda = 0.01\) and a step \(\alpha \leq 0.6\) that is halved until no joint centre dips below the keep-out plane 2 cm above the table. A pose out of reach raises ValueError, and the behaviour tree treats it as a failed action.

Task Description

Objective

Implement in lab6_bin_picking.py:

1. Sequence.tick(blackboard): tick the children in order; return FAILURE at the first failure, SUCCESS when all succeed.
2. Selector.tick(blackboard): tick the children in order; return SUCCESS at the first success, FAILURE when all fail.
3. select_can_candidates(observations, blocked_ids): the recognized cans inside the bin, visible by at least MIN_MASK_AREA pixels and not blocked, sorted from the highest to the lowest.
4. grasp_candidates(can): the top-down grasp and pre-grasp poses across one can, for both wrist yaws and the shifts in AXIS_SHIFTS and SIDE_SHIFTS (four yaws and no shift for an upright can).
5. choose_feasible_grasp(cans, collision_world, plot, object_tolerance, bin_tolerance): test every candidate with collision_world.check_grasp(), draw it, and return the feasible candidate with the best score \(S\), or None.
6. object_in_hand(palm_distance): decide from the palm sensor whether the lift brought a can along.

The behaviour tree itself (build_behavior_tree()), its leaves and the main loop are given: read them, they are the other half of the lesson.

System Configuration

• Robot: Universal Robots UR5e (6 revolute joints) on a table at \(z = 0.8\) m, with a Robotiq 3F gripper whose three fingers close together (open_gripper(), close_gripper()).
• Sensors on the flange: a 320 × 240 camera with object recognition and segmentation (0.09 m in front of the flange, looking along the approach axis), a palm distance sensor (0.08 m in front of the flange, range 0.1 m), and the joint position sensors.
• The input bin (DEF BIN, fixed): 0.6 × 0.45 × 0.2 m, centred at \((-1.45, -0.54)\) m, with ten cans (radius 3.2 cm, length 12.2 cm, 0.35 kg) and four other objects that settle into it during the first two seconds.
• The output tray (DEF OUTPUT_BIN, fixed): 0.5 × 0.35 × 0.08 m behind the robot, centred at \((-1.45, -1.50)\) m. It is low because the arm starts stretched out over it, at the height of its rim.
• Provided modules: robot.py (devices), motion.py (kinematics, MoveJ, MoveL), perception.py (recognition in the world frame), collision_world.py (PyBullet), bin_plot.py (the plot in the Visualization tab).

Performance Metrics

A supervisor watches the real objects (not the controller's estimates). After the objects have settled it records the cans lying in the bin; the run ends when none of them is left there, when the gripper has not moved for 20 s (the controller finished or is stuck), or after 600 s of simulated time.

Suggested Algorithm

One tick of the tree:

1. Move to the view pose, recognize the objects, keep those inside the bin, choose the can candidates.
2. If there is no can, the bin is empty: stop.
3. Load the objects into the collision world; build the grasp candidates of every can; keep the feasible ones and take the best. If there is none, try again with the larger overlap of the nudging fallback.
4. Open the gripper, move to the pre-grasp pose (MoveJ), descend straight down (MoveL), guarded by the palm sensor.
5. Close the gripper while holding the arm still, then lift to the carry height.
6. If the palm sensor sees a can, turn the base joint towards the output tray (the hand stays level), lower the can onto the tray floor, open the gripper and pull the hand up; otherwise open, block the can and let the tick fail.

Validation Checklist

• The console shows one tick per pick, the leaves' results, and Plan: can … clearance … cm before each grasp.
• The plot shows the bin from above with the recognized cans; the chosen grasp's fingers land on both sides of a can, across its axis.
• The fingers come down vertically and close around the can's middle; the can comes up with the gripper and stays behind in the tray when the gripper opens.
• A missed can is reported (missed can …) and the next tick tries another one.
• The cans land in the output tray, not on the table.
• The supervisor reports each can as it lands (can… in the output tray), and the run ends with success: true: at least eight of the ten cans in the tray. A can wedged against a wall may stay behind.

References

• N. Correll, B. Hayes, C. Heckman, A. Roncone, Introduction to Autonomous Robots, MIT Press (Open Access): the chapters on manipulation, grasping and task execution.
• M. Colledanchise, P. Ögren, Behavior Trees in Robotics and AI: An Introduction, CRC Press, 2018 (arXiv:1709.00084).
• Webots: UR5e and Robotiq 3F gripper.
• Webots: Camera recognition.
• PyBullet Quickstart Guide (getClosestPoints).

Extension Ideas

• Add a decorator node (for example Retry(n), which re-ticks its child up to \(n\) times) and use it to retry a missed can once before blocking it.
• Check the whole approach, not just the final pose: test a few gripper poses between the pre-grasp and the grasp pose.
• Add a Push away from the wall action as the last fallback: press the closed fingers on a can lying against a wall and roll it towards the middle of the bin, so that the next tick can grasp it.