A Pick-and-Place State Machine That Survives Failure

Why the demo works and the system does not

Everyone’s first pick-and-place is a list of delay() calls. Move here, close the gripper, move there, open it. It works on video and it works about eighty times in a hundred on the bench, which feels close enough to done.

It is not close to done, because the twenty failures are all silent. The gripper closed on nothing and the arm carried air across the table and opened over the bin. The part was at a slight angle and got knocked over instead of grasped. The servo browned out mid-lift and the controller reset into an unknown pose with the jaws closed.

What turns those from disasters into recoveries is one thing: a verify step between grasping and lifting, and a state machine that has somewhere to go when it fails. That is the whole subject of this page.

The states

A pick-and-place cycle is nine states, and it is worth naming all nine rather than collapsing them.

Two design points are doing most of the work here.

Approach and retreat poses are separate states, above the object. A gripper that dives straight at a part from an arbitrary direction knocks it over, and a gripper that lifts along a curve drags the part sideways before it clears. Always come in from directly above (or directly along the insertion axis) with a short straight segment, and leave the same way.

VERIFY is a state, not a line of code. It is the only place in the cycle where the machine finds out whether reality matched the plan, and it needs somewhere to fail to.

Verifying the grasp

Four ways, cheapest first. You want at least one.

The jaw position check (free)

If your gripper has any series compliance, a grasp on an object stops the jaws short of where they go when closing on air. If you can read the jaw position, comparing it against the empty-close position tells you something is between the fingers.

On a plain hobby servo you cannot read the position — it is open-loop. But a servo with the potentiometer wiper brought out to an ADC pin gives you this for the price of one wire, and that mod takes ten minutes.

Limits: it tells you something is there, not that it is the right thing or that it is held well.

A microswitch on the finger (about 50p)

A lever microswitch on the inside face of one finger, wired to an input pin with a pull-up. Closes when the pad contacts something.

This is the best value option by a wide margin. It is a direct digital answer to “is there something between the jaws”, it survives everything, and it debounces in software in three lines. Put it on the compliant part of the finger so it trips reliably at a repeatable force.

Current sensing (about £2)

A shunt or a hall sensor in the servo supply. A servo closing on air ramps down to idle; a servo closing on an object stays at elevated current.

Limits: noisy, needs a threshold you calibrate, and it goes wrong when the battery sags. Worth it when you also want to detect a crush rather than just a hold — a current far above the expected value means the part is harder than you expected or you missed and grabbed the fixture.

Beam break across the jaws (about £1)

An IR emitter and phototransistor across the jaw opening. Broken beam means something is in the gap.

Limits: it sees the object before you have grasped it, so it verifies presence rather than grip. Pair with something else. Genuinely good on machines that pick transparent or dark objects where vision struggles.

Writing it

The pattern is a plain finite-state machine with a non-blocking timer, so nothing in the loop stalls and the estop stays responsive:

enum State { IDLE, APPROACH, DESCEND, GRASP, VERIFY, LIFT, TRANSFER, PLACE, RECOVER };

State state = IDLE;
uint32_t stateEnteredAt = 0;
uint8_t  attempts = 0;

const uint8_t  MAX_ATTEMPTS = 3;
const uint16_t GRASP_DWELL_MS = 350;   // let the compliance settle before you trust it
const uint16_t SETTLE_MS      = 250;

void enter(State next) { state = next; stateEnteredAt = millis(); }
bool elapsed(uint16_t ms) { return millis() - stateEnteredAt >= ms; }

// Debounced: one reading is a glitch, three in a row is a fact.
bool objectHeld() {
  static uint8_t streak = 0;
  streak = digitalRead(GRIP_SWITCH) == LOW ? min(streak + 1, 3) : 0;
  return streak >= 3;
}

void loop() {
  arm.update();          // non-blocking motion, every pass
  gripper.update();

  switch (state) {
    case IDLE:
      if (jobPending()) { attempts = 0; arm.moveTo(approachPose); enter(APPROACH); }
      break;

    case APPROACH:
      gripper.open();
      if (arm.arrived() && elapsed(SETTLE_MS)) { arm.moveTo(graspPose); enter(DESCEND); }
      break;

    case DESCEND:
      if (arm.arrived()) { gripper.close(); enter(GRASP); }
      break;

    case GRASP:
      // The dwell is not politeness. A compliant finger rings after closing,
      // and reading the switch during the ring gives you a coin flip.
      if (elapsed(GRASP_DWELL_MS)) enter(VERIFY);
      break;

    case VERIFY:
      if (objectHeld()) { arm.moveTo(approachPose); enter(LIFT); }
      else              { enter(RECOVER); }
      break;

    case LIFT:
      if (arm.arrived()) { arm.moveTo(placeApproach); enter(TRANSFER); }
      break;

    case TRANSFER:
      if (arm.arrived()) { arm.moveTo(placePose); enter(PLACE); }
      break;

    case PLACE:
      if (arm.arrived() && elapsed(SETTLE_MS)) {
        gripper.open();
        // Open BEFORE retreating, and retreat straight up, or you drag the part.
        if (elapsed(SETTLE_MS + 200)) { arm.moveTo(placeApproach); enter(IDLE); }
      }
      break;

    case RECOVER:
      gripper.open();
      if (elapsed(SETTLE_MS)) {
        if (++attempts < MAX_ATTEMPTS) { arm.moveTo(approachPose); enter(APPROACH); }
        else { raiseFault("grasp failed after 3 attempts"); enter(IDLE); }
      }
      break;
  }
}

Three details in there earn their place:

The grasp dwell. A compliant finger rings for a couple of hundred milliseconds after closing. Verify during the ring and you get a coin flip. 300–400 ms is usually plenty; measure yours by logging the switch for a second after a close.

Debounced verification. One switch reading is a glitch. Three consecutive readings is a fact, and it costs three passes of a loop that runs at hundreds of hertz.

A bounded retry. Retrying forever is how a robot spends the night grinding at a part that fell on the floor an hour ago. Three attempts and then a fault that a human has to clear.

What to do when the retry does not help

RECOVER as written just tries again from the same pose, which fixes the common case — the part was slightly off, the jaws nudged it, and the second attempt lands. It does not fix a part that is somewhere else entirely. Useful escalations, in order:

1. Nudge the approach. Offset the grasp pose by a couple of millimetres in a small spiral on each retry. Costs three lines and recovers a surprising fraction.
2. Re-sense. If you have a camera or a distance sensor, take a fresh reading rather than trusting the pose you were given.
3. Widen the opening. A jaw opening sized tight to the nominal part fails on a part that is rotated. Open wider on retry.
4. Give up loudly. A fault that stops the machine and says which state it was in beats a machine that keeps going and quietly fills the reject bin.

The failures that are not the gripper’s fault

When a pick-and-place is unreliable, it is often not the grasp:

The fourth row is worth dwelling on, because it is the one that looks most like a mystery. A grasp that holds perfectly on the bench can let go 40 ms into a lift — before the arm has visibly moved — because the acceleration term dominates. The gripper simulator will show you the exact instant for your numbers, and the fix is usually to slow the profile rather than squeeze harder.

Where to go from here

Once the cycle is reliable, the next things worth adding, in order of value:

1. Cycle counting and a fault log. You cannot improve a reliability number you are not measuring.
2. A proper motion profile on the transfer, so the acceleration is bounded and you stop over-sizing the grip to survive jerky moves.
3. Pose from sensing, not from constants, which is the step from a demo to a machine.

Next

• Finite-state machines for robot behavior — the general pattern this is an instance of
• Servo gripper force, stall and compliance — why the dwell exists and why the jaws should not stay closed
• Robot gripper types and grip force — sizing the grasp the sequence depends on
• The gripper simulator — find the instant a grasp lets go on your own numbers