How I built a simulated 6-DOF arm with damped IK and a URDF viewer

I built humanoid-arm-teleop as a portfolio simulation demo: move a Cartesian target in the browser and watch a single six-degree-of-freedom arm follow it. Despite the repo name, this is not a full humanoid. There is no physical robot, serial bus, or real-time hardware controller.

Screenshot: humanoid-arm-teleop repo.

The shared deadman and e-stop ideas are covered in my browser teleop console tutorial; here I focus on arm geometry, inverse kinematics, and trajectories.

One model, two consumers

I keep the arm definition in arm/arm.urdf. Python reads its joint origins, axes, and limits for forward and inverse kinematics. The browser loads the same file for rendering.

FastAPI in backend/main.py serves the page, the URDF, and a WebSocket endpoint. The browser sends a target pose. backend/control_loop.py proposes joint positions, bounds the step, checks the resulting geometry, and commits it. Telemetry sends committed joint positions back to the viewer.

The configured loop is 50 Hz, with a 0.02 second simulation step. The server sleeps before each tick, so this is a nominal rate, not a measured scheduling guarantee. I do not have timing benchmarks in this repo.

A few decisions shaped the rest of the build. The server is authoritative: the browser only proposes a target, and the backend decides what the arm actually does. Inbound messages are validated against pydantic models, and the server times everything with its own monotonic clock rather than trusting client timestamps. The frontend has no bundler or build step. It uses vendored copies of three.js and urdf-loader in web/vendor/, wired up with an import map, so the page works offline from a plain static mount. That choice had a cost: the vendored three.js 0.186 pulls in extra files such as three.core.js and the Collada loader's helpers, and they all had to be committed for the page to load.

Describe the elbow before solving it

The model has six revolute joints and one fixed joint ending at tool0. The upper arm and forearm visual cylinders are 0.30 and 0.25 meters long.

This is the elbow definition from arm/arm.urdf:

  <joint name="joint_3" type="revolute">
    <parent link="upper_arm_link"/>
    <child link="forearm_link"/>
    <origin xyz="0 0 0.30" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="0" upper="2.6180" velocity="2.5" effort="100"/>
  </joint>

The joint origin places it at the end of the upper arm. Its axis is local Y, and its angle is constrained to 0 through 2.6180 radians. The velocity value is radians per second. The URDF also contains effort values, but this simulation does not solve forces or torques.

backend/urdf_model.py deliberately supports a narrow model: revolute and fixed joints, with no branching or mimic joints. Forward kinematics walks the file's joint order, multiplying each origin transform and joint rotation. It is built for this serial chain rather than arbitrary robot trees.

Turn a target pose into joint motion

Build the error and Jacobian

In backend/kinematics.py, each Jacobian column uses the joint axis expressed in world coordinates. Its linear part is the cross product of that axis with the vector from joint origin to tool. Its angular part is the axis itself.

Position error is target minus current position. Orientation error comes from the relative rotation, with separate handling near zero and near pi. I scale both the angular error and angular Jacobian rows by a characteristic length of 0.3. This gives rotation a deliberate weight beside translation.

Dampen the solve near singularities

The update is damped least squares: dq = J_s^T (J_s J_s^T + lambda^2 I)^-1 e. Here J_s is the scaled Jacobian and e is the combined error. The actual implementation in backend/kinematics.py uses a linear solve:

            s = np.linalg.svd(j_full, compute_uv=False)
            sigma_min = float(s[-1]) if len(s) else 0.0
            if sigma_min < 0.01:
                near_sing = True
                lam = 0.05 + 2.0 * (0.01 - sigma_min)
            else:
                lam = 0.05
            last_lambda = lam
            max_lambda = max(max_lambda, lam)

            a = j_scaled @ j_scaled.T + (lam ** 2) * np.eye(6)
            dq = j_scaled.T @ np.linalg.solve(a, e)

Damping starts at 0.05 and increases when the smallest singular value drops below 0.01. These are implementation settings, not measured accuracy results.

The solver suppresses outward motion for joints already at a limit, caps the largest update at 0.5 radians, and tries up to five successively halved steps to reduce error. Three stalled iterations stop the search. Default convergence tolerances are 0.002 meters and 0.01 radians, with at most 50 iterations.

The control loop seeds each solve from committed joints. If it does not converge, the loop reports UNREACHABLE and keeps the previous configuration. That status can also mean this local solver failed, rather than proving no solution exists.

Bound a step before committing it

A converged IK solution is only a candidate. velocity_bound in backend/safety.py scales the joint displacement against URDF velocity limits. It then evaluates tool translation and rotation over the step, halving the displacement up to eight times against configured limits of 1 meter per second and 2 radians per second.

The live-motion checks in backend/control_loop.py are:

        q_bounded = velocity_bound(self.kin, self.q_committed, q_cand, dt, self.velocity_limits)
        q_bounded, lim_rej = apply_joint_limit_clamp(q_bounded, self.joint_limits)
        if lim_rej:
            target_status = "REJECTED_LIMITS"

        fk_b = self.kin.forward(q_bounded)
        if not in_envelope(fk_b.position):
            self._publish_state("REJECTED_ENVELOPE", ik_converged, ik_iterations, ik_pos_err, ik_ori_err, ik_near_sing)
            return
        if check_keepout(self.kin, q_bounded, config.DEFAULT_KEEPOUT_ZONES):
            self._publish_state("REJECTED_KEEPOUT", ik_converged, ik_iterations, ik_pos_err, ik_ori_err, ik_near_sing)
            return

        self._commit(q_bounded, dt, target_status, ik_converged, ik_iterations, ik_pos_err, ik_ori_err, ik_near_sing)

Joint clamping can still produce a committed step with REJECTED_LIMITS status. Envelope and keep-out failures return before committing. The envelope checks the tool position against configured bounds, not every point on the arm.

Keep-out checks approximate links as segments with a 0.04 meter capsule radius and 0.03 meter clearance. Sphere checks use segment-to-center distance. Box checks sample endpoints and midpoint against an expanded box. That sampling can miss intersections, and there is no self-collision check. These are simulation guardrails, not a rated safety system.

Render the URDF and move the gizmo

In web/viewer.js, a root group rotates by negative pi over two around X to display the arm's Z-up coordinates in the three.js scene. The robot and target share that group. The loader is small:

  let robot = null;
  const loader = new URDFLoader();
  loader.load('/arm/arm.urdf', (r) => {
    robot = r;
    zUpRoot.add(robot);
  });

Incoming joint telemetry looks up each named robot joint and calls setJointValue. I render accepted joint state rather than a browser-side IK prediction.

The target is a wireframe sphere. This excerpt from web/viewer.js attaches a translation gizmo and disables camera orbit during dragging:

  const transform = new TransformControls(camera, renderer.domElement);
  transform.attach(targetMesh);
  transform.setMode('translate');
  scene.add(transform.getHelper());
  transform.addEventListener('dragging-changed', (event) => {
    orbit.enabled = !event.value;
  });

web/app.js sends the target's local position and quaternion in base_link. The gizmo only translates; the UI does not expose an orientation rotation mode. End-effector telemetry also resets the target marker, so requested and accepted poses do not remain separate visual objects. That is one interaction I would improve next.

Record joint trajectories, then recheck them

I record committed joint positions rather than rerunning Cartesian IK during replay. Each JSONL file starts with this header from backend/recorder.py:

        header = {
            "schema_version": 1,
            "joint_names": self.joint_names,
            "urdf_sha256": self.urdf_sha256(),
            "tick_dt": config.CONTROL_DT,
        }

Rows contain time, joint positions, and safety mode. Recording happens inside _commit, so held or rejected ticks do not automatically create samples. Files stop growing at the configured 10 MiB limit.

Replay requires HOLD and rejects a different URDF hash. It advances through joint samples, retaining each target until reached within tolerance. It reapplies velocity bounds, joint clamping, envelope checks, and keep-out checks. Recorded timestamps are not used for playback timing, and replay bypasses the live deadman lease. I would strengthen format validation and replay control semantics before extending this demo.

Run it and inspect the checks

The README quickstart is:

make install
make run

Use a Python environment plus Node/npm. Installation reads requirements.txt and runs npm install. Open localhost on port 8000, request control, then hold Shift while nudging the target or dragging the gizmo. Stop recording and release Shift before requesting replay from HOLD.

The README also lists make test, make smoke, and make docker-build. The 20 pytest tests cover URDF parsing, fixture FK, a finite-difference Jacobian comparison, IK round trips, damping, state transitions, safety rejections, and recording. The IK assertions use selected fixture poses, not an exhaustive reachability survey.

One safety test sends an unreachable target, ticks the loop, and then checks the committed state directly, from tests/test_safety.py:

    fk = loop.kin.forward(loop.q_committed)
    # An unreachable target must not drag the arm far or out of the safe envelope.
    assert np.linalg.norm(fk.position - loop.kin.forward(q0).position) < 0.5
    assert in_envelope(fk.position)

The Playwright smoke test loads the page, waits for the URDF to produce at least six links, fails on console errors, and checks that joint telemetry changes after a target nudge through test hooks. GitHub Actions runs Python, browser, and Docker smoke jobs, and all three pass.

Some things are honestly untested. The smoke test does not cross-check the browser's rendered tool transform against the backend's forward kinematics; it only proves the arm moves. Gamepad input uses the browser Gamepad API and was never tried with a physical controller. The safety tests cover the core state transitions rather than every row of the original plan's table. None of this is hardware validation.

Keep exploring

I would start improvements with collision checking, replay validation, and separate requested-pose visualization. The repo contains the model, solver, viewer, and tests described here. For the shared browser control patterns, see the browser teleop console tutorial.