Four-bar loop closure

43 · Mimic joints and loop closures

A crank-rocker four-bar built as a three-link chain plus one closing revolute joint outside the articulation tree. The closure becomes five bilateral rows (three point rows, two hinge-axis rows) solved in the same sweep as everything else, so its residual depends on the iteration count. enable_bilateral_preelimination projects the closure rows once per step before the sweep.

drag to orbit · scroll to zoom

0.00 s

What to look at

2 iterations: the loop opens by up to 16 cm.
12 iterations: under 6 mm.
2 iterations with pre-elimination: under 2 mm.

Key parameter

enable_bilateral_preelimination

Eliminates mimic and loop rows through a regularized Schur complement before the sweep. Up to eight bilateral rows per articulation.

Try changing

Turn the crank at 30 rad/s.
Use a BALL closure instead of REVOLUTE.

Concept

Loop closures and pre-elimination

What it is

A joint outside the tree closes a loop with rows: three point rows, plus two axis rows for a revolute or three rotation rows for a weld. Iterating them with everything else leaves a residual. enable_bilateral_preelimination projects them once per step through a regularized Schur complement before the sweep.

In this scene

At 2 iterations the four-bar opens by up to 165 mm; 12 iterations hold it to 5.5 mm and pre-elimination at 2 iterations to 1.9 mm.

Considerations

Cost: A small dense factorization per articulation; up to eight bilateral rows each.
Interactions: Disabled with velocity iterations, a kinematic or world parent, or more than eight rows; incompatible with joint friction.
Use it when: Linkages that must stay closed at low iteration counts.

The script

cd tutorials && python 43_loop_closure.py prints each variant's final metric; --record out.json also writes the replay. Every tutorial imports replay.py from the same folder.

43_loop_closure.py
replay.py

# SPDX-FileCopyrightText: Copyright (c) 2026 Skild AI and the Steel contributors
# SPDX-License-Identifier: Apache-2.0

"""Loop closures: a four-bar linkage as a chain plus one closing revolute joint."""

import math

import newton
import numpy as np
import warp as wp
from newton.solvers import SolverSteel

from replay import Recorder

DT = 1.0 / 240.0
SUBSTEPS = 8
CRANK, COUPLER, ROCKER, GROUND = 0.12, 0.42, 0.30, 0.36


def joints_xz():
    """Pivot points A, B, C, D of the linkage with the crank at 60 degrees."""
    a, d = np.array([0.0, 0.0, 0.6]), np.array([GROUND, 0.0, 0.6])
    b = a + CRANK * np.array([math.cos(math.radians(60)), 0.0, math.sin(math.radians(60))])
    dist = np.linalg.norm(d - b)
    along = (COUPLER**2 - ROCKER**2 + dist**2) / (2 * dist)
    e = (d - b) / dist
    c = b + along * e + math.sqrt(COUPLER**2 - along**2) * np.array([-e[2], 0.0, e[0]])
    return a, b, c, d


def bar(p0, p1):
    """Center, orientation (x along the bar) and half length of a bar from p0 to p1."""
    v = p1 - p0
    return (
        tuple((p0 + p1) / 2),
        wp.quat_from_axis_angle(wp.vec3(0.0, -1.0, 0.0), math.atan2(v[2], v[0])),
        float(np.linalg.norm(v)) / 2,
    )


def build():
    builder = newton.ModelBuilder()
    a, b, c, d = joints_xz()
    cfg = newton.ModelBuilder.ShapeConfig(density=600.0)
    base = builder.add_link()
    builder.add_shape_box(
        base,
        xform=wp.transform((GROUND / 2, 0.0, 0.6), wp.quat_identity()),
        hx=GROUND / 2 + 0.03,
        hy=0.005,
        hz=0.01,
        cfg=newton.ModelBuilder.ShapeConfig(density=0.0, has_shape_collision=False),
    )
    links = []
    for p0, p1, width in ((a, b, 0.012), (b, c, 0.009), (d, c, 0.012)):
        center, rot, half = bar(p0, p1)
        link = builder.add_link(xform=wp.transform(center, rot))
        builder.add_shape_box(link, hx=half, hy=width, hz=width, cfg=cfg)
        links.append((link, rot, half))
    (crank, qa, ha), (coupler, qb, hb), (rocker, qc, hc) = links
    builder.add_shape_sphere(
        rocker,
        xform=wp.transform((hc, 0.0, 0.0), wp.quat_identity()),
        radius=0.04,
        cfg=newton.ModelBuilder.ShapeConfig(density=6000.0, has_shape_collision=False),
    )
    tree = [
        builder.add_joint_fixed(-1, base),
        builder.add_joint_revolute(
            base,
            crank,
            axis=newton.Axis.Y,
            parent_xform=wp.transform(tuple(a), qa),
            child_xform=wp.transform((-ha, 0.0, 0.0), wp.quat_identity()),
            target_qd=15.0,
            target_kd=200.0,
        ),
        builder.add_joint_revolute(
            crank,
            coupler,
            axis=newton.Axis.Y,
            parent_xform=wp.transform((ha, 0.0, 0.0), wp.mul(wp.quat_inverse(qa), qb)),
            child_xform=wp.transform((-hb, 0.0, 0.0), wp.quat_identity()),
        ),
        builder.add_joint_revolute(
            coupler,
            rocker,
            axis=newton.Axis.Y,
            parent_xform=wp.transform((hb, 0.0, 0.0), wp.mul(wp.quat_inverse(qb), qc)),
            child_xform=wp.transform((hc, 0.0, 0.0), wp.quat_identity()),
        ),
    ]
    builder.add_articulation(tree)
    # the closing joint is not in the articulation: Steel solves it as five bilateral rows
    builder.add_joint_revolute(
        base,
        rocker,
        axis=newton.Axis.Y,
        parent_xform=wp.transform(tuple(d), qc),
        child_xform=wp.transform((-hc, 0.0, 0.0), wp.quat_identity()),
    )
    return builder.finalize(), rocker, hc, d


def simulate(rec, label, **options):
    model, rocker, half, d = build()
    solver = SolverSteel(model, **options)
    pipeline = newton.CollisionPipeline(model)
    contacts = pipeline.contacts()
    state_0, state_1, control = model.state(), model.state(), model.control()

    run = rec.variant(label, model)
    for _ in range(rec.frames(4.0)):
        pose = state_0.body_q.numpy()[rocker]
        end = wp.transform_point(
            wp.transform(wp.vec3(*pose[:3]), wp.quat(*pose[3:])), wp.vec3(-half, 0.0, 0.0)
        )
        run.frame(state_0, gap_mm=1000.0 * np.linalg.norm(np.array(end) - d))
        for _ in range(SUBSTEPS):
            pipeline.collide(state_0, contacts)
            solver.step(state_0, state_1, control, contacts, DT)
            state_0, state_1 = state_1, state_0
    run.check(solver)


if __name__ == "__main__":
    rec = Recorder()
    simulate(rec, "iterations=2", iterations=2)
    simulate(rec, "iterations=12 (default)")
    simulate(
        rec,
        "iterations=2, enable_bilateral_preelimination=True",
        iterations=2,
        enable_bilateral_preelimination=True,
    )
    rec.save()

# SPDX-FileCopyrightText: Copyright (c) 2026 Skild AI and the Steel contributors
# SPDX-License-Identifier: Apache-2.0

"""Record Steel tutorial runs as replay JSON for the tutorials page, or print a summary.

Every tutorial imports this module. ``python NN_name.py`` runs the variants and prints
the final metric of each; ``--record out.json`` also writes body poses, shapes and metric
series that the page replays in 3D.
"""

import argparse
import base64
import json
import os
import subprocess
import sys
import time

import newton
import numpy as np
import warp as wp

from source_hash import source_sha256

_RENDERED_TYPES = {
    newton.GeoType.PLANE,
    newton.GeoType.SPHERE,
    newton.GeoType.CAPSULE,
    newton.GeoType.ELLIPSOID,
    newton.GeoType.CYLINDER,
    newton.GeoType.BOX,
    newton.GeoType.MESH,
    newton.GeoType.CONE,
    newton.GeoType.CONVEX_MESH,
}


class Recorder:
    """Collect the variants of one tutorial; ``fps`` is the replay frame rate."""

    def __init__(self, fps: int = 30):
        parser = argparse.ArgumentParser()
        parser.add_argument(
            "--record", metavar="OUT.json", help="write a replay file for the tutorials page"
        )
        parser.add_argument("--device", default=None, help="Warp device, for example cuda:0")
        self.args = parser.parse_args()
        if self.args.device:
            wp.set_device(self.args.device)
        self.fps = fps
        self.script = os.path.abspath(sys.argv[0])
        self.variants: list[Variant] = []

    def frames(self, seconds: float) -> int:
        """Number of replay frames in ``seconds``, including the first."""
        return int(round(seconds * self.fps)) + 1

    def variant(
        self,
        label: str,
        model: newton.Model,
        *,
        ghost: bool = False,
        slot: int | None = None,
        scene: str | None = None,
    ) -> "Variant":
        """Start one run; ``ghost`` overlays run ``slot``, ``scene`` groups runs for charts."""
        variant = Variant(label, model, self.fps, ghost, slot, scene)
        self.variants.append(variant)
        return variant

    def save(self) -> None:
        """Print a summary and, with ``--record``, write the replay file."""
        for variant in self.variants:
            last = variant.metrics[-1] if variant.metrics else {}
            flag = "  (rows dropped)" if variant.overflow else ""
            print(f"{variant.label:<48} {json.dumps(last)}  {variant.wall:.2f} s{flag}")
        if not self.args.record:
            return
        source_sha = source_sha256(self.script)
        doc = {
            "script": os.path.basename(self.script),
            "script_sha256": source_sha,
            "newton_commit": _newton_commit(),
            "newton_version": newton.__version__,
            "warp_version": wp.config.version,
            "gpu": wp.get_device().name,
            "fps": self.fps,
            "variants": [v.to_json() for v in self.variants],
        }
        with open(self.args.record, "w") as f:
            json.dump(doc, f, separators=(",", ":"))
        print(f"wrote {self.args.record}")


class Variant:
    """One recorded run: a pose per frame for every body, plus named metric values."""

    def __init__(self, label, model, fps, ghost, slot, scene):
        self.label, self.model, self.fps, self.ghost, self.slot = label, model, fps, ghost, slot
        self.scene = scene
        self.poses: list[np.ndarray] = []
        self.metrics: list[dict] = []
        self.awake: list[list[int]] = []
        self.particles: list[np.ndarray] = []
        self.overflow = False
        self.start = time.perf_counter()
        self.wall = 0.0

    def frame(self, state: newton.State, awake=None, particles=None, **metrics) -> None:
        """Record ``state.body_q``, optional awake flags, particle positions and metric values."""
        self.poses.append(state.body_q.numpy().astype(np.float32).copy())
        if particles is not None:
            self.particles.append(particles.numpy().astype(np.float32).copy())
        self.metrics.append({k: _round(v) for k, v in metrics.items()})
        if awake is not None:
            self.awake.append([int(x) for x in awake.numpy()])
        self.wall = time.perf_counter() - self.start

    def check(self, solver) -> None:
        """Note whether the solver dropped rows; call once after the run."""
        try:
            solver.check_constraint_capacity()
        except RuntimeError:
            self.overflow = True

    def to_json(self) -> dict:
        poses = (
            np.stack(self.poses)
            if self.poses
            else np.zeros((0, self.model.body_count, 7), np.float32)
        )
        return {
            "label": self.label,
            "fps": self.fps,
            "ghost": self.ghost,
            "slot": self.slot,
            "scene": self.scene,
            "bodies": int(self.model.body_count),
            "frames": int(poses.shape[0]),
            "poses": base64.b64encode(poses.astype(np.float32).tobytes()).decode(),
            "shapes": _shape_table(self.model),
            "metric": self.metrics,
            "awake": self.awake or None,
            "particles": self._particles_json(),
            "wall_s": round(self.wall, 3),
            "overflow": self.overflow,
        }

    def _particles_json(self):
        if not self.particles:
            return None
        frames = np.stack(self.particles)
        tris = self.model.tri_indices
        radius = self.model.particle_radius.numpy()
        return {
            "count": int(frames.shape[1]),
            "frames": base64.b64encode(frames.astype(np.float32).tobytes()).decode(),
            "radius": round(float(radius.mean()), 5) if radius.size else 0.0,
            "tris": [int(v) for v in tris.numpy().reshape(-1)]
            if tris is not None and tris.shape[0]
            else None,
        }


def _shape_table(model):
    types = model.shape_type.numpy()
    scales = model.shape_scale.numpy()
    xforms = model.shape_transform.numpy()
    bodies = model.shape_body.numpy()
    flags = model.shape_flags.numpy()
    shapes = []
    for i, t in enumerate(types):
        if int(t) not in _RENDERED_TYPES or not flags[i] & int(newton.ShapeFlags.VISIBLE):
            continue
        shape = {
            "b": int(bodies[i]),
            "t": int(t),
            "s": [round(float(v), 5) for v in scales[i]],
            "x": [round(float(v), 6) for v in xforms[i]],
        }
        if int(t) in (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH):
            mesh = model.shape_source[i]
            shape["v"] = [round(float(v), 5) for v in np.asarray(mesh.vertices).reshape(-1)]
            shape["i"] = [int(v) for v in np.asarray(mesh.indices).reshape(-1)]
        shapes.append(shape)
    return shapes


def _round(value):
    value = float(value)
    return round(value, 5) if np.isfinite(value) else None


def _newton_commit():
    if os.environ.get("NEWTON_COMMIT"):
        return os.environ["NEWTON_COMMIT"]
    try:
        root = os.path.dirname(os.path.dirname(newton.__file__))
        return subprocess.check_output(["git", "-C", root, "rev-parse", "HEAD"], text=True).strip()
    except (OSError, subprocess.CalledProcessError):
        return None

Recorded with SolverSteel on Newton 1.9.0.dev0, Warp 1.19.0, NVIDIA RTX PRO 6000 Blackwell Server Edition. Replays read the state back every frame, so they are not a speed benchmark. Script · page text

Steel docs and tutorials are licensed under Apache-2.0. Third-party notices. "Skild", "Skild AI" and the Skild logo are trademarks of Skild AI and are not licensed under Apache-2.0.