Architecture#
flowchart LR
subgraph PC["PC · your workstation"]
C["FrankaTwinClient<br/>examples/*.py"]
end
subgraph NUC["NUC · PREEMPT_RT kernel"]
D["frankatwin.daemon"]
S[("POSIX shm<br/>/frankatwin_osc")]
O["osc_shm<br/>C++ · 1 kHz task impedance"]
M["move_to<br/>C++ · one-shot position control"]
D --- S
S --- O
D -. "stop / start" .- O
D -. "reset" .- M
end
subgraph ROBOT["Franka FR3"]
R["FCI"]
end
C -- "ZMQ REQ · 5555 · ≤ 50 Hz" --> D
D -- "ZMQ PUB · 5556 · 100 Hz" --> C
O <== "libfranka · 1 kHz" ==> R
M <== "libfranka" ==> R
Control law (osc_shm)#
Per 1 kHz tick, with x, q_cur from model.pose(kEndEffector) and
J = model.zeroJacobian(kEndEffector):
e_p = x_des − x (clipped to ±error_delta_pos if > 0)
e_o = 2 · vec(q_des ⊗ q_cur⁻¹) shortest-path sign, never clipped (pure impedance)
v, ω = J q̇
F = [ Kp_pos e_p − Kd_pos v ; Kp_ori e_o − Kd_ori ω ]
τ = Jᵀ F + C(q, q̇) q̇ Coriolis from libfranka (`--no-coriolis` to drop)
τ = clamp(τ, ±τ_max) τ_max = [87 87 87 87 12 12 12] N·m
τ = slew(τ, τ_prev, 800 N·m/s) per joint, per tick
Kd = 2√Kpwhen the command’skd_*is 0 (the default) — critical damping.No null-space term. Jᵀ lets the redundant DOF settle under gravity and joint damping. This is deliberate: it is the simplest law that is trivially mirrored in simulation, and the sysid absorbs the resulting joint-space behaviour.
No apparent-mass projection (
Λ = (J M⁻¹ Jᵀ)⁻¹). The sim controller offers it ascontrol_mode="osc"for experiments, but the real controller never uses it and neither should the sim when you compare.libfranka adds gravity and friction compensation on top of the commanded τ.
tauin the state frame is the commanded impedance torque (gravity excluded);tau_Jis the measured link-side torque (gravity included) and is what you compare against the actuator limits.
The IsaacLab mirror is isaaclab_sysid/.../franka_sysid/control.py
(compute_task_space_torques, control_mode="task_impedance"). If you touch one,
touch the other.
Daemon behaviour#
One lock around every command.
ping,set_ee_target,set_gains,enable,disable,get_state,move_to_q,move_to_pose,log_start,log_stop,log_fetchandshutdownare JSON over a ZMQ REQ/REP socket and run one at a time, so amove_to_*— which stopsosc_shm, runsmove_toand restarts it — can never interleave with a setpoint write. Only alog_fetchreply carries more than JSON: a second frame with the rows of a 1 kHz log as raw float64. The daemon writes no run data to disk; the client does, on the PC.Gains outlive the controller.
osc_shmseeds the command block with its own defaults and the current EE pose as target, so the arm holds in place. The daemon then overwrites the gain fields — on the first start with thecontrol:block ofrobot.yaml, on every later start (insidemove_to_*, a watchdog relaunch) with a snapshot of whatever the client last set viaset_gains(snapshot_gains/restore_gainsinlocal_controller.py). Gains, clamps andenabledtherefore persist across controller restarts; only the anchor pose is re-captured.Watchdog. Every 0.5 s the daemon checks that
osc_shmis alive and relaunches it if not, so a libfranka reflex mid-run cannot leave the daemon ACKing commands into a dead controller. The relaunch re-anchors the setpoint at the current pose and restores the gains; the client’s nextset_ee_targetresumes control.Two workers, not user interfaces.
osc_shm(the 1 kHz controller) andmove_to(one-shot resets) are spawned by the daemon; running either by hand while the daemon is up is refused by the FCI lock (src/fci_lock.h). The threeread_*binaries onlyreadOnce()andgripper_cmduses the hand’s own connection (port 1338), so those take no lock and run alongside it.
Running it#
NUC python -m frankatwin.daemon [-c robot.yaml] [-v] [--load-mass KG …]
(--help lists the flags). -v passes osc_shm’s own output through; the
banner you should see:
[osc_shm] robot_ip = 172.16.0.2
[osc_shm] shm_name = /frankatwin_osc (opened)
[osc_shm] pid = 12345
[osc_shm] RT = SCHED_FIFO # "non-RT" -> no rtprio; fix before real runs
[osc_shm] tau_rate = 800.000000 Nm/s (slew limit)
[osc_shm] load = none (using Desk-configured load) # or: 0.15 kg, com=[0, 0, 0.05] m (gravity-compensated)
[osc_shm] collision = torque 100 Nm, cartesian 100 N/Nm (reflex thresholds)
[osc_shm] q_init = … # joints at start
[osc_shm] x_anchor = … # EE pose the controller holds until a client sends a target
[osc_shm] quat (wxyz) = …
[osc_shm] starting 1 kHz loop. SIGINT to stop.
INFO frankatwin.local_controller: osc_shm running; gains restored: kp=200.0/20.0 err_delta_pos=0.000 enabled=True
INFO frankatwin.daemon: daemon ready, awaiting commands
WARN: setLoad FAILED means the payload is not compensated; RT = non-RT
means the loop runs without real-time priority — fix both before real runs.
A move_to_* stops osc_shm, runs move_to and restarts osc_shm at the new
pose; the first robot.control() after that can be rejected with Move command aborted!, which the daemon retries up to 3 times, 0.5 s apart. In the log,
watchdog: osc_shm restarted marks a controller that died mid-run (the
preceding osc_shm exited unexpectedly (code=…) line carries libfranka’s
reason).
Ctrl-C (or {"op": "shutdown"}) is a controlled stop: osc_shm ramps its
torque to zero, waits for the arm to settle and exits; the daemon unlinks the
shm segment and releases the ports. Don’t kill -9 it.
Restart the daemon after editing robot.yaml (it reads the file once, at
start; no rebuild needed) and after rebuilding osc_shm, move_to or
gripper_cmd. One daemon per robot: a second one fails to bind the ports, and
any other controlling FCI client (franka-interface, franka_ros*) must be
stopped first — libfranka allows one control session at a time;
python -m frankatwin.doctor lists competitors.
Daemon protocol#
Any language that speaks ZMQ + JSON can drive the arm. Two sockets on the NUC
(network.cmd_port / network.state_port, default 5555 / 5556):
Commands — REQ/REP, one JSON object per request.
Request {"op": "<name>", ...}; reply {"ok": true, ...} or
{"ok": false, "error": "<message>"}. Commands are executed one at a time.
Every reply is that single JSON frame, except log_fetch, which appends one
binary frame (read it with recv_multipart).
|
request fields |
reply fields |
blocking |
|---|---|---|---|
|
— |
|
no |
|
|
|
no |
|
any of |
— |
no |
|
— |
— |
no |
|
— |
|
no |
|
optional |
|
no — a daemon thread drains the ring |
|
— |
ring-log summary ( |
yes (merges frames and setpoints) |
|
optional |
|
no |
|
|
— |
yes |
|
|
— |
yes |
|
— |
|
no — runs on a daemon thread |
|
|
|
no — runs on a daemon thread |
|
optional |
|
no — runs on a daemon thread |
|
— |
|
yes (quick) |
|
optional |
|
|
|
— |
|
no (daemon exits after replying) |
Gripper commands return as soon as the daemon has started them, so a policy
loop’s set_ee_target is never held behind a 2 s grasp on the serial REQ/REP
socket. Poll gripper_state (without fresh) until busy is false and
last.seq matches — that is what the Python client’s wait=True does. A
second command while one is running is refused (gripper busy); gripper_stop
aborts it.
State — PUB, JSON, 100 Hz (sent whenever a new 1 kHz frame exists):
{"timestamp_s": 12.345, "seq": 12345,
"q": [7], "dq": [7], "ee_pos": [3], "ee_quat": [4],
"ee_linvel": [3], "ee_angvel": [3], "tau": [7], "tau_J": [7]}
Minimal raw client (this is all FrankaTwinClient does underneath):
import json, zmq
ctx = zmq.Context()
req = ctx.socket(zmq.REQ); req.connect("tcp://172.16.0.1:5555")
sub = ctx.socket(zmq.SUB); sub.connect("tcp://172.16.0.1:5556"); sub.setsockopt(zmq.SUBSCRIBE, b"")
req.send_json({"op": "set_gains", "kp_pos": 500, "kp_ori": 30}); assert req.recv_json()["ok"]
state = json.loads(sub.recv()) # latest frame
req.send_json({"op": "set_ee_target", "pos": state["ee_pos"], "quat": state["ee_quat"]}); req.recv_json()
Clients should time out on REQ (the Python client uses 5 s, 60 s for move_to_*)
and rebuild the socket on timeout — ZMQ REQ sockets are strictly alternating.
Safety chain#
Ordered from first to last line of defence:
Reference pre-flight (Python, before anything is sent): the trajectory generators and
python examples/cart_impedance.pyprint peak|ẋ|,|ω|and the max orientation offset of the commanded trajectory. Reported for inspection only – nothing caps them;osc_shm’s tracking-error clamp is the safety net.Error clamp (
error_delta_pos, per tick): when > 0, clips the position error coordinate-wise and aborts the loop if the unclipped error exceeds it — bounds the controller’s own translational push toKp_pos · error_delta_pos(≈ 25 N atkp_pos=500,0.05 m).robot.yamlships0, which disables both: pure impedance, matching whatosc_shmcompiles in and what the sim does, and what the sysid excitations need — a 0.05 clamp caps the push at 10 N atkp_pos=200, too little to followmultiband, so the loop aborts. Set a positive value to get the clamp and the abort back. The orientation channel has no clamp and no tracking abort at any setting. Override at runtime withset_gains(error_delta_pos=…)orpython examples/cart_impedance.py --err-delta-pos.Torque clamp
τ_maxper joint.Torque slew limiter 800 N·m/s per joint (libfranka’s own limit is 1000). The Jᵀ law emits a torque step whenever its input jumps — a new setpoint,
set_gains,enable/disable, a phase boundary in a scripted motion. Without the limiter a 7.5 N lateral step at an outstretched configuration produced Δτ₁ ≈ 2.9 N·m in one tick (≈ 2900 N·m/s) and trippedcontroller_torque_discontinuity. With it, transitions ramp over a few ms and in-phase motion is untouched.Controlled stop: SIGINT/SIGTERM/duration/abort do not return
MotionFinishedwith a zero-torque step; the slew limiter ramps τ to zero and the loop finishes once|τ|∞ < 0.05 N·mand|q̇|∞ < 0.05 rad/s(or after 1 s).libfranka reflexes: collision thresholds (configurable; 100 N / 100 N·m by default because insertion contact at
kp_pos=500legitimately reaches ~25 N and the factory 20 N default aborted every insertion), joint/velocity/torque limits,communication_constraints_violation.automaticErrorRecovery()atosc_shmstart clears a latched reflex so one incident doesn’t require a daemon restart.Watchdog relaunch (above).
Nominal joint-limit checks in osc_shm/move_to are warnings, not aborts — the
robot’s hard limits still apply at the driver level.
IsaacLab tasks#
Installed into an IsaacLab checkout by isaaclab_sysid/install_into_isaaclab.sh
(package isaaclab_tasks.direct.franka_sysid, asset franka_mimic.usd, the four
scripts under scripts/tools/ — re-run it after pulling, the scripts are copies):
task id |
env class |
purpose |
key cfg fields |
|---|---|---|---|
|
|
Replay a target trajectory under the mirrored controller; log the sim state. |
|
|
|
Same controller, |
as above + per-env q_init / gains / armature / friction / delay written by the sysid script |
script |
role |
|---|---|
|
CMA-ES over 29 parameters → |
|
Replay with the fitted parameters, then compare sim against real (figures + |
|
ZOH replay of a 50 Hz log at 1 kHz |
Details and the parameterisation: System identification.
Repository layout#
src/ C++: osc_shm (1 kHz controller), move_to (reset), gripper_cmd, read_* utilities,
shm_layout.h (the shm ABI), fci_lock.h
python/frankatwin/ daemon, FrankaTwinClient (PC), LocalController (NUC), config, shm_layout,
ring_log, quat, doctor, excitation/ (multiband + chirp reference math)
config/robot.yaml network, robot IP, gains, safety clamps, collision thresholds, gripper, payload
examples/ move_to (home / joints / EE pose), cart_impedance (sine / multiband / chirp),
gripper, policy_loop
scripts/ gen_*_traj (write references), shm_log, check_torque_limits, plot_ee_tracking,
read_q.sh
isaaclab_sysid/ self-contained IsaacLab extension: the two tasks, franka_mimic.usd, and the four
scripts/tools/ (sysid fit, ZOH replay, apply + compare)
tests/ 11 pytest files, none of which need a robot: the shm ABI (compiles shm_layout.h
and diffs every offset against the numpy dtypes), config, doctor, excitation
math, quaternions, ring log, gain persistence, gripper, command round-trip,
1 kHz logging, sim-vs-real comparison
docs/ Guide: installation · usage · sysid. Reference: sysid_details · architecture ·
configuration · api · data_format · troubleshooting