""" build_scene.py — Turn the raw compiled RX1 model into a fully-dynamic scene. All 22 URDF revolute joints stay articulated. Each group gets a position servo tuned for its load: Torso (3 DoF) kp=1000 — heavy, high-inertia spine Head (5 DoF) kp=500 — lighter neck + ear gimbal Arms (7+7) kp=300 — IK-driven manipulation Fingers(9+9) kp=120 — light grasp actuators (added programmatically) Source: humanoid_torso/rx1.urdf → convert_urdf.py → rx1_compiled.xml Run: python rx1_mujoco/build_scene.py Produces: rx1_mujoco/rx1_scene.xml """ from __future__ import annotations import pathlib import xml.etree.ElementTree as ET import mujoco HERE = pathlib.Path(__file__).resolve().parent SRC = HERE / "rx1_compiled.xml" OUT = HERE / "rx1_scene.xml" # ── joint groups with per-group servo gains ─────────────────────────────────── TORSO = [ "base2torso_yaw_joint", "torso_yaw2pitch_joint", "torso_pitch2roll_joint", ] HEAD = [ "head_base2neck_yaw_joint", "neck_yaw2pitch_joint", "neck_pitch2head_depth_cam_mount_joint", "head_depth_cam_mount2right_ear_joint", "head_depth_cam_mount2left_ear_joint", ] RIGHT_ARM = [ "right_shoul_base2shoul_joint", "right_shoul2shoul_rot_joint", "right_arm2armrot_joint", "right_armrot2elbow_joint", "right_forearm2forearmrot_joint", "right_forearmrot2forearm_pitch_joint", "right_forearm_pitch2forearm_roll_joint", ] LEFT_ARM = [j.replace("right_", "left_") for j in RIGHT_ARM] # kp per group. Pure-P position servos droop under gravity by τ_gravity/kp, so # the arm gain is raised to 700 to keep steady-state droop small (the 5 Hz IK # replan also closes the loop on the actual hand position). KP = { "torso": 1200.0, "head": 500.0, "arm": 700.0, "finger": 120.0, } # Force limits (N·m) cap actuator torque. They must be high enough to HOLD each # link against gravity (the shoulder carries the whole arm's weight moment, so # 80 N·m saturates and the arm droops ~0.4 rad) yet a runaway slam is already # prevented by the slew-rate limit in env/rx1_env.py — so we can size these for # holding torque, not for flail-avoidance. FORCE = { "torso": 600.0, "head": 60.0, "arm": 250.0, "finger": 20.0, } # Critical-damping ratio for the position servos. MuJoCo computes the matching # velocity-feedback gain (kv) per actuator from kp and the joint's effective # inertia, so dampratio=1 gives a critically-damped servo: it reaches the target # without overshoot or ringing. This — not a hand-tuned global damping — is what # removes the jitter when kp is high. >1 is slightly over-damped (extra stable). DAMPRATIO = 1.2 DAMP = 1.0 # small residual joint damping (dampratio supplies the main D) ARMATURE = 0.05 # reflected motor inertia — smooths high-kp servo response # ── manipulable scene objects ───────────────────────────────────────────────── OBJECTS = [ ("red_cube", "box", "0.022 0.022 0.022", "0.85 0.2 0.2 1", "0.31 -0.05 0.445"), ("blue_cube", "box", "0.022 0.022 0.022", "0.42 0.35 0.9 1", "0.31 -0.10 0.445"), ] BOX = dict(pos=(0.375, -0.085, 0.43), half=0.05, wall_h=0.03) def _add_fingers(root: ET.Element, side: str) -> None: """Add articulated 5-finger hand to _hand_link.""" body = root.find(f".//body[@name='{side}_hand_link']") if body is None: return ET.SubElement(body, "site", name=f"{side}_grasp", pos="0 0.015 -0.05", size="0.008", rgba="0 1 0 0.4") knuckle_x = [-0.028, -0.010, 0.008, 0.026] for i, x in enumerate(knuckle_x): f = ET.SubElement(body, "body", name=f"{side}_finger{i}", pos=f"{x} 0.02 -0.03") ET.SubElement(f, "joint", name=f"{side}_finger{i}", type="hinge", axis="1 0 0", range="0 1.5", damping="0.2") ET.SubElement(f, "geom", type="capsule", fromto="0 0 0 0 0 -0.035", size="0.007", rgba="0.3 0.3 0.35 1", contype="0", conaffinity="0") d2 = ET.SubElement(f, "body", name=f"{side}_finger{i}_tip", pos="0 0 -0.035") ET.SubElement(d2, "joint", name=f"{side}_finger{i}_tip", type="hinge", axis="1 0 0", range="0 1.5", damping="0.2") ET.SubElement(d2, "geom", type="capsule", fromto="0 0 0 0 0 -0.03", size="0.006", rgba="0.3 0.3 0.35 1", contype="0", conaffinity="0") th = ET.SubElement(body, "body", name=f"{side}_thumb", pos="0.04 0.015 -0.01") ET.SubElement(th, "joint", name=f"{side}_thumb", type="hinge", axis="0 1 0", range="0 1.4", damping="0.2") ET.SubElement(th, "geom", type="capsule", fromto="0 0 0 -0.035 0 -0.02", size="0.008", rgba="0.3 0.3 0.35 1", contype="0", conaffinity="0") def _joint_group(name: str) -> str: if name in set(TORSO): return "torso" if name in set(HEAD): return "head" if name in set(RIGHT_ARM) or name in set(LEFT_ARM): return "arm" return "finger" # finger joints (added programmatically) def _joint_kp(name: str) -> str: return str(KP[_joint_group(name)]) def _joint_force(name: str) -> str: f = FORCE[_joint_group(name)] return f"{-f} {f}" # ── collision groups (contype / conaffinity bitmasks) ───────────────────────── # Two geoms collide iff (contypeA & conaffinityB) or (contypeB & conaffinityA). # Bits: robot=1, world(floor/table)=2, object(cube/box)=4. # ROBOT : contype=1 conaffinity=6 -> hits world+object, NOT itself (no self-collision) # WORLD : contype=2 conaffinity=5 -> hits robot+object # OBJECT : contype=4 conaffinity=7 -> hits everything (rests on table, stacks) # Disabling robot self-collision is essential now that all 40 joints are dynamic: # the URDF link meshes overlap at the joints and would generate dozens of phantom # contacts that kick the servos and make the robot flail. Grasping is weld-based # (see env/rx1_env.py _set_weld), so true finger-contact physics isn't needed. COL_ROBOT = {"contype": "1", "conaffinity": "6"} COL_WORLD = {"contype": "2", "conaffinity": "5"} COL_OBJECT = {"contype": "4", "conaffinity": "7"} def main() -> None: tree = ET.parse(SRC) root = tree.getroot() # Tag every existing (robot link) geom so the robot cannot self-collide. # These are the URDF mesh geoms; floor/table/objects/fingers are added later. for g in root.find("worldbody").iter("geom"): g.set("contype", COL_ROBOT["contype"]) g.set("conaffinity", COL_ROBOT["conaffinity"]) # ── simulation options ──────────────────────────────────────────────────── opt = ET.Element("option", timestep="0.002", gravity="0 0 -9.81", integrator="implicitfast", iterations="100", ls_iterations="50") root.insert(0, opt) visual = ET.Element("visual") ET.SubElement(visual, "global", offwidth="896", offheight="512") root.insert(0, visual) default = ET.Element("default") ET.SubElement(default, "joint", damping=str(DAMP), armature=str(ARMATURE)) ET.SubElement(default, "position", kp=str(KP["arm"]), dampratio=str(DAMPRATIO)) root.insert(1, default) # ── visual assets ───────────────────────────────────────────────────────── asset = root.find("asset") ET.SubElement(asset, "texture", name="sky", type="skybox", builtin="gradient", rgb1="0.3 0.5 0.7", rgb2="0 0 0", width="512", height="512") ET.SubElement(asset, "texture", name="grid", type="2d", builtin="checker", rgb1="0.2 0.3 0.4", rgb2="0.1 0.15 0.2", width="512", height="512") ET.SubElement(asset, "material", name="grid", texture="grid", texrepeat="6 6", reflectance="0.1") # ── worldbody: floor, light, overview camera ────────────────────────────── world = root.find("worldbody") ET.SubElement(world, "light", name="top", pos="0 0 3", dir="0 0 -1", diffuse="0.8 0.8 0.8", specular="0.2 0.2 0.2") ET.SubElement(world, "geom", name="floor", type="plane", size="5 5 0.1", material="grid", pos="0 0 0", **COL_WORLD) ET.SubElement(world, "camera", name="overview", pos="1.5 -0.6 1.1", mode="targetbody", target="torso_link", fovy="55") # ── table + objects ─────────────────────────────────────────────────────── ET.SubElement(asset, "material", name="wood", rgba="0.7 0.5 0.3 1") table = ET.SubElement(world, "body", name="table", pos="0.36 0 0.40") ET.SubElement(table, "geom", name="table_top", type="box", size="0.22 0.4 0.02", material="wood", **COL_WORLD) ET.SubElement(table, "geom", name="table_leg", type="box", size="0.02 0.02 0.20", pos="0 0 -0.20", material="wood", **COL_WORLD) for name, gtype, size, rgba, pos in OBJECTS: ET.SubElement(asset, "material", name=f"mat_{name}", rgba=rgba) b = ET.SubElement(world, "body", name=name, pos=pos) ET.SubElement(b, "freejoint", name=f"{name}_free") ET.SubElement(b, "geom", name=f"{name}_geom", type=gtype, size=size, material=f"mat_{name}", mass="0.1", friction="1 0.05 0.001", **COL_OBJECT) # Open-top container ET.SubElement(asset, "material", name="mat_box", rgba="0.55 0.4 0.25 1") bx, by, bz = BOX["pos"] h, wh = BOX["half"], BOX["wall_h"] box = ET.SubElement(world, "body", name="box", pos=f"{bx} {by} {bz}") ET.SubElement(box, "geom", name="box_floor", type="box", size=f"{h} {h} 0.005", material="mat_box", **COL_OBJECT) for wx, wy, sx, sy in [(h, 0, 0.005, h), (-h, 0, 0.005, h), (0, h, h, 0.005), (0, -h, h, 0.005)]: ET.SubElement(box, "geom", type="box", size=f"{sx} {sy} {wh}", pos=f"{wx} {wy} {wh}", material="mat_box", **COL_OBJECT) # ── ego camera on head ──────────────────────────────────────────────────── cam_body = root.find(".//body[@name='camera_link']") if cam_body is None: cam_body = root.find(".//body[@name='head_depth_cam_mount_link']") ET.SubElement(cam_body, "camera", name="ego", pos="0.02 0 0", xyaxes="0 -1 0 0.819 0 0.574", fovy="75") # ── articulated fingers on BOTH hands ───────────────────────────────────── _add_fingers(root, "right") _add_fingers(root, "left") # ── weld constraints + contact excludes ─────────────────────────────────── eq = ET.SubElement(root, "equality") contact = ET.SubElement(root, "contact") for name, *_ in OBJECTS: ET.SubElement(eq, "weld", name=f"weld_{name}", body1="right_hand_link", body2=name, active="false", solref="0.02 1") ET.SubElement(contact, "exclude", body1="right_hand_link", body2=name) ET.SubElement(contact, "exclude", body1="left_hand_link", body2=name) ET.SubElement(contact, "exclude", body1="right_hand_link", body2="box") ET.SubElement(contact, "exclude", body1="left_hand_link", body2="box") # ── actuators: position servo on EVERY joint ────────────────────────────── # ALL joints stay articulated — no welding. The PostureController (or IK # policy) commands each group; gains are tuned per group inertia class. hinge_joints = [ (j.get("name"), j.get("range")) for j in root.iter("joint") if j.get("type", "hinge") == "hinge" and j.get("name") ] actuator = ET.SubElement(root, "actuator") for jname, rng in hinge_joints: attrs = { "name": f"act_{jname}", "joint": jname, "kp": _joint_kp(jname), "dampratio": str(DAMPRATIO), "forcerange": _joint_force(jname), } if rng: attrs["ctrlrange"] = rng ET.SubElement(actuator, "position", **attrs) ET.indent(tree, space=" ") tree.write(OUT, encoding="unicode", xml_declaration=False) print(f"[build_scene] wrote {OUT}") m = mujoco.MjModel.from_xml_path(str(OUT)) print(f"[build_scene] OK nq={m.nq} nu={m.nu} ncam={m.ncam} ngeom={m.ngeom}") # Print actuator list so IK code can be updated print("[build_scene] actuators:") for i in range(m.nu): print(f" [{i:2d}] {mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_ACTUATOR, i)}") if __name__ == "__main__": main()