From camera coordinates to mechanical radians
Twenty-one points in a camera’s coordinate system on one side. A CAD assembly with hard mechanical stops on the other. This is the arithmetic that makes them agree — and the one row where it currently does not. The vision layer gives you 21 points floating in a normalized coordinate space that has no physical units and a faked depth axis. The mechanical layer gives you fifteen revolute joints, each with a lower and upper bound in radians that came out of a CAD mate.
Nothing connects them. Building that connection is the actual work of this project, and it happens in about forty lines of Python.
flowchart LR A["21 landmarks
(x, y, z) normalized"] --> B["Triplet selection
15 × (p₁, p₂, p₃)"] B --> C["Two vectors per joint
v₁ = p₁ − p₂ · v₂ = p₃ − p₂"] C --> D["Dot product → arccos
θ in radians"] D --> E["Normalize
flexion ∈ [0, 1]"] E --> F["Lerp onto URDF limits
θ_urdf"] F --> G["JointState
names + positions"] Step 1 — pick three points # To measure a joint you need the joint itself and the two bones meeting at it. In landmark terms: the vertex, plus its two neighbours.