Complete pick and place program

Overview

This article presents a robot-independent pick and place logic that serves as a great starting point to build real applications on. It consists of a more feature-rich version of the simple pick and place logic, which presents the basic ideas behind pick and place. It is highly recommended to read that article first. A complete description of the Pickit interface can be found here.

All the logic presented here is written in robot-independent pseudo-code using a syntax similar to that of Python. The program can be downloaded here and, similar to the simple pick and place program, it consists of two main parts:

• Generic pick and place function, which typically remains constant across applications.

• Application-specific hooks that depend on things like the robot, the gripper, the part to pick and the cell layout.

Note

The code samples presented in this article are pseudo-code, and are not meant to be executed.

The minimum inputs required to run a pick and place program are the same as for the simple pick and place logic.

Generic pick and place function

def pick_and_place(setup, product, target_picks=-1, retries=5):
    if not pickit_is_running():
        print("Pickit is not in robot mode. Please enable it in the web interface.")
        halt()

    before_start()
    pickit_configure(setup, product)

    goto_detection()
    pickit_find_objects_with_retries(retries)
    pickit_get_result()

    picks = 0
    while True:
        if not pickit_object_found():
            # There are no pickable objects, bail out.
            break

        # Compute bin_entry, pre-pick, post-pick and bin_exit points.
        compute_extraction_path()

        if pickit_is_reachable(PickitPick, BinEntry, PrePick, PostPick, BinExit):
            # Object is pickable! Attempt pick.
            pick_success = pick()
            if pick_success:
                picks += 1
                done_picking = target_picks > 0 and picks == target_picks
                if done_picking:
                    # Target picks reached. Place without further detections.
                    place()
                    break
                else:
                    # Target picks not reached. Place and detect next object.
                    goto_detection()
                    pickit_find_objects_with_retries(retries)
                    place()  # In parallel to detection, saves cycle time.
                    pickit_get_result()
            else:
                # Picking failed, skip place and detect next object.
                on_pick_failure()
                goto_detection()
                pickit_find_objects_with_retries(retries)
                pickit_get_result()
        else:
            # Object is unreachable, get the next detection, if any.
            pickit_next_object()
            pickit_get_result()

    after_end()

    return picks

Click below to expand a flowchart of the implemented logic.

Flowchart

The lines that differ with respect to the simple pick and place logic are highlighted above, and implement the following additional features:

• The ability to not only pick all objects (the default), but also to specify a target number of picks.

• Some robot tools have the means to measure pick success. When this is available, cycle time can be optimized by skipping the place motion on pick failure, and instead proceed to trigger a new object detection.

• Wrap the logic in a pick_and_place function, so it can be reused more easily and with less code duplication. It outputs the number of successful picks, and takes as inputs:

  • Required: the Pickit configuration, setup and product.

  • Optional: target_picks, which defaults to -1 (pick all); and retries, which denotes how many times to retry object detection when the Region of Interest (ROI) is not empty, but no objects are detected.

Application-specific hooks

The pick_and_place function requires the following application-specific hooks to be defined. Click on the entries below to expand them and learn more about their default implementation and behavior:

before_start

# Action performed once before starting pick and place.
def before_start():
    gripper_release()

This hook is executed once before starting to pick and place. It’s recommended to add logic required to bring the robot to a sane configuration before starting to pick objects. This can be especially useful when the robot program was previously interrupted at mid-run.

By default, it makes sure the gripper is open to prepare it for picking an object.

goto_detection

# Move the robot to the point from which object detection is triggered.
def goto_detection():
    movej(Detect)

This is a motion sequence that moves the robot to the point from which object detection is triggered. For simple applications, this typically corresponds to a single waypoint.

Some applications using a robot-mounted camera might require a non-constant Detect point. For instance, when a bin is wider than the camera field of view, multiple detection points are required to fully cover it.

compute_extraction_path

When picking from shallow bins, or in non-bin picking applications, it’s typically sufficient to use a three-point extraction path PrePick → PickitPick → PostPick.

# Compute pre-pick and post-pick points.
def compute_extraction_path(PickitPick, pre_pick_offset=100, post_pick_offset=100):
    PrePick = PickitPick * Pose(0, 0, -pre_pick_offset, 0, 0, 0)

    PostPick = PickitPick
    PostPick.z = PostPick.z + post_pick_offset

More precisely the PrePick is chosen such that the approach motion is aligned with the object, while PostPick is chosen for a straight-up retreat (which makes it less likely to collide with obstacles like the bin).

When picking from deep bins, a five-point extraction path is preferred, which is similar to the above, but with the addition of BinEntry at the beginning of the path and BinExit at the end. These points are vertical translations of PrePick and PostPick, respectively, as shown below.

# Compute BinEntry, PrePick, PostPick and BinExit points.
def compute_extraction_path(PickitPick, bin_entry_z=300, pre_pick_offset=100,
                            post_pick_offset=100, bin_exit_z=300):
    PrePick = PickitPick * Pose(0, 0, -pre_pick_offset, 0, 0, 0)

    PostPick = PickitPick * Pose(0, 0, -post_pick_offset, 0, 0, 0)

    BinEntry = PrePick
    BinEntry.z = bin_entry_z

    BinExit = PostPick
    BinExit.z = bin_exit_z

The first path segment performs bin entry in a safe way: It brings the tool to BinEntry and descends vertically to PrePick. The same concept is applied to the last path segment (PostPick to BinExit), which exits the bin. The parameters bin_entry_z and bin_exit_z are user-defined, and should always be larger than the bin height.

The user can decide which path to use, and then modify the other parts of the program accordingly. In particular, it will affect the argument of the function pickit_is_reachable() and the move commands inside pick().

pick

# Sequence for performing the picking motion:
# - Starts and ends at AbovePickArea, a point reachable in a collision-free way.
# - BinEntry --> PrePick: Linear move along the Z direction to enter in the bin.
# - PrePick --> PickitPick: Linear approach to the pick point.
# - A grasping action.
# - PickitPick --> PostPick: Linear retreat away from the pick point.
# - PostPick --> BinExit: Linear move along the Z direction to exit the bin.
def pick():
    movej(AbovePickArea)
    movel(BinEntry)
    movel(PrePick)
    movel(PickitPick)
    gripper_grasp()  # For a suction-like gripper, do this one line above.
    movel(PostPick)
    movel(BinExit)
    movel(AbovePickArea)

    return gripper_pick_success()

Note

movej(p) and movel(p) represent a robot motion to reach waypoint p following a path interpolated linearly in joint or Cartesian space, respectively. Motions between points of the pick sequence should be linear (movel(p)) to guarantee a predictable path. Joint motions (movej(p)) are discouraged during the pick sequence, as the robot may take an unexpected path that causes a collision with the bin (if present) or neighboring parts. They are however recommended for motions in open, unconstrained space, such as in the goto_detection and place sequences.

The pick sequence performs the actual picking motion, which consists of a linear approach to the pick point, a grasping action, and a linear retreat away from it.

• The sequence starts and ends at AbovePickArea, a waypoint known to be reachable without collision both from the pick area and from the other user-defined waypoints.

• The place where gripper_grasp() is called depends on the type of gripper. Fingered grippers perform the grasp action at the pick point, but for suction-like grippers this typically takes place before heading to the pick point.

  

• The pick point, PickitPick, is computed by Pickit.

• Points PrePick and PostPick (plus optionally BinEntry and BinExit) are computed in compute_extraction_path().

• The pick() function returns a boolean indicating whether the pick was successful or not.

  Note

  The check represented by gripper_pick_success() assumes that the gripper has the means to check pick success from sensor input (like vacuum or force). If this is not the case for your gripper, the pick() function can simply return True always, and the pick failure logic will never be triggered.

  
  

on_pick_failure

# Action taken when picking an object failed.
def on_pick_failure():
    gripper_release()

This hook is executed whenever the pick success check fails, (see gripper_check_success() in the pick hook). The default implementation opens the gripper to prepare it for picking the next object.

place

# Sequence for placing the object at the specified dropof location.
def place():
    movej(Dropoff)
    gripper_release()

This sequence places the object at the specified dropoff location. For simple applications the implementation is trivial, as shown above. However, some applications require more advanced place motions.

The robot sometimes needs to know about the way the object was picked, in order to place it appropriately. Refer to the smart placing examples to learn how to do this with minimal programming effort.

It can also be the case that the drop-off point is not constant, as when parts need to be stacked or palletized. Many robot programming languages provide helpers and templates for stacking and palletizing, which can replace the fixed Dropoff point.

after_end

# Action performed once after pick and place has finished.
def after_end():
    if not pickit_object_found():
        if pickit_empty_roi():
            print("The ROI is empty.")
        elif pickit_no_image_captured():
            print("Failed to capture a camera image.")
        else:
            print("The ROI is not empty, but the requested object was not found or is unreachable.")

This hook is executed once after pick and place has finished. The proposed implementation identifies the termination reason and prints an informative statement if there are no more pickable objects. This is very useful to debug your application while you’re setting it up.

When getting your application ready for production, you should handle the cases that make sense to you with appropriate logic. For instance, a continuous-running application might want to request more parts once all pickable objects have been processed.

def after_end():
    # Unrecoverable error. Raise alarm and stop program.
    if pickit_no_image_captured():
        alarm("Failed to capture a camera image.")
        halt()

    if not pickit_empty_roi():
        # Save a snapshot to learn why no objects were detected in a non-empty ROI.
        pickit_save_snapshot()

    # Request more parts to start picking all over again.
    feed_more_parts()

Notice how the application only stops on non-recoverable errors, and triggers saving a snapshot whenever it fails to empty the ROI. Inspecting these snapshots allow to improve the application by answering questions like:

• Are there actually unpicked objects in the ROI, or are there unexpected contents in it?

• If there are objects, are they detected but unpickable by the robot (because they are unreachable or the picking action failed)?

• If there are objects, but they are not detected, can we optimize the detection parameters or camera location to make them detectable?

Example usage

The following is a minimal example of how the pick and place function can be used.

# Application inputs (needs replacing with actual values).
Detect = [x,y,z,rx,ry,rz]
AbovePickArea = [x,y,z,rx,ry,rz]
Dropoff = [x,y,z,rx,ry,rz]

setup = 1
product = 1

def gripper_release():
    # Add custom gripper release logic.

def gripper_grasp():
    # Add custom gripper grasp logic.

def gripper_pick_success():
    # Add custom gripper pick success check. Returns a boolean.
    # If you don't have the means to measure pick success, return always True.

# Pick all objects and write number of successful picks to 'picks'.
picks = pick_and_place(setup, product)

Advanced topics

Robot-mounted camera

If a robot-mounted camera is used, it’s not possible to perform multiple detection retries (including camera captures) in parallel to the place motion sequence, as camera capture can only take place from the Detect point. To correctly handle the robot-mounted camera scenario, replace lines 31-34 with the following:

# Try first a single detection in parallel to place...
goto_detection()
capture_ok = pickit_capture_image()
if capture_ok:
    pickit_process_image()
    place()
    pickit_get_result()
# If not successful, detect with retries. No longer in parallel with motions.
if not capture_ok or not pickit_object_found():
    goto_detection()
    pickit_find_objects_with_retries(retries)
    pickit_get_result()

Notice the use of the functions pickit_capture_image() and pickit_process_image().

A variant of the pick and place function with the above changes, named pick_and_place_robot_mounted, can be downloaded here.

Collision recovery

During the pick sequence, unexpected collisions may occasionally happen, which, if unhandled, can trigger a protective stop.

To prevent robot downtime and the human intervention required to recover from a protective stop, it is recommended to add a collision recovery routine to the robot program, if supported by the robot programming language. An example recovery strategy would be to release the gripper (and picked part, if any), move safely to the Detect point to trigger a new detection, and pick a new part, as shown below:

# Sequence for performing the picking motion
def pick():
    movej(AbovePickArea)
    movel(BinEntry)
    movel(PrePick)
    movel(PickitPick)
    gripper_grasp()  # For a suction-like gripper, do this one line above.
    movel(PostPick)
    movel(BinExit)
    movel(AbovePickArea)

    Error # Catch error in this loop.
        if collision:   # Collision detected by robot, trying to recover.
            # Stop motion and clear path, if needed by the programming language.            
            stop_motion()
            # Raise the gripper.
            movel(BinExit)
            return false;
        
    return gripper_pick_success()

Warning

If another collision is detected while executing the collision recovery sequence, it will not be handled and a protective stop will be raised.

The above sequence does not detect collisions outside the bin. If desired, other collision recovery routines can be added to handle such cases.