cohnt · September 1, 2026 14:43
diff --git a/mesh_vs_limits_upstream.py b/mesh_vs_limits_upstream.py
 #!/usr/bin/env python3
 """Check every declared revolute joint limit in the RB-Y1 description against
 the link geometry that joint moves.

 A joint limit that reaches past the angle at which the two links either side of
 it physically interfere is invisible to everything downstream. Links that share
 a joint are filtered from collision checking in *any* collision checker --
 correctly, since they are in contact at the bearing in every configuration -- so
 the joint limit is the only thing that can keep them apart. A wrong limit
 therefore propagates straight through IK, sampling-based planning, trajectory
 optimisation and any dense feasibility check with nothing firing, and the first
 thing that notices is the robot.

 This script is standalone. It clones RainbowRobotics/rby1-sdk beside itself,
 sweeps the models inside that checkout, and deletes the clone again when it is
 done. Nothing else is needed:

    pip install numpy scipy trimesh pycollada
    python rby1_joint_limit_audit.py --markdown

 How it works
 ------------
 For a revolute joint the parent and child geometry differ only by a rotation
 about the joint axis, so the question is one-dimensional. Both visual meshes are
 placed in the joint frame with the axis aligned to +y, the joint is swept across
 its declared range, and the minimum surface distance between dense surface
 samples is measured at each angle. Contact is ``--touch`` metres or less, and
 the first contacting angle is then bisected to convergence.

 Adjacent links touch at the shared bearing collar at *every* angle, which would
 make a naive test report contact at 0 rad for every joint. The collar is
 therefore removed before measuring: a sphere about the joint centre for
 hinge-like joints, or a cylinder about the axis for coaxial (roll) joints,
 whichever first leaves the two links clear of each other in the neutral pose.
 The radius that achieved that is reported, so the exclusion is never silent.

 Limitations, stated so results are not over-read:
  * Visual meshes only. They are the best available description in this
    repository, but they are not the hardware, and covers or cabling are not in
    them. Treat a reported contact angle as an upper bound on the usable travel,
    and leave margin below it.
  * Only joint-adjacent pairs are examined. Interference between links two or
    more joints apart is a collision checker's job, and collision checkers do
    check those.
  * Surface sampling is stochastic. Repeat runs of the same model move a
    reported contact angle by a couple of milliradians; raise ``--samples`` to
    tighten that.
  * A joint whose links cannot be separated by either exclusion shape is
    reported UNRESOLVED rather than cleared, and a model whose meshes are not
    present in the repository is reported as such rather than skipped silently.

 Usage
 -----
    rby1_joint_limit_audit.py                       # rby1a, current model
    rby1_joint_limit_audit.py --markdown            # table for a bug report
    rby1_joint_limit_audit.py --all                 # every variant, one clone
    rby1_joint_limit_audit.py --list                # what variants exist
    rby1_joint_limit_audit.py --model rby1m/urdf/model_v1.3.urdf
    rby1_joint_limit_audit.py --keep                # leave the clone in place
    rby1_joint_limit_audit.py --samples 300000      # slower, tighter

 Exits nonzero if any joint's declared range reaches into mesh contact, so it can
 be used as a check rather than only as a report.
 """

 import argparse
 import json
 import os
 import re
 import shutil
 import subprocess
 import sys

 import numpy as np
 import trimesh
 from scipy.spatial import cKDTree

 HERE = os.path.dirname(os.path.abspath(__file__))
 UPSTREAM_URL = "https://github.com/RainbowRobotics/rby1-sdk.git"
 CLONE = os.path.join(HERE, "rby1-sdk")
 DEFAULT_MODEL = "rby1a/urdf/model.urdf"

 # Directory the current model's mesh paths resolve against; set per model.
 MESHES = None

 # Exclusion radii tried in order. The smallest one that separates the links in
 # the neutral pose wins, so the collar is removed and no more.
 R_CANDIDATES = (0.015, 0.02, 0.025, 0.03, 0.035, 0.04, 0.05, 0.06,
                0.07, 0.08, 0.10, 0.12, 0.15, 0.18, 0.22)
 # Neutral-pose separation that says the exclusion has cleared the collar. Below
 # this the two links are still touching and the sweep would measure the bearing.
 CLEAR_M = 0.015


 def _rpy(r, p, y):
    cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y)
    return (np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]])
            @ np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]])
            @ np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]]))


 def _T(xyz, rot):
    m = np.eye(4)
    m[:3, :3] = rot
    m[:3, 3] = xyz
    return m


 def _origin_of(fragment):
    xyz, rot = np.zeros(3), np.eye(3)
    o = re.search(r"<origin([^/>]*)/>", fragment)
    if o:
        a = re.search(r'xyz="([^"]+)"', o.group(1))
        b = re.search(r'rpy="([^"]+)"', o.group(1))
        if a:
            xyz = np.fromstring(a.group(1), sep=" ")
        if b:
            rot = _rpy(*np.fromstring(b.group(1), sep=" "))
    return _T(xyz, rot)


 def parse_urdf(path):
    """-> ({link: (mesh_file, X_link_visual)}, [joint dicts]) for revolute joints."""
    text = open(path).read()
    links = {}
    for m in re.finditer(r'<link name="([^"]+)"(.*?)</link>', text, re.S):
        vis = re.search(r"<visual>(.*?)</visual>", m.group(2), re.S)
        if not vis:
            continue
        f = re.search(r'filename="[^"]*/([^"/]+)"', vis.group(1))
        if f:
            links[m.group(1)] = (f.group(1), _origin_of(vis.group(1)))

    joints = []
    for m in re.finditer(r'<joint name="(\w+)" type="revolute">(.*?)</joint>', text, re.S):
        name, body = m.groups()
        parent = re.search(r'<parent link="([^"]+)"', body)
        child = re.search(r'<child link="([^"]+)"', body)
        lim = re.search(r'<limit[^>]*lower="([-\d.e]+)"[^>]*upper="([-\d.e]+)"', body)
        axis = re.search(r'<axis xyz="([^"]+)"', body)
        if not (parent and child and lim and axis):
            continue
        a = np.fromstring(axis.group(1), sep=" ")
        joints.append({
            "name": name, "parent": parent.group(1), "child": child.group(1),
            "origin": _origin_of(body), "axis": a / np.linalg.norm(a),
            "lower": float(lim.group(1)), "upper": float(lim.group(2)),
        })
    return links, joints


 _MESH_CACHE = {}


 def surface_points(mesh_file, X, n):
    if mesh_file not in _MESH_CACHE:
        _MESH_CACHE[mesh_file] = trimesh.load(
            os.path.join(MESHES, mesh_file), force="mesh", process=False)
    mesh = _MESH_CACHE[mesh_file]
    pts, _ = trimesh.sample.sample_surface(mesh, n)
    # Vertices as well as samples: sampling is area-weighted and can miss a small
    # protruding feature, which is exactly the kind of thing that sets a limit.
    pts = np.vstack([np.asarray(pts), np.asarray(mesh.vertices)])
    return (X[:3, :3] @ pts.T).T + X[:3, 3]


 def align_to_y(axis):
    """Rotation taking the joint axis onto +y, so a joint angle q is a rotation
    about +y and cylindrical phi maps to phi + q."""
    y = np.array([0.0, 1.0, 0.0])
    if np.allclose(axis, y):
        return np.eye(3)
    if np.allclose(axis, -y):
        return np.diag([1.0, -1.0, -1.0])
    v = np.cross(axis, y)
    s = np.linalg.norm(v)
    c = float(axis @ y)
    k = np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])
    return np.eye(3) + k + k @ k * ((1.0 - c) / s ** 2)


 def _roty(q):
    c, s = np.cos(q), np.sin(q)
    return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])


 def choose_exclusion(A, B):
    """Remove the shared bearing collar. Returns (label, tree_over_A, B_points)."""
    sph_a, sph_b = np.linalg.norm(A, axis=1), np.linalg.norm(B, axis=1)
    cyl_a = np.hypot(A[:, 0], A[:, 2])
    cyl_b = np.hypot(B[:, 0], B[:, 2])
    why = "links never separate at any exclusion radius"
    for tag, ka, kb in (("sphere", sph_a, sph_b), ("cylinder", cyl_a, cyl_b)):
        for r in R_CANDIDATES:
            a, b = A[ka > r], B[kb > r]
            if len(a) < 300 or len(b) < 300:
                why = f"{tag}: link too small, the exclusion consumes it above r={r:.3f}"
                break
            if cKDTree(a).query(b, k=1, workers=-1)[0].min() > CLEAR_M:
                return f"{tag} r={r:.3f}", cKDTree(a), b
    return None, why, None


 def sweep(joint, links, n_samples, n_angles, touch):
    F = align_to_y(joint["axis"])
    X_parent = _T(np.zeros(3), F) @ np.linalg.inv(joint["origin"]) @ links[joint["parent"]][1]
    X_child = _T(np.zeros(3), F) @ links[joint["child"]][1]
    A = surface_points(links[joint["parent"]][0], X_parent, n_samples)
    B = surface_points(links[joint["child"]][0], X_child, n_samples)

    label, tree, b = choose_exclusion(A, B)
    if label is None:
        return {"status": "unresolved", "why": tree}

    qs = np.linspace(joint["lower"], joint["upper"], n_angles)
    d = np.array([tree.query((_roty(q) @ b.T).T, k=1, workers=-1)[0].min() for q in qs])

    def touches(q):
        return tree.query((_roty(q) @ b.T).T, k=1, workers=-1)[0].min() <= touch

    # The two bounds are searched independently. A joint can interfere at BOTH
    # ends -- the stock RB-Y1 torso_2 does -- and treating the contact set as one
    # interval mixes the two up and reports nonsense.
    i0 = int(np.argmin(np.abs(qs)))          # grid point nearest the neutral pose
    if d[i0] <= touch:
        return {"status": "unresolved",
                "why": "links are already in contact at the neutral pose; the "
                       "exclusion did not isolate the collar"}

    contacts = []
    for side, order in (("lower", range(i0, -1, -1)), ("upper", range(i0, len(qs)))):
        idx = [k for k in order if d[k] <= touch]
        if not idx:
            continue
        first = idx[0]                       # nearest contacting grid point to neutral
        clear = float(qs[first + 1] if side == "lower" else qs[first - 1])
        contact = float(qs[first])
        for _ in range(20):
            mid = 0.5 * (clear + contact)
            if touches(mid):
                contact = mid
            else:
                clear = mid
        contacts.append({"side": side, "contact_q": contact,
                         "overshoot_rad": float(abs(joint[side] - contact))})

    if not contacts:
        return {"status": "clear", "exclusion": label,
                "min_gap_m": float(d.min()), "min_gap_q": float(qs[int(np.argmin(d))])}
    return {"status": "contact", "exclusion": label, "contacts": contacts,
            "usable_range": [contacts[0]["contact_q"] if contacts[0]["side"] == "lower"
                             else joint["lower"],
                             contacts[-1]["contact_q"] if contacts[-1]["side"] == "upper"
                             else joint["upper"]]}



 def _git(*argv):
    r = subprocess.run(["git", "-C", CLONE, *argv], capture_output=True, text=True)
    return r.stdout.strip() if r.returncode == 0 else None


 def warn_if_behind():
    """Report drift against the remote. Never move someone's checkout."""
    if _git("fetch", "--quiet", "origin") is None:
        print("[warn] could not fetch rby1-sdk; the checkout may be out of date")
        return
    head = _git("rev-parse", "HEAD")
    remote = _git("rev-parse", "origin/HEAD") or _git("rev-parse", "origin/main")
    if not head or not remote or head == remote:
        return
    behind = _git("rev-list", "--count", f"{head}..{remote}") or "?"
    print(f"[warn] rby1-sdk is at {head[:7]}, origin at {remote[:7]} ({behind} behind).\n"
          "       This sweep describes the checkout as it stands and does not move it.")


 def ensure_clone(may_fetch):
    if not os.path.isdir(CLONE):
        print(f"[clone] {UPSTREAM_URL} -> {os.path.basename(CLONE)}", flush=True)
        if subprocess.run(["git", "clone", UPSTREAM_URL, CLONE]).returncode != 0:
            sys.exit(f"could not clone {UPSTREAM_URL} (it is public; check the network)")
    elif may_fetch:
        warn_if_behind()
    sha = _git("rev-parse", "HEAD") or "unknown"
    print(f"rby1-sdk @ {sha}")
    return sha


 def remove_clone():
    """Delete the checkout this run used.

    Guarded on the directory actually being the clone -- an ``origin`` pointing
    at rby1-sdk -- so a mistyped ``CLONE``, or a directory somebody put there
    for their own reasons, is left alone rather than removed.
    """
    if not os.path.isdir(CLONE):
        return
    origin = _git("remote", "get-url", "origin")
    if origin is None or "rby1-sdk" not in origin:
        print(f"[warn] {os.path.basename(CLONE)} does not look like the "
              f"rby1-sdk clone (origin: {origin}); leaving it alone")
        return
    shutil.rmtree(CLONE, ignore_errors=True)
    print(f"[clean] removed {os.path.basename(CLONE)}")


 def list_models():
    root = os.path.join(CLONE, "models")
    out = []
    for dirpath, _, files in os.walk(root):
        for f in sorted(files):
            if f.endswith(".urdf"):
                out.append(os.path.relpath(os.path.join(dirpath, f), root))
    return sorted(out)


 def markdown(rows, touch):
    lines = ["| joint | declared (rad) | verdict |", "|---|---|---|"]
    for r in rows:
        rng = f"[{r['lower']:+.4f}, {r['upper']:+.4f}]"
        if r["status"] == "contact":
            where = "; ".join(
                f"contact at {c['contact_q']:+.4f} — the {c['side']} bound "
                f"overshoots it by {c['overshoot_rad'] * 1000:.0f} mrad"
                for c in r["contacts"])
            lo, hi = r["usable_range"]
            body = f"**{where}. Usable [{lo:+.4f}, {hi:+.4f}]**"
            name = f"**{r['joint']}**"
        elif r["status"] == "clear":
            body = f"clear ({r['min_gap_m'] * 1000:.1f} mm at q={r['min_gap_q']:+.4f})"
            name = r["joint"]
        elif r["status"] == "skipped":
            body = f"not checked — {r.get('why', 'unavailable')}"
            name = r["joint"]
        else:
            body = f"unresolved — {r['why']}"
            name = r["joint"]
        lines.append(f"| {name} | {rng} | {body} |")
    lines.append("")
    lines.append(f"Contact is a surface separation of {touch * 1000:.0f} mm or less "
                 "between the two links a joint moves, measured with the shared "
                 "bearing collar excluded.")
    return "\n".join(lines)


 def resolve_meshes(links, urdf_path):
    """Repoint each link's mesh at its path as the URDF writes it.

    ``parse_urdf`` keeps only the mesh basename, which would be enough if every
    mesh lived in one directory. The variants each carry their own ``meshes/``
    beside the URDF, and one of them (``leader_arm``) references a mesh
    directory that is not in the repository at all. Resolving the filename as
    written, relative to the URDF, keeps the variants from borrowing each
    other's geometry and turns the missing set into a reported skip instead of
    a crash.

    Returns the links whose mesh file does not exist.
    """
    text = open(urdf_path).read()
    missing = set()
    for m in re.finditer(r'<link name="([^"]+)"(.*?)</link>', text, re.S):
        name = m.group(1)
        if name not in links:
            continue
        vis = re.search(r"<visual>(.*?)</visual>", m.group(2), re.S)
        f = re.search(r'filename="([^"]+)"', vis.group(1)) if vis else None
        if not f:
            continue
        rel = re.sub(r"^package://[^/]+/", "", f.group(1))
        links[name] = (rel, links[name][1])
        if not os.path.exists(os.path.join(MESHES, rel)):
            missing.add(name)
    return missing


 def sweep_model(rel, sha, args):
    """Sweep every revolute joint of one variant. Returns (rows, n_contact)."""
    path = os.path.join(CLONE, "models", rel)
    if not os.path.exists(path):
        sys.exit(f"no such model: {path}\n(try --list)")

    # Meshes resolve relative to the variant's own URDF. The mesh cache is
    # keyed on that path, which repeats across variants that reuse a filename
    # for different geometry, so it has to be cleared per model or the second
    # variant silently measures the first one's links.
    global MESHES
    MESHES = os.path.dirname(path)
    _MESH_CACHE.clear()

    links, joints = parse_urdf(path)
    missing = resolve_meshes(links, path)
    if missing:
        print(f"  (no mesh file for {len(missing)} link(s), e.g. "
              f"{sorted(missing)[0]}: {links[sorted(missing)[0]][0]})")
    print(f"\n{'=' * 96}\nmodels/{rel} @ {sha[:7]}\n{'=' * 96}")
    if not joints:
        print("  (no revolute joints)")
        return [], 0

    rows, n_contact = [], 0
    for j in joints:
        rng = f"[{j['lower']:+.4f}, {j['upper']:+.4f}]"
        if j["parent"] not in links or j["child"] not in links:
            print(f"  {j['name']:14s} {rng}  SKIPPED (no visual mesh on one side)")
            r = {"status": "skipped", "why": "no visual mesh on one side"}
        elif j["parent"] in missing or j["child"] in missing:
            print(f"  {j['name']:14s} {rng}  SKIPPED (mesh file not present in the repository)")
            r = {"status": "skipped", "why": "mesh file not present in the repository"}
        else:
            r = sweep(j, links, args.samples, args.angles, args.touch)
            if r["status"] == "contact":
                n_contact += 1
                where = "; ".join(
                    f"{c['side']} bound at {c['contact_q']:+.4f}, overshoots by "
                    f"{c['overshoot_rad'] * 1000:.0f} mrad" for c in r["contacts"])
                print(f"  {j['name']:14s} {rng}  {r['exclusion']:<16s} *** CONTACT: {where} "
                      f"-> usable [{r['usable_range'][0]:+.4f}, "
                      f"{r['usable_range'][1]:+.4f}] ***")
            elif r["status"] == "clear":
                print(f"  {j['name']:14s} {rng}  {r['exclusion']:<16s} clear "
                      f"(min gap {r['min_gap_m'] * 1000:6.1f} mm at q={r['min_gap_q']:+.4f})")
            else:
                print(f"  {j['name']:14s} {rng}  UNRESOLVED ({r['why']})")
        r.update({"model": rel, "sha": sha, "joint": j["name"],
                  "lower": j["lower"], "upper": j["upper"]})
        rows.append(r)
    return rows, n_contact


 def main():
    ap = argparse.ArgumentParser(
        description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--model", default=DEFAULT_MODEL,
                    help=f"URDF under rby1-sdk/models/ (default {DEFAULT_MODEL})")
    ap.add_argument("--all", action="store_true",
                    help="sweep every variant, from the one clone")
    ap.add_argument("--list", action="store_true", help="list the variants and exit")
    ap.add_argument("--keep", action="store_true",
                    help="leave the clone in place instead of removing it afterwards")
    ap.add_argument("--no-fetch", action="store_true",
                    help="do not contact the remote to check for drift")
    ap.add_argument("--markdown", action="store_true",
                    help="also print the results as a markdown table")
    ap.add_argument("--samples", type=int, default=120_000,
                    help="surface samples per link (default 120000)")
    ap.add_argument("--angles", type=int, default=46, help="sweep resolution (default 46)")
    ap.add_argument("--touch", type=float, default=0.001,
                    help="separation counted as contact, metres (default 0.001)")
    ap.add_argument("--json", metavar="PATH")
    args = ap.parse_args()

    try:
        sha = ensure_clone(not args.no_fetch)
        if args.list:
            for m in list_models():
                print(f"  {m}")
            return 0

        models = list_models() if args.all else [args.model]
        all_rows, n_contact, flagged, errors = [], 0, [], []
        for rel in models:
            try:
                rows, n = sweep_model(rel, sha, args)
            except Exception as e:                      # noqa: BLE001
                print(f"  ERROR: {type(e).__name__}: {e}")
                errors.append((rel, f"{type(e).__name__}: {e}"))
                continue
            all_rows += rows
            n_contact += n
            if n:
                flagged.append((rel, n))

        if args.json:
            with open(args.json, "w") as f:
                json.dump(all_rows, f, indent=2)
            print(f"\nwrote {args.json}")

        if args.markdown:
            for rel in models:
                rows = [r for r in all_rows if r["model"] == rel]
                if not rows:
                    continue
                print(f"\n### models/{rel} @ {sha[:7]}\n")
                print(markdown(rows, args.touch))

        print()
        for rel, msg in errors:
            print(f"  models/{rel}: not swept -- {msg}")
        if n_contact:
            for rel, n in flagged:
                print(f"  models/{rel}: {n} joint(s)")
            print(f"FAIL: {n_contact} joint(s) across {len(flagged)} model(s) can be "
                  f"commanded into mesh contact within their declared limits.")
            return 1
        print("OK: no declared joint limit reaches mesh contact between its own two links.")
        return 0
    finally:
        if args.keep:
            print(f"[keep] {os.path.basename(CLONE)} left in place")
        else:
            remove_clone()


 if __name__ == "__main__":
    sys.exit(main())
	#!/usr/bin/env python3
	"""Check every declared revolute joint limit in the RB-Y1 description against
	the link geometry that joint moves.

	A joint limit that reaches past the angle at which the two links either side of
	it physically interfere is invisible to everything downstream. Links that share
	a joint are filtered from collision checking in any collision checker --
	correctly, since they are in contact at the bearing in every configuration -- so
	the joint limit is the only thing that can keep them apart. A wrong limit
	therefore propagates straight through IK, sampling-based planning, trajectory
	optimisation and any dense feasibility check with nothing firing, and the first
	thing that notices is the robot.

	This script is standalone. It clones RainbowRobotics/rby1-sdk beside itself,
	sweeps the models inside that checkout, and deletes the clone again when it is
	done. Nothing else is needed:

	pip install numpy scipy trimesh pycollada
	python rby1_joint_limit_audit.py --markdown

	How it works
	------------
	For a revolute joint the parent and child geometry differ only by a rotation
	about the joint axis, so the question is one-dimensional. Both visual meshes are
	placed in the joint frame with the axis aligned to +y, the joint is swept across
	its declared range, and the minimum surface distance between dense surface
	samples is measured at each angle. Contact is ``--touch`` metres or less, and
	the first contacting angle is then bisected to convergence.

	Adjacent links touch at the shared bearing collar at every angle, which would
	make a naive test report contact at 0 rad for every joint. The collar is
	therefore removed before measuring: a sphere about the joint centre for
	hinge-like joints, or a cylinder about the axis for coaxial (roll) joints,
	whichever first leaves the two links clear of each other in the neutral pose.
	The radius that achieved that is reported, so the exclusion is never silent.

	Limitations, stated so results are not over-read:
	* Visual meshes only. They are the best available description in this
	repository, but they are not the hardware, and covers or cabling are not in
	them. Treat a reported contact angle as an upper bound on the usable travel,
	and leave margin below it.
	* Only joint-adjacent pairs are examined. Interference between links two or
	more joints apart is a collision checker's job, and collision checkers do
	check those.
	* Surface sampling is stochastic. Repeat runs of the same model move a
	reported contact angle by a couple of milliradians; raise ``--samples`` to
	tighten that.
	* A joint whose links cannot be separated by either exclusion shape is
	reported UNRESOLVED rather than cleared, and a model whose meshes are not
	present in the repository is reported as such rather than skipped silently.

	Usage
	-----
	rby1_joint_limit_audit.py # rby1a, current model
	rby1_joint_limit_audit.py --markdown # table for a bug report
	rby1_joint_limit_audit.py --all # every variant, one clone
	rby1_joint_limit_audit.py --list # what variants exist
	rby1_joint_limit_audit.py --model rby1m/urdf/model_v1.3.urdf
	rby1_joint_limit_audit.py --keep # leave the clone in place
	rby1_joint_limit_audit.py --samples 300000 # slower, tighter

	Exits nonzero if any joint's declared range reaches into mesh contact, so it can
	be used as a check rather than only as a report.
	"""

	import argparse
	import json
	import os
	import re
	import shutil
	import subprocess
	import sys

	import numpy as np
	import trimesh
	from scipy.spatial import cKDTree

	HERE = os.path.dirname(os.path.abspath(__file__))
	UPSTREAM_URL = "https://github.com/RainbowRobotics/rby1-sdk.git"
	CLONE = os.path.join(HERE, "rby1-sdk")
	DEFAULT_MODEL = "rby1a/urdf/model.urdf"

	# Directory the current model's mesh paths resolve against; set per model.
	MESHES = None

	# Exclusion radii tried in order. The smallest one that separates the links in
	# the neutral pose wins, so the collar is removed and no more.
	R_CANDIDATES = (0.015, 0.02, 0.025, 0.03, 0.035, 0.04, 0.05, 0.06,
	0.07, 0.08, 0.10, 0.12, 0.15, 0.18, 0.22)
	# Neutral-pose separation that says the exclusion has cleared the collar. Below
	# this the two links are still touching and the sweep would measure the bearing.
	CLEAR_M = 0.015


	def _rpy(r, p, y):
	cr, sr, cp, sp, cy, sy = np.cos(r), np.sin(r), np.cos(p), np.sin(p), np.cos(y), np.sin(y)
	return (np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]])
	@ np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]])
	@ np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]]))


	def _T(xyz, rot):
	m = np.eye(4)
	m[:3, :3] = rot
	m[:3, 3] = xyz
	return m


	def _origin_of(fragment):
	xyz, rot = np.zeros(3), np.eye(3)
	o = re.search(r"<origin([^/>]*)/>", fragment)
	if o:
	a = re.search(r'xyz="([^"]+)"', o.group(1))
	b = re.search(r'rpy="([^"]+)"', o.group(1))
	if a:
	xyz = np.fromstring(a.group(1), sep=" ")
	if b:
	rot = _rpy(*np.fromstring(b.group(1), sep=" "))
	return _T(xyz, rot)


	def parse_urdf(path):
	"""-> ({link: (mesh_file, X_link_visual)}, [joint dicts]) for revolute joints."""
	text = open(path).read()
	links = {}
	for m in re.finditer(r'<link name="([^"]+)"(.*?)</link>', text, re.S):
	vis = re.search(r"<visual>(.*?)</visual>", m.group(2), re.S)
	if not vis:
	continue
	f = re.search(r'filename="[^"]*/([^"/]+)"', vis.group(1))
	if f:
	links[m.group(1)] = (f.group(1), _origin_of(vis.group(1)))

	joints = []
	for m in re.finditer(r'<joint name="(\w+)" type="revolute">(.*?)</joint>', text, re.S):
	name, body = m.groups()
	parent = re.search(r'<parent link="([^"]+)"', body)
	child = re.search(r'<child link="([^"]+)"', body)
	lim = re.search(r'<limit[^>]lower="([-\d.e]+)"[^>]upper="([-\d.e]+)"', body)
	axis = re.search(r'<axis xyz="([^"]+)"', body)
	if not (parent and child and lim and axis):
	continue
	a = np.fromstring(axis.group(1), sep=" ")
	joints.append({
	"name": name, "parent": parent.group(1), "child": child.group(1),
	"origin": _origin_of(body), "axis": a / np.linalg.norm(a),
	"lower": float(lim.group(1)), "upper": float(lim.group(2)),
	})
	return links, joints


	_MESH_CACHE = {}


	def surface_points(mesh_file, X, n):
	if mesh_file not in _MESH_CACHE:
	_MESH_CACHE[mesh_file] = trimesh.load(
	os.path.join(MESHES, mesh_file), force="mesh", process=False)
	mesh = _MESH_CACHE[mesh_file]
	pts, _ = trimesh.sample.sample_surface(mesh, n)
	# Vertices as well as samples: sampling is area-weighted and can miss a small
	# protruding feature, which is exactly the kind of thing that sets a limit.
	pts = np.vstack([np.asarray(pts), np.asarray(mesh.vertices)])
	return (X[:3, :3] @ pts.T).T + X[:3, 3]


	def align_to_y(axis):
	"""Rotation taking the joint axis onto +y, so a joint angle q is a rotation
	about +y and cylindrical phi maps to phi + q."""
	y = np.array([0.0, 1.0, 0.0])
	if np.allclose(axis, y):
	return np.eye(3)
	if np.allclose(axis, -y):
	return np.diag([1.0, -1.0, -1.0])
	v = np.cross(axis, y)
	s = np.linalg.norm(v)
	c = float(axis @ y)
	k = np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])
	return np.eye(3) + k + k @ k * ((1.0 - c) / s ** 2)


	def _roty(q):
	c, s = np.cos(q), np.sin(q)
	return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])


	def choose_exclusion(A, B):
	"""Remove the shared bearing collar. Returns (label, tree_over_A, B_points)."""
	sph_a, sph_b = np.linalg.norm(A, axis=1), np.linalg.norm(B, axis=1)
	cyl_a = np.hypot(A[:, 0], A[:, 2])
	cyl_b = np.hypot(B[:, 0], B[:, 2])
	why = "links never separate at any exclusion radius"
	for tag, ka, kb in (("sphere", sph_a, sph_b), ("cylinder", cyl_a, cyl_b)):
	for r in R_CANDIDATES:
	a, b = A[ka > r], B[kb > r]
	if len(a) < 300 or len(b) < 300:
	why = f"{tag}: link too small, the exclusion consumes it above r={r:.3f}"
	break
	if cKDTree(a).query(b, k=1, workers=-1)[0].min() > CLEAR_M:
	return f"{tag} r={r:.3f}", cKDTree(a), b
	return None, why, None


	def sweep(joint, links, n_samples, n_angles, touch):
	F = align_to_y(joint["axis"])
	X_parent = _T(np.zeros(3), F) @ np.linalg.inv(joint["origin"]) @ links[joint["parent"]][1]
	X_child = _T(np.zeros(3), F) @ links[joint["child"]][1]
	A = surface_points(links[joint["parent"]][0], X_parent, n_samples)
	B = surface_points(links[joint["child"]][0], X_child, n_samples)

	label, tree, b = choose_exclusion(A, B)
	if label is None:
	return {"status": "unresolved", "why": tree}

	qs = np.linspace(joint["lower"], joint["upper"], n_angles)
	d = np.array([tree.query((_roty(q) @ b.T).T, k=1, workers=-1)[0].min() for q in qs])

	def touches(q):
	return tree.query((_roty(q) @ b.T).T, k=1, workers=-1)[0].min() <= touch

	# The two bounds are searched independently. A joint can interfere at BOTH
	# ends -- the stock RB-Y1 torso_2 does -- and treating the contact set as one
	# interval mixes the two up and reports nonsense.
	i0 = int(np.argmin(np.abs(qs))) # grid point nearest the neutral pose
	if d[i0] <= touch:
	return {"status": "unresolved",
	"why": "links are already in contact at the neutral pose; the "
	"exclusion did not isolate the collar"}

	contacts = []
	for side, order in (("lower", range(i0, -1, -1)), ("upper", range(i0, len(qs)))):
	idx = [k for k in order if d[k] <= touch]
	if not idx:
	continue
	first = idx[0] # nearest contacting grid point to neutral
	clear = float(qs[first + 1] if side == "lower" else qs[first - 1])
	contact = float(qs[first])
	for _ in range(20):
	mid = 0.5 * (clear + contact)
	if touches(mid):
	contact = mid
	else:
	clear = mid
	contacts.append({"side": side, "contact_q": contact,
	"overshoot_rad": float(abs(joint[side] - contact))})

	if not contacts:
	return {"status": "clear", "exclusion": label,
	"min_gap_m": float(d.min()), "min_gap_q": float(qs[int(np.argmin(d))])}
	return {"status": "contact", "exclusion": label, "contacts": contacts,
	"usable_range": [contacts[0]["contact_q"] if contacts[0]["side"] == "lower"
	else joint["lower"],
	contacts[-1]["contact_q"] if contacts[-1]["side"] == "upper"
	else joint["upper"]]}



	def _git(*argv):
	r = subprocess.run(["git", "-C", CLONE, *argv], capture_output=True, text=True)
	return r.stdout.strip() if r.returncode == 0 else None


	def warn_if_behind():
	"""Report drift against the remote. Never move someone's checkout."""
	if _git("fetch", "--quiet", "origin") is None:
	print("[warn] could not fetch rby1-sdk; the checkout may be out of date")
	return
	head = _git("rev-parse", "HEAD")
	remote = _git("rev-parse", "origin/HEAD") or _git("rev-parse", "origin/main")
	if not head or not remote or head == remote:
	return
	behind = _git("rev-list", "--count", f"{head}..{remote}") or "?"
	print(f"[warn] rby1-sdk is at {head[:7]}, origin at {remote[:7]} ({behind} behind).\n"
	" This sweep describes the checkout as it stands and does not move it.")


	def ensure_clone(may_fetch):
	if not os.path.isdir(CLONE):
	print(f"[clone] {UPSTREAM_URL} -> {os.path.basename(CLONE)}", flush=True)
	if subprocess.run(["git", "clone", UPSTREAM_URL, CLONE]).returncode != 0:
	sys.exit(f"could not clone {UPSTREAM_URL} (it is public; check the network)")
	elif may_fetch:
	warn_if_behind()
	sha = _git("rev-parse", "HEAD") or "unknown"
	print(f"rby1-sdk @ {sha}")
	return sha


	def remove_clone():
	"""Delete the checkout this run used.

	Guarded on the directory actually being the clone -- an ``origin`` pointing
	at rby1-sdk -- so a mistyped ``CLONE``, or a directory somebody put there
	for their own reasons, is left alone rather than removed.
	"""
	if not os.path.isdir(CLONE):
	return
	origin = _git("remote", "get-url", "origin")
	if origin is None or "rby1-sdk" not in origin:
	print(f"[warn] {os.path.basename(CLONE)} does not look like the "
	f"rby1-sdk clone (origin: {origin}); leaving it alone")
	return
	shutil.rmtree(CLONE, ignore_errors=True)
	print(f"[clean] removed {os.path.basename(CLONE)}")


	def list_models():
	root = os.path.join(CLONE, "models")
	out = []
	for dirpath, _, files in os.walk(root):
	for f in sorted(files):
	if f.endswith(".urdf"):
	out.append(os.path.relpath(os.path.join(dirpath, f), root))
	return sorted(out)


	def markdown(rows, touch):
	lines = ["\| joint \| declared (rad) \| verdict \|", "\|---\|---\|---\|"]
	for r in rows:
	rng = f"[{r['lower']:+.4f}, {r['upper']:+.4f}]"
	if r["status"] == "contact":
	where = "; ".join(
	f"contact at {c['contact_q']:+.4f} — the {c['side']} bound "
	f"overshoots it by {c['overshoot_rad'] * 1000:.0f} mrad"
	for c in r["contacts"])
	lo, hi = r["usable_range"]
	body = f"{where}. Usable [{lo:+.4f}, {hi:+.4f}]"
	name = f"{r['joint']}"
	elif r["status"] == "clear":
	body = f"clear ({r['min_gap_m'] * 1000:.1f} mm at q={r['min_gap_q']:+.4f})"
	name = r["joint"]
	elif r["status"] == "skipped":
	body = f"not checked — {r.get('why', 'unavailable')}"
	name = r["joint"]
	else:
	body = f"unresolved — {r['why']}"
	name = r["joint"]
	lines.append(f"\| {name} \| {rng} \| {body} \|")
	lines.append("")
	lines.append(f"Contact is a surface separation of {touch * 1000:.0f} mm or less "
	"between the two links a joint moves, measured with the shared "
	"bearing collar excluded.")
	return "\n".join(lines)


	def resolve_meshes(links, urdf_path):
	"""Repoint each link's mesh at its path as the URDF writes it.

	``parse_urdf`` keeps only the mesh basename, which would be enough if every
	mesh lived in one directory. The variants each carry their own ``meshes/``
	beside the URDF, and one of them (``leader_arm``) references a mesh
	directory that is not in the repository at all. Resolving the filename as
	written, relative to the URDF, keeps the variants from borrowing each
	other's geometry and turns the missing set into a reported skip instead of
	a crash.

	Returns the links whose mesh file does not exist.
	"""
	text = open(urdf_path).read()
	missing = set()
	for m in re.finditer(r'<link name="([^"]+)"(.*?)</link>', text, re.S):
	name = m.group(1)
	if name not in links:
	continue
	vis = re.search(r"<visual>(.*?)</visual>", m.group(2), re.S)
	f = re.search(r'filename="([^"]+)"', vis.group(1)) if vis else None
	if not f:
	continue
	rel = re.sub(r"^package://[^/]+/", "", f.group(1))
	links[name] = (rel, links[name][1])
	if not os.path.exists(os.path.join(MESHES, rel)):
	missing.add(name)
	return missing


	def sweep_model(rel, sha, args):
	"""Sweep every revolute joint of one variant. Returns (rows, n_contact)."""
	path = os.path.join(CLONE, "models", rel)
	if not os.path.exists(path):
	sys.exit(f"no such model: {path}\n(try --list)")

	# Meshes resolve relative to the variant's own URDF. The mesh cache is
	# keyed on that path, which repeats across variants that reuse a filename
	# for different geometry, so it has to be cleared per model or the second
	# variant silently measures the first one's links.
	global MESHES
	MESHES = os.path.dirname(path)
	_MESH_CACHE.clear()

	links, joints = parse_urdf(path)
	missing = resolve_meshes(links, path)
	if missing:
	print(f" (no mesh file for {len(missing)} link(s), e.g. "
	f"{sorted(missing)[0]}: {links[sorted(missing)[0]][0]})")
	print(f"\n{'=' * 96}\nmodels/{rel} @ {sha[:7]}\n{'=' * 96}")
	if not joints:
	print(" (no revolute joints)")
	return [], 0

	rows, n_contact = [], 0
	for j in joints:
	rng = f"[{j['lower']:+.4f}, {j['upper']:+.4f}]"
	if j["parent"] not in links or j["child"] not in links:
	print(f" {j['name']:14s} {rng} SKIPPED (no visual mesh on one side)")
	r = {"status": "skipped", "why": "no visual mesh on one side"}
	elif j["parent"] in missing or j["child"] in missing:
	print(f" {j['name']:14s} {rng} SKIPPED (mesh file not present in the repository)")
	r = {"status": "skipped", "why": "mesh file not present in the repository"}
	else:
	r = sweep(j, links, args.samples, args.angles, args.touch)
	if r["status"] == "contact":
	n_contact += 1
	where = "; ".join(
	f"{c['side']} bound at {c['contact_q']:+.4f}, overshoots by "
	f"{c['overshoot_rad'] * 1000:.0f} mrad" for c in r["contacts"])
	print(f" {j['name']:14s} {rng} {r['exclusion']:<16s} *** CONTACT: {where} "
	f"-> usable [{r['usable_range'][0]:+.4f}, "
	f"{r['usable_range'][1]:+.4f}] ***")
	elif r["status"] == "clear":
	print(f" {j['name']:14s} {rng} {r['exclusion']:<16s} clear "
	f"(min gap {r['min_gap_m'] * 1000:6.1f} mm at q={r['min_gap_q']:+.4f})")
	else:
	print(f" {j['name']:14s} {rng} UNRESOLVED ({r['why']})")
	r.update({"model": rel, "sha": sha, "joint": j["name"],
	"lower": j["lower"], "upper": j["upper"]})
	rows.append(r)
	return rows, n_contact


	def main():
	ap = argparse.ArgumentParser(
	description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
	ap.add_argument("--model", default=DEFAULT_MODEL,
	help=f"URDF under rby1-sdk/models/ (default {DEFAULT_MODEL})")
	ap.add_argument("--all", action="store_true",
	help="sweep every variant, from the one clone")
	ap.add_argument("--list", action="store_true", help="list the variants and exit")
	ap.add_argument("--keep", action="store_true",
	help="leave the clone in place instead of removing it afterwards")
	ap.add_argument("--no-fetch", action="store_true",
	help="do not contact the remote to check for drift")
	ap.add_argument("--markdown", action="store_true",
	help="also print the results as a markdown table")
	ap.add_argument("--samples", type=int, default=120_000,
	help="surface samples per link (default 120000)")
	ap.add_argument("--angles", type=int, default=46, help="sweep resolution (default 46)")
	ap.add_argument("--touch", type=float, default=0.001,
	help="separation counted as contact, metres (default 0.001)")
	ap.add_argument("--json", metavar="PATH")
	args = ap.parse_args()

	try:
	sha = ensure_clone(not args.no_fetch)
	if args.list:
	for m in list_models():
	print(f" {m}")
	return 0

	models = list_models() if args.all else [args.model]
	all_rows, n_contact, flagged, errors = [], 0, [], []
	for rel in models:
	try:
	rows, n = sweep_model(rel, sha, args)
	except Exception as e: # noqa: BLE001
	print(f" ERROR: {type(e).__name__}: {e}")
	errors.append((rel, f"{type(e).__name__}: {e}"))
	continue
	all_rows += rows
	n_contact += n
	if n:
	flagged.append((rel, n))

	if args.json:
	with open(args.json, "w") as f:
	json.dump(all_rows, f, indent=2)
	print(f"\nwrote {args.json}")

	if args.markdown:
	for rel in models:
	rows = [r for r in all_rows if r["model"] == rel]
	if not rows:
	continue
	print(f"\n### models/{rel} @ {sha[:7]}\n")
	print(markdown(rows, args.touch))

	print()
	for rel, msg in errors:
	print(f" models/{rel}: not swept -- {msg}")
	if n_contact:
	for rel, n in flagged:
	print(f" models/{rel}: {n} joint(s)")
	print(f"FAIL: {n_contact} joint(s) across {len(flagged)} model(s) can be "
	f"commanded into mesh contact within their declared limits.")
	return 1
	print("OK: no declared joint limit reaches mesh contact between its own two links.")
	return 0
	finally:
	if args.keep:
	print(f"[keep] {os.path.basename(CLONE)} left in place")
	else:
	remove_clone()


	if __name__ == "__main__":
	sys.exit(main())
No results found