One robot arm. One model. Joint space to Cartesian.
The UR5 — a 6-revolute industrial arm — modelled end-to-end as a multibody DAE. Closed-form inverse kinematics feeds a per-joint PD controller with gravity feedforward that holds a Cartesian end-effector path in simulation. Kinematics, inertia, and control, all on one living digital replica.
Every subsystem, on one canvas.
Nothing is stubbed — the kinematic chain, link inertia, and control all carry their own physics.
Six revolute joints
Standard UR5 DH parameters chain six revolute joints from base to wrist. Rotor inertia (Jr) is available as an opt-in parameter but held at 0 for the 6R chain — Jr > 0 triggers an MTK index-reduction pathology that freezes chains with N ≥ 5 joints (ADR-0072).
Full-inertia links
Link masses and diagonal principal inertias come from the published UR5 URDF. Off-diagonal inertia terms are neglected; the multibody DAE captures the dominant inertial forces without full tensor coupling.
Closed-form IK
A decoupled-wrist analytic solver closes the inverse-kinematics loop in one branch. Round-trip error (FK ∘ IK) is ~1e-16 rad — at floating-point machine precision.
PD + gravity feedforward control
Per-joint PD plus gravity feedforward: in_tau → −q. Each joint's control law is independent, with gains tuned to the arm's natural frequency.
FK end-effector observer
Forward kinematics propagates joint angles to a Cartesian end-effector position that is compared to the reference path, providing the tracking error signal.
Multibody DAE composition
Revolute, FixedTranslation, FixedRotation, and Link primitives from the pure-DJL multibody kit compose the DH chain. The engine lowers this to a symbolic DAE.
Built from the DH chain, not hand-assembled matrices.
The UR5 model is assembled from the same pure-DJL multibody kit that powers the Djinborn quadcopter — Revolute, FixedTranslation, FixedRotation, and Link primitives wired according to the standard DH parameters. Kinematics and inertial forces emerge from the composition; there are no hand-coded Jacobians or mass matrices. The engine init-policy is set to NoInit (ADR-0071) so the analytic IK can pre-condition joint angles before the DAE integrator starts — a requirement for the decoupled-wrist closed-form solver to operate cleanly in the loop.
- Pure-DJL multibody kit (Revolute/FixedTranslation/FixedRotation/Link) — no Rust or Julia changes. — ADR-0067
- Closed-form decoupled-wrist IK, one branch, non-singular poses. — ADR-0072
- NoInit engine policy lets analytic IK pre-condition the DAE state. — ADR-0071
Four gates. Real numbers.
Simulation verified against four gates on the feat/multibody-ur5-arm branch. Scope: non-singular poses, one IK branch. No physical robot — these are digital-twin simulation results.
| Gate | Result | |
|---|---|---|
| G-B1: gravity-comp pose hold | PASS | joint angles stable, no drift |
| G-B2: joint tracking error | PASS | <0.0013 rad per joint |
| G-B3a: IK round-trip (FK∘IK) | PASS | ~1e-16 rad (machine precision) |
| G-B3b: Cartesian tracking (peak) | PASS | 0.485 cm |
| G-B3b: Cartesian tracking (steady) | PASS | 0.41 cm |
The bug that looked like a physics gap.
When the 6R chain first ran, the DAE appeared under-determined — the solver couldn't close. The first hypothesis was missing rotor inertia (an easy fix: add Jr per joint). The model ran, but tracking remained poor. The real cause was a dangling terminal frame: the sixth joint's output Frame connector had no downstream body to react against, so the constraint equations were degenerate. Identifying that — and not the inertia — as the root cause required reading the residual structure rather than guessing. The fix was one line. Framing the misdiagnosis honestly is part of the record: engineering rigour means following the residual, not the first plausible story.
See it run on your own models.
Book a guided walkthrough and we'll open the UR5 — or a model from your domain — and run it end to end.