The build scripts now model the parts the system will be made of: - Head: the sleeve's hub was a separate solid, held by nothing. It now stands on a roof, with four M2.5 screws into the roll horn. The holder is screwed to the yoke, with a 10-24 insert for a misting nozzle and an M5 insert for an SMC KQ2L04-M5A elbow. The tip is 105.3 mm below the wrist. - Feed tube: modelled the whole way, about 1.13 m, clipped along each link with a loop at every joint. CableLoop draws those loops in blue, and LeadDisplay lists them beside the leads'. - Electrics: one bay beside the column, since behind it the plate would hang off the bench. It holds the adapter, a Pololu D42V110F7 (Pololu's own STEP, in tools/parts), the Black Pill pump board, two fuses and an XT60 inlet. - Bench: the booth's shelf carries the pump, bottle, strainer and brick, and the STOP station hangs on the table's side. check_spray_arm now also checks the tube and the elbow: clear across every joint's travel. tools/compile_spray_system.py packages the system as one GLB and the printed parts as STLs in docs/spray_arm, with a parts list. The app's own spray arm and booth models are not regenerated from this yet. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> |
||
|---|---|---|
| addons/godot_ai | ||
| Assets | ||
| Collision | ||
| core | ||
| docs | ||
| Environments | ||
| python_bridge | ||
| Robots | ||
| Scenes | ||
| Scripts | ||
| tests | ||
| tools | ||
| .editorconfig | ||
| .gitattributes | ||
| .gitignore | ||
| export_presets.cfg | ||
| icon.svg | ||
| icon.svg.import | ||
| project.godot | ||
| README.md | ||
Comfac Robot Arm Controller
A Godot bench for LeRobot SO-101 and FANUC R-2000iC/165F robot arms, and a controller for Feetech STS servos on a Waveshare-style board: simulate an arm, set up a physical one servo by servo, and drive it.
Version 0.10.0 -- in development; 1.0.0 will be the first official release. This is the app's second
build, a rebuild rather than a revision of the first; that one's last version, v0.9, is preserved on
backup/v0.9-so101-final.
Quick start
Requirements
- Godot 4.7.2-stable — the project is
GL Compatibilityand runs on modest hardware. - git-lfs — the
.glbmodels are LFS objects. Clone without it and every arm is a 130-byte pointer file, which fails as "no joints resolved" rather than as a missing asset. - Python 3.12 or 3.13 in
python_bridge/.venv, only for Controller mode in the editor. An exported app carries a frozen bridge instead. See The physical link.
godot --path . # opens Simulation: the SO-101 leader & follower bench
Press Esc or F10 (or the bar's Menu button) for the menu. It offers the two modes and works from anywhere.
The two modes
| Mode | What it is | Reaches hardware |
|---|---|---|
| Simulation | Godot alone. Opens the SO-101 leader & follower bench; the bar's dropdown switches to the other simulations | Never |
| Controller | Finds the servo board, scans the servos, and puts a card per joint on screen | Yes |
Simulation never drives the physical arm. DriverManager refuses to install or launch the bridge
while the app is in Simulation, and a test asserts it.
The simulations, from the Simulation bar's dropdown:
| Scene | What it is |
|---|---|
Scenes/So101Virtual.tscn |
The bench, leader and follower |
Scenes/So101Solo.tscn |
One SO-101 at its own bench, wearing the leader or the follower model, with things to grasp: three printed cubes (20, 25, 30 mm), two dowel pegs and a peg board they drop into, and an open container -- real sizes, weights and shapes (core/prop_catalog.gd) |
Scenes/So101TwoJaw.tscn |
The two-jaw prototype: the solo bench, with a two-servo head (one servo per jaw) |
Scenes/So101Spray.tscn |
The spray-paint arm, modelled as it is to be built: the follower with a pump-fed nozzle on a sixth joint (nozzle_tilt, the gripper's servo) in place of its gripper, so the nozzle can be aimed any way; the pump and paint reservoir on the table's shelf, run by a switched output, feeding the nozzle through a tube along the arm; in its own paint shop (tools/build_spray_booth.py, built in Blender) -- the solo bench's table and mount in an open-fronted booth (filter bank, lit canopy, slots for the line), in a shop with a loading rack and control desk on one side, a drying rack and curing oven on the other, and a paint store behind -- and an overhead indexing conveyor: a track under the canopy running straight through the booth, wall to wall, in front of the arm, with each part hung from a trolley on a hook that lets it swing. The parts are furniture in miniature, modelled in Blender (tools/spray_part_models.py, tools/build_spray_parts.py): a table, a chair, a window frame, a stool, a bookshelf, a panel door and a slatted crate, 40-80 mm, each of its legs, rails, slats and panels a piece that collides and takes paint. The Spray panel runs the line (Belt Run / Stop, Paint parts, Feed): parts are hung on outside the left wall, in turn, and the line moves them 160 mm at a time past three stops, waiting for them to stop swinging, where, at the stop in front of the arm, the part is painted all round: its hook turns it a quarter at a time so each side faces the arm in turn and is painted square, the top and underside from both ways where they have depth, and deep sides -- a bookcase's cubbies -- tipped into from above and below; the arm rasters each side of the part's outline, the spray landing on whatever stands there, every point solved and checked against the arm's reach and both guards (SprayPlanner, PaintCell); then they are taken off past the right wall. Between faces the arm's moves are eased and routed clear of the parts. STOP stops the line |
Scenes/FanucVirtual.tscn |
The cell, six axes |
Scenes/FanucSolo.tscn |
One Fanuc at its own station |
A Solo scene shows one arm: its own environment, one mount, one view. The LeRobot one carries a skin toggle — the two SO-101 GLBs are one kinematic arm with two different end effectors, so a scene showing one of them has a genuine choice to offer. Switching the skin switches the profile, so the 6th axis' travel, the 6th link's mass and the tool point follow the model on screen.
Every simulation is driven with the Controller's cards, docked beside the 3D views:
- The band across the top is the Controller screen's (
StatusBand), and shows the simulation picker, the input in use and STOP. Its ☰ Menu opens Endpoints…, the Fanuc's Cartesian jog…, Snap to rest pose, and the way to modes, settings and exit. - One compact card per joint you drive, up to 10, with the same hold-to-move buttons and per-card 1°/5°/10° step. Holding a card is what arms the arm; letting go stops it. A step the safety machine refuses (tip speed, the bench) is trimmed until it fits, down to 0.05°, and the card says why when it cannot move at all.
- Poses: Home, Rest and any you save (
user://poses/<robot>.json). A pose runs only while its button is held, through the same safety checks. - Animations (
user://animations/<robot>.json): a list of steps -- move to a position recorded with + Move here, wait some seconds, or go to Home or Rest (+ Home, + Rest), each move at 25, 50 or 100 % speed, and each step straight on to the next -- run once, a set number of times, or until stopped. Run is pressed once and the animation goes on its own, each move through the same safety checks as a pose. It stops on its Stop, on STOP or Snap to rest, or when a card or pose button is used. The editor says what follows every step -- the next step, another run, or the end and where the arm is left -- and flags a move recorded twice without the arm moving, which would finish at once. Each move's Jump here puts the arm at that step's position at once. In the spray scene each move is also a travel move (spray off) or a spray move at Low, Medium or High, on for that move only -- a 3D printer's extrude and travel moves -- so the arm never stands spraying one spot between passes; waits, the end of a run and anything that stops it turn the spray off. - Keys as on the Controller screen, plus H for Home.
Controller (Scenes/Controller.tscn) is laid out like this:
-
The startup screen launches the bridge and searches every servo-adapter USB port. No port is assumed or remembered. If the search cannot choose, it lists the candidates.
-
The cards are the screen.
- The status band across the top shows the arm (a picker for saved arms), the board, servo power, which input is driving, a large STOP, and the ☰ Menu. The menu holds Configure, Calibrate, Reset servos, Release all, the theme and the way to the main menu.
- One card per joint, up to 10, in the arm's order and under its names. Each card shows a large angle, a bar showing where the joint sits in its travel, and its state in words ("Locked", "Limp", "Near limit", "Not answering", "Stopped"). Its hold-to-move buttons move the joint only while held. A servo that is missing or shares an ID gets a frozen card that says why.
- Alarms stay listed beside the cards until they are over and acknowledged.
- Poses beside the cards: Home (every servo centred) and any you save with the arm. A pose runs only while its button is held.
- Animations in a column of their own beside the poses, kept with the arm: steps that move to positions recorded with + Move here, go to Home (+ Home) and wait, each move at 25, 50 or 100 % speed, run once, a set number of times, or until stopped. Run is pressed once; the animation then moves the arm on its own, each move under the same travel and lead as a pose button. Each move's Hold: go here takes the arm to that step's position while held. It stops on its Stop, on STOP or Release all, when a card or pose button is used, or when the board's link is lost.
- Keyboard: 1–9, 0 pick a joint, hold ↑ ↓ to move it, Q W E set 1°, 5° or 10°, hold H for Home, and Space is STOP. Leaving the window lets go of everything.
- Themes: Light, Dark or Grey, remembered.
-
Configure is a pop-up. It names, reorders, includes or excludes servos, changes IDs, and identifies a servo by swinging it. The arm is saved as a profile (
user://controllers/). -
New servos all answer on ID 1. Add them to the chain one at a time with Configure open. With Auto-number on (the default), each one gets its ID the moment it appears: the arm's lowest missing ID, then IDs past the arm, and never ID 1.
- If the arm keeps a joint on ID 1, a servo that arrives there could be that joint or a new one, so Configure asks which.
- Two servos on one ID cannot be told apart on the bus, so plug in one at a time.
-
Each card has its own speed: 1°, 5° or 10° a step (20, 100 or 200 °/s), saved with the arm.
- Each goal tells the servo how fast to move.
- The goal never runs more than two steps ahead of where the servo reports itself. A servo that can't keep up is waited for, and letting go stops it within those two steps.
-
Reset servos… (in Configure) has two options for the servos you tick:
- Clear calibration sets the zero offset back to 0 and the limits to the whole turn, keeping the ID.
- Factory reset does the same, and also puts the servo back on ID 1 and removes it from the arm.
The baud rate is kept either way. Factory reset asks for confirmation, because servos reset together all share ID 1 afterwards.
-
Calibrate is a four-step pop-up for every servo of the arm at once:
- Home pose: release the arm, pose it, and Set zero. Each servo's Homing_Offset is written, so that pose reads 0°.
- Travel: turn each joint by hand from stop to stop. A joint with no stops is marked Free rotation.
- Direction: Nudge +5°, and tick Reverse if the joint went the wrong way.
- Save: the travel, 2° inside each end, is written into the servo's own limit registers and saved with the arm.
The cards obey both the measured travel and the servo's registers, and a reversed servo's buttons and readout are flipped. A servo whose zero no longer matches its calibration (swapped or reset) is flagged "zero changed", and only its registers apply.
-
STOP (or Space) holds every servo where it is and refuses all motion until Clear is clicked. Release all (in the ☰ menu) turns torque off, so the arm goes limp.
The robots
Each robot is a profile — data, not code — in Robots/. Adding an arm means adding a profile and a
scene, never editing the engines.
LeRobot SO-101 — 7 joints, 6 of them physical: shoulder pan, shoulder lift, elbow flex, wrist flex,
wrist roll, gripper. base is view-only. Reach is 0.35 m declared; the measured reach is 0.3858 m
and the tool point is 0.3561 m from the pan axis. Link masses are measured from mesh volumes at
PLA density (0.739 kg follower, 0.793 kg leader, against a published arm-only figure of about
800 g), and each joint is rated at the servo's published 15 kg·cm stall torque (1.471 N·m).
Four SO-101 profiles, all built by ProfileBuilder from one body of shared facts:
| profile | what it is |
|---|---|
Robots/so101_follower.tres |
the arm that carries a load |
Robots/so101_leader.tres |
the operator's input device |
Robots/so101.tres |
the same arm under the identity the app has always defaulted to |
Robots/so101_two_jaw.tres |
the follower with a two-servo head: two identical jaws, each on its own servo (bus ID 7 for jaw B) — 7 physical joints |
Robots/so101_spray.tres |
the follower rebuilt for spraying, on the follower's joint frames but with its shoulder centred on the turntable (the lift joint 30.4 mm back over the pan axis) and its wrist roll 14 mm further out -- all-new structure, with a cable bay behind every servo and the daisy-chained servo leads routed through the arm, each within a stock 22 cm lead -- built as it would be assembled: each servo in a cradle under a screwed-on retainer, its cable out of a window in the back; every joint, the nozzle tilt included, carried on both sides by a horn plate and a bolt through a bearing; hollow arms with the feed tube inside; a column clamping the pan servo on a wide ballasted base plate -- with a pump-fed spray nozzle on a sixth joint (tools/build_spray_arm.py): no gripper joint; nozzle_tilt, the gripper's own servo (bus ID 6) turned upright under the wrist, tilts the head off the roll axis; the wrist roll limited to ±180° for the feed tube; the tool point is the nozzle tip, in the tilt joint's frame; the pump's duty per spray level — 6 physical joints |
The two physical arms are one kinematic chain and two different end effectors, verified from their two URDFs and independently from the shipped GLBs (6/6 link lengths and 7/7 joint origins agree to 0.000 mm). So the per-arm profiles differ in exactly three things: the 6th axis' travel (jaw −10…+100, trigger 0…+45 and provisional), what the 6th link weighs, and where the tool point is.
FANUC R-2000iC/165F — 6 axes, 165 kg payload, 1090 kg of machine. A shoulder-offset 6R with a
spherical wrist: J4 and J6 roll about the link axis and are collinear, J5 bends, and all three axes
meet at J5's origin. Link masses are the model's own proportions anchored to FANUC's published 1090 kg
— the model's volumes at steel density come to 1967 kg, so taking them directly would be wrong by a
factor of 1.8. Joint origins were corrected from the CAD by segmenting the castings and fitting
coincident bore axes: J2 moved 125 mm, J3 149 mm, and J2→J3 / J3→J5 are 1.0677 m and
1.2996 m (not the published 0.8308 / 1.2437, which were the old, wrong frames).
Profiles are generated, not hand-edited:
godot --headless --path . --script tools/generate_profiles.gd
core/profile_builder.gd is the source of truth; the generated .tres are what the scenes load, so a
change to the builder does nothing until you regenerate — and tools/verify_rebuild.gd asserts every
.tres in Robots/ still equals what the builder would write, field by field.
Architecture
core/ the engines and the panels: state, safety, motion, drivers, kinematics, displays
Scripts/ main.gd, the root of every scene
Scenes/ the five simulations and Controller
Robots/ generated profiles
Assets/ models and environments (Git LFS)
tests/ the suites
tools/ the verification gates and the generators
python_bridge/ the servo bridge for Controller mode (own README)
addons/ godot_ai, a development tool — not part of the app
Six autoloads, each one answer to a question the whole app asks (the project registers a seventh,
_mcp_game_helper, which belongs to the development addon):
| Autoload | Owns |
|---|---|
JointState |
Commanded, actual and effort per joint; the only thing panels read |
Driver |
Which driver is live, and the bridge process — never a bridge in Simulation |
AppMode |
The mode — Simulation (and its solo variants) or Controller |
Sessions |
Recording and replay |
Display |
Fullscreen or windowed, the resolution, and UI scaling |
Menu |
The scene overlay |
State before everything. Every panel writes to JointState and reads back from it. Writes carry a
provenance — ui, hardware or system — and only ui is forwarded to hardware. A value that
arrived from the machine is never sent straight back.
One source of geometry. A scene names a model; ArmRig resolves that model's nodes to joints by
name and alias and reports loudly when the rig is incomplete, because a partly-resolved rig still
solves and still moves — just to the wrong place. The tool frame is the flange, not the end joint:
the end joint is the moving jaw, so a fixed tool point written in its frame would swing as the jaws
open.
Two solvers for a 5-DOF arm, one for a 6-axis machine. The SO-101 uses ArmIk (CCD) and
ArmSolver (damped least squares with planar seeds). The Fanuc gets a closed-form decomposition — a
two-link triangle for J1–J3 and an X–(−Z)–X Euler for J4–J6 — because an industrial arm does not move
like a hobby arm.
The driver seam. RobotDriver is the interface; MockDriver and BridgeDriver implement it, and
the mode decides which is installed. The mock is deliberately not a pass-through: it moves at a finite
rate and settles short of its target, so following error and divergence are exercised before hardware
exists. BridgeDriver is Controller mode's: it speaks to servos by ID, not to a modelled arm.
Reading the load
- Effort is what a joint is doing now — a third value per joint beside commanded and actual, written only by a driver, never clamped, and deliberately independent of position. A joint holding a static pose while carrying something has equal commanded and actual values, and the effort is the only thing that says the machine is working.
LoadModelfills it in simulation: the gravity torque on each joint, from geometry read off the rig and masses declared by the profile. Gravity only — no friction, inertia or servo model, because those would be claims about a machine nobody here has seen. Each link's measured centroid is used, not its midpoint: the midpoint changes the SO-101's elbow torque by up to 0.1992 N·m (13.5 % of its rating) and the Fanuc's j2 by 298.9 N·m.- Duty (
LoadDuty) is that effort averaged over a stated 12-second window — an honest filter, not a thermal model, and the legend says which is on screen. LoadScaleholds the colours and bands once: idle, working and straining at 35 % and 70 % of rated. A joint nothing has measured is left untinted rather than painted the green that means "fine".
The physical link
python_bridge/ holds the servo board's serial port for Controller mode and serves the app over a local
socket. It has its own README covering wiring, jumper caps, the protocol and
troubleshooting. Three things are worth saying here:
-
It is native. It talks to Feetech STS servos by ID through the Feetech SDK. There is no LeRobot, no six-servo limit and no calibration file, and it starts in about a second.
-
The app starts it and it exits with the app. In the editor that is
python_bridge/.venvrunningarm_bridge.py:uv venv python_bridge/.venv --python 3.13 uv pip install --python python_bridge/.venv/Scripts/python.exe feetech-servo-sdk pyserial -
An export needs the frozen bridge beside it, or it cannot find the board. Export, then:
powershell -ExecutionPolicy Bypass -File python_bridge/build.ps1 -ExportDir .This puts a 21 MB
bridge\arm_bridge.exenext to the.exe. Re-run it wheneverpython_bridge/changes.
Verification
$env:GODOT = "C:\path\to\Godot_v4.7.2-stable_win64_console.exe" # the gates invoke %GODOT%
tools\verify_all.cmd
Seven gates, because no one of them is sufficient:
| Gate | Command | Catches |
|---|---|---|
| Wiring | tools/verify_rebuild.gd |
Subsystems that each work and are not connected |
| Clearance | tools/check_bench_clearance.gd |
An arm sunk into what it stands on, on any of its skins |
| Immutability | tools/verify_arm_immutability.gd |
Physics that can move a proxy-riding arm (the two-jaw) — 500 kg driven into it for 15 frames |
| Behaviour | tools/run_tests.gd |
The suites, and a suite that failed to load |
| Props | tools/check_props.gd |
Props that sink, jam, slide where they should roll, or are not the weight they say |
| Grip | tools/check_grip.gd |
A jaw that does not stop on what it closes on, a held object that shakes, slips or is not let go, and one too heavy that is held anyway |
| Conveyor | tools/check_conveyor.gd |
A line that does not stop hung parts at its stations, a part not painted all round -- every face of every piece of it measured -- a part not taken off past the track's end |
Individual gates, and the generators:
godot --headless --path . --script tools/verify_rebuild.gd
godot --headless --path . --script tools/check_bench_clearance.gd
godot --headless --path . --script tools/verify_arm_immutability.gd
godot --headless --path . --script tools/run_tests.gd
godot --headless --path . --script tools/check_props.gd
godot --headless --path . --script tools/check_grip.gd
godot --headless --path . --script tools/check_conveyor.gd
godot --headless --path . --script tools/generate_profiles.gd # regenerate Robots/*.tres
godot --headless --path . --script tools/measure_link_masses.gd # derive link masses from the models
Five things about the gates are worth knowing before trusting them:
GODOTmust be set, to the_consolebuild — the GUI-subsystem binary captures no stdout, and every gate result is read from stdout.- Compare the count, not just the verdict. A gate that stops early prints a lower total rather than
an error, and a tree with a parse error has printed
REBUILD VERIFIED: 118 checks passed. - Run
--importfirst on a fresh clone or after any asset or profile change..godot/is gitignored, so a clean clone has no class table and the wiring gate fails with an unresolvable class name that reads like a code fault. A stale import cache is indistinguishable from a broken arm. - The suite count is asserted, because a one-line parse error once removed an entire suite and the
run still printed
ALL TESTS PASSED. - The editor's test runner uses a stale script cache. A green editor run is not evidence; the headless one is.
Most of this project's real defects were found by looking at the running app, and several were invisible to every gate: an inverted joint axis, joint origins off their bores, a legend anchored off the bottom of the screen, and a bench that shipped with both arms sunk 107 mm into it while every test passed. The gates are the floor, not the ceiling.
Known gaps
Honest and current:
- Calibration is verified on a single bench servo, not yet on the assembled arm. Joints with real end stops and load have not been calibrated.
- Controller mode drives servos, not a modelled arm. The cards are per servo, with no IK, no 3D twin and no load display. The simulations have those, but they never reach hardware.
- One board per app. Several boards, factory reset and mirrored paired arms are designed only.
- The Fanuc has no rated effort, so its load is uncoloured. Needs the machine's own maintenance manual; the app says so rather than inventing a denominator.
- No DH parameters. Profiles are
MEASUREDand the validator refuses DH until the parameter storage exists. - Collision geometry is a floor, not a world.
workspace_floor_yis enforced on the tool; the reach walls inworkspace_bounds()are centred on the world origin rather than on the arm and are recorded as unenforced rather than guessed at. - The collision guard is two-endpoint, not swept, so the path between endpoints is unverified at speed.
- The control loop is frame-driven, not fixed-period.
- No real Fanuc driver. The bridge speaks Feetech servos only.
- LeRobot dataset export is not built. Recordings are the app's own JSON Lines event stream, and replay writes them back as hardware observations, so a replay never drives a physical arm.
- Fanuc joint travel limits come from secondary sources; J3 (±312°) is uncorroborated, and FANUC's own J2/J3 limits are a split range rather than a symmetric total.
docs/PROJECT_NOTES.md is the other half of this document. It holds the
open questions with what would settle each one, the derivations of measured numbers the code states
without explaining, and the decisions whose reasons are not in a comment. Read it before changing a
number: most of them are argued somewhere.