Most robot arms are told what position to hold. Very few are told what they are carrying. Pick up an unknown object and gravity pulls the arm off its trajectory — and unless the controller's model knows the extra mass, it has no way to push back with the right amount of torque.
This is a control stack that estimates the payload and compensates for it, written in real-time C++ against ROS 2 and taken from a MuJoCo model to a real 5-DOF arm. In simulation the estimator identifies the mass to within a few grams. On the real arm it returns almost nothing — for four coupled reasons that can be named, and that took a different method to work around.
One loop, two plants
The whole stack is built around a single abstraction. PlantInterface
takes torque in and gives state out. MuJoCo implements it by writing torque
straight into the simulator. The hardware backend implements it by talking to six
Feetech STS3215 servos over a serial bus. Above that line, the controllers — PD,
computed torque, adaptive computed torque with online payload estimation — never
learn which side of the boundary they are on.
The runner puts the loop at 200 Hz on a SCHED_FIFO thread with
mlockall, under a PREEMPT_RT kernel, with its period
driven by an absolute deadline (clock_nanosleep with
TIMER_ABSTIME) so a late wakeup never shifts the next one. That is the
configuration today; the tracking logs below predate it, so the tracking numbers are
not real-time measurements. The jitter figures further down are, on a separate
bench.
What simulation says
Before writing a line of controller code, I put in a blocking gate: the MuJoCo
plant and the Pinocchio model the controller uses must agree on gravity, nonlinear
bias and the mass matrix across five poses: worst disagreement is
1.33e-15 N·m on gravity, 8.66e-13 N·m on the
bias, 5.64e-12 on the mass matrix. If
they disagree, the benchmark refuses to run. That gate is the reason the simulation
numbers mean anything.
With it in place, computed torque does what theory says. Against a naive PD controller carrying a known 200 g payload, settled arm RMS drops from 0.0216 to 0.0028 rad and the end-effector miss from 0.0300 to 0.00018 m. The mechanism is not gain tuning — plain PD must retain position error to generate holding torque, while computed torque supplies the modelled gravity torque at zero error.
Then take the payload out of the controller's model and let a recursive least-squares estimator find it online. It closes 102.4% of the RMS gap between the empty-model controller and the perfect-model bound. It edges past 100% because the residual Coulomb deadband differs between the two trajectories, not because an estimated model beats a perfect one. The mass itself lands at 202.3 g against a true 200 g.
That is the result I expected to carry over to hardware. It did not.
Why the estimator fails on a real servo bus
The estimator solves
τ − ID_empty(q, q̇, q̈) = Φ(q, q̇, q̈)·m — the torque left over after
the empty arm's dynamics are accounted for is attributable to the payload. On a
torque-controlled plant that identity holds. On this hardware it is false, for four
coupled reasons:
- τ is not plant torque. The STS3215 has no closed-loop N·m mode. What the controller emits is realized as a position offset on the servo's own inner PD loop. The torque that actually holds the arm is generated inside the servo, and it is not the number in the equation. This one does not vanish at rest.
- Acceleration is a double difference of a 12-bit encoder. One encoder count at 200 Hz reads as 61.4 rad/s² — against a true minimum-jerk peak near 4.6. Noise is roughly 13× signal, and it sits in the regressor, so it biases the estimate systematically rather than averaging out.
- Modelled inertia swamps gravity. The joint armature term turns one count of that noise into 1.72 N·m of phantom inertial torque — already 2.1× the gravity torque on the shoulder-lift joint, with the wrong sign, 799 times per run.
- An unmodelled friction floor. On an earlier run the empty-arm bias came out ~18× its simulation value, consistent with a 345:1 gearbox the model does not represent.
Ported unchanged, that estimator returns 0.8 g for a 90 g
payload and 9.4 g for a 180 g one. It does not diverge — it
collapses to near zero and passes untouched through the [0, 0.5] kg
projection, so the arm tracks within 7% of a controller carrying no payload model at
all. An earlier run failed more loudly: the same estimator drove the mass to
−0.175 kg and only the projection held it at zero. Quiet
or loud, there is no payload information in it.
What replaced it
Causes 2 and 3 vanish at rest. Cause 1 does not. So the hardware path stops trying to estimate during motion: hold a pose, read motor current from the servo's current register, approach from both directions to cancel Coulomb friction, solve for mass, freeze it, then track.
That raw reading turns out to be a biased instrument — it reads about 2.3× high. Fitting an affine correction across four masses and applying it inside the controller, before the physical mass clamp, is what makes the estimate usable:
| Payload | Baseline → result | Settled arm RMS | Mass estimate |
|---|---|---|---|
| 90 g | naive PD → computed torque, empty model | 0.0268 → 0.0093 rad (−65%) | not estimated |
| 180 g | naive PD → computed torque, empty model | 0.0356 → 0.0177 rad (−50%) | not estimated |
| 90 g | empty model → calibrated static ID | 0.0093 → 0.0064 rad (−31%) | 82.8 g |
| 180 g | empty model → calibrated static ID | 0.0177 → 0.0097 rad (−45%) | 176.0 g |
Hardware RMS is pooled over four joints, excluding wrist-roll, which the controller cannot reach and which holds a constant ~0.027 rad offset; the simulation figure quoted earlier is a five-joint mean, so the two plants are comparable only within themselves. The hardware PD and computed-torque gains are retuned for the servo's own inner position loop — same controller names as the simulation experiment, not the same experiment.
The detail I find most useful is the orange trace. An uncalibrated payload estimate is worse than not compensating at all — 0.0127 rad against 0.0093 for the empty model at 90 g. Feeding a biased mass into an otherwise correct model actively hurts. The calibration is not an optimisation; it is what makes the estimate usable in the first place.
Where the time actually goes
A real-time kernel is easy to treat as a talisman. It is more useful to measure what it buys. Under load, wakeup jitter on this box is 6.18 µs at p99 and 11.34 µs at p99.9, on a 5000 µs period. The serial round-trip to the servos is 2.14 ms — about 189× larger.
So the scheduler was never the constraint; the bus is. A 2.14 ms round-trip does not fit inside a 1 ms tick, so 1 kHz is out of reach on this wiring — whether trimming each servo's Return Delay Time would bring it back is untested. The useful part is that you only find out which end binds by instrumenting both.
What this does not claim
It is an elbow result. Only the elbow's servo stiffness is identified from measurement; the other five values are frozen placeholders. The shoulder-lift offset actually regresses in both calibrated runs.
The calibration belongs to one grasp. Jaw opening moved monotonically with mass as the weights were stacked, so the 2.315 scale conflates instrument gain with grasp geometry until a fixed-geometry campaign separates them. Run-to-run spread in the mass estimate (7.8 g at 90 g, 14.6 g at 180 g) also exceeds the 5.03 g fit residual, so the spread is the honest bound, not any single run's error.
One held-out mass is not a generalisation test. Both headline payloads are points the calibration was fitted on. A 70 g object outside the fit came back at 56.7 g — 13.3 g out, inside the 14.6 g run-to-run spread but about 19% of the payload. It was also gripped differently, off the stacked-weight series, so it moves mass and geometry at once: it establishes neither same-geometry interpolation nor that the scale carries to a new centre of mass.
Those caveats are in the repository too, at similar length.
Source
Code, all hardware logs, and the full derivation with discarded runs and their
reasons:
github.com/0xshinobii/so101-rt-control.
Stack is C++17, ROS 2 Jazzy, Pinocchio, MuJoCo, on Ubuntu with a
PREEMPT_RT kernel, against a ThinkRobotics SO-ARM101.