"""Usability metric: sample N random whole-arm configurations within joint limits, report the fraction
that are self-collision-free (servo-servo or bridge-through-other-servo). A serial arm naturally folds
into itself when one proximal joint is swept alone; random whole-arm poses are the realistic test.
"""
import sys
import numpy as np
import xml.etree.ElementTree as ET
from so100_check import check, D

urdf = "so100_para_connected.urdf"
N = int(sys.argv[1]) if len(sys.argv) > 1 else 20
root = ET.parse(D + urdf).getroot()
lims = [(j.get("name"), float(j.find("limit").get("lower")), float(j.find("limit").get("upper")))
        for j in root.findall("joint") if j.get("type") == "revolute"]
rng = np.random.default_rng(0)
free = 0
for i in range(N):
    qd = {n: float(rng.uniform(lo, hi)) for n, lo, hi in lims}
    sp, bs = check(urdf, qd)
    if not sp and not bs:
        free += 1
print(f"{free}/{N} random whole-arm configs self-collision-free ({100*free/N:.0f}%)")