Franka Haply KOMO Teleop

Teleoperating a Franka Panda arm with a Haply Inverse3, with the IK phrased as a single-step KOMO problem.
Code

This mini project is a small teleop stack for a Franka Panda arm driven by a Haply Inverse3, a 6D space mouse with linear haptic feedback. The operator's hand pose drives the Panda end-effector target, and at every control cycle the corresponding joint configuration is recovered by solving inverse kinematics as a single-step KOMO problem. Only the arm is controlled — the (mobile) base is held static. The same script runs against either the simulator or the real robot via robotic / botop, and an alpha-stage force-feedback path closes the loop from the robot's external wrench back into the haptic device.

Why KOMO for the IK?

At first glance, IK for a 7-DoF Franka is the textbook setting for damped least squares on the manipulator Jacobian. That works, but it tends to ossify into a small pile of hand-rolled heuristics: a nullspace projector for posture biasing, a separate clamping pass for joint limits, an ad-hoc field for self-collision avoidance, a different code path the moment you want to add a position-only or orientation-only objective, and yet another one if you ever want to extend the same controller to a short trajectory horizon.

Phrasing the per-cycle IK as a one-phase, one-step KOMO problem (ry.KOMO(C, 1, 1, 0, ...)) sidesteps essentially all of that. Concretely, in this project the same NLP carries:

  • A pose equality on the end-effector (FS.poseDiff against a target frame) — both position and orientation in one shot, by simply switching the feature symbol. Position-only or orientation-only constraints are a one-line change.

  • Two SOS regularizers on the joint state — one toward q0 (the current configuration, for temporal smoothness) and one toward qHome (a posture bias). They take the role of damping and nullspace projection in classical IK, but live as plain costs in the same objective rather than as separate algorithmic stages.

  • Optional self-collision avoidance as an equality on FS.accumulatedCollisions. There's no need to assemble a custom potential field or repulsive Jacobian; flipping the --avoid-self-collisions flag adds the constraint and the rest of the formulation is unchanged.

  • Joint limits and other inequalities for free. The same NLP framework already understands box constraints and inequality features, so adding e.g. velocity or acceleration limits later is a matter of registering one more objective rather than rewriting the solver.

The other appealing property is generalization. A one-step KOMO is genuinely a degenerate special case of multi-step trajectory optimization. The exact same objective definitions extend to a short receding-horizon controller (e.g. ry.KOMO(C, T, K, ..., ...)) with no change to the feature definitions — useful if the teleop later wants to look ahead a few control ticks for smoother force feedback or anticipatory collision avoidance. Classical Jacobian IK has no such ladder to climb.

In return, KOMO IK is a hair more expensive than a single Jacobian pseudoinverse. In practice this is a non-issue at the 120 Hz sync rate used here: the NLP is warm-started from the current joint state, so it typically converges in a couple of inner iterations and the dispatched BotOp.moveTo(..., overwrite=True) call dominates the cycle.

The rest of the loop

  • A dedicated thread runs an async WebSocket client against the Haply daemon. Cursor position, Verse Grip orientation, buttons, and the most recent commanded force are kept in a lock-guarded HaplyState object so the control loop reads them without blocking.

  • The Haply pose is mapped to a robot-frame pose in haply_to_robot_pose(...): axis sign flips and a quaternion-component reorder align the device's frame with the robot's, and a configurable scale factor blows the small Inverse3 workspace up into the larger Panda one. The neutral pose is initialized from the first received Haply position, so the device's "home" is wherever the operator's hand happens to be when the script connects.

  • If the KOMO solve is feasible, the resulting joints are dispatched via BotOp.moveTo(..., overwrite=True), and gripper open/close commands are issued from the Verse Grip's a/b buttons.

  • Force feedback (real-robot only, alpha) reads the external wrench, clips it, and pushes it back to the Haply as a cursor force command. Because get_tauExternal alone does not give the contact wrench via a least-squares problem involving the transpose of the contact Jacobian — the exact control torques applied by libfranka are not exposed in botop — the wrench is instead surfaced through a botop fork that forwards the sensed external wrench from libfranka.

Operator controls

  • Move the Inverse3 to drive the Panda end-effector target.
  • Press a on the Verse Grip to close the gripper.
  • Press b on the Verse Grip to open the gripper.
  • Stop safely with Ctrl + C.

See the README for the full CLI flag reference (real vs. simulation, workspace scale, sync rate, gripper speed, force-feedback toggle, self-collision avoidance) and for the hardware setup notes (Haply calibration, FCI activation on the Franka control box, IP configuration).