Low-latency robot teleoperation & constrained IK

1Problem

The operator needs the arm to respond right away, but the camera, Wi-Fi, compute and drives each add delay and jitter. I also needed the system to stop safely when data goes stale or the link drops.

2What I built

1. 01

   Control loop

   Camera, marker pose, constrained IK and Franka commands run at 100 Hz. The p99 loop period was 10.1 ms, with no overruns in any logged session.

2. 02

   Network fix

   The Wi-Fi link averaged 43.4 ms ping with 85 ms of jitter. I moved control traffic to USB-C networking, which brought it to 0.35 ms average and 0.21 ms jitter.

3. 03

   Inverse kinematics

   I wrote my own Franka forward kinematics and Jacobian, plus damped least-squares IK with adaptive singularity damping, null-space joint-limit avoidance and velocity limits. It's covered by gtest.

4. 04

   Safety checks

   The supervisor handles e-stop, stale robot state, following error, joint limits and marker loss, and ramps motion down in a controlled way. I tested it by injecting delay, jitter, 5 to 20% packet loss and full link cuts. Recovery is latched, so it needs an explicit reset.

5. 05

   Video and logging

   Operator video uses JPEG, which costs about 1.4 ms to encode and decode. Each session logs camera, joint, command, pose and safety-state data with synchronized timestamps.

3Result

Camera-to-arm latency came in at 65 ms p50 and 117 ms p95. The p99 loop period was 10.1 ms with no logged overruns. After the network change and drive retuning, command-to-motion p50 went from 158 ms to 30 ms.