"""
Build a self-contained torso + arm + Shadow-Hand scene for demo2 pick-and-place.
The /demo2 Shadow Hand is fixed-base (forearm bolted to the world), so it can
only grasp in place. This grafts the hand onto a purpose-built torso+arm (kept
independent of the RX1 stack, per the demo2 design) so it can reach, pick from a
table, carry, and place on a pad. Uses MuJoCo's MjSpec.attach to merge the hand
(assets/tendons/actuators) onto a wrist frame at the end of the arm.
Run: python demo2/build_arm_scene.py -> model/scene_arm.xml
"""
from __future__ import annotations
from pathlib import Path
import mujoco
HERE = Path(__file__).resolve().parent
HAND_XML = HERE / "model" / "right_hand.xml"
OUT_MJB = HERE / "model" / "scene_arm.mjb"
# Arm: a self-contained chain. Fixed base column on the floor; waist yaw; a
# shoulder (pitch+roll); upper arm; elbow; forearm; then a wrist mount frame
# oriented so the attached hand points DOWN at the table (palm-down picking).
# The hand model lies along +x in its own frame (forearm root -> fingertips),
# so the mount frame rotates +x_hand -> -z_world (point down).
ARM_XML = """
"""
def build_model():
"""Compose torso+arm + Shadow Hand into a single compiled MjModel.
Returns the compiled model. This is the single source of truth — MjSpec's
XML round-trip mangles the merged blocks (attach artifact), so the
runtime loads the compiled model (saved as .mjb) rather than re-parsed XML.
"""
arm = mujoco.MjSpec.from_string(ARM_XML)
hand = mujoco.MjSpec.from_file(str(HAND_XML))
# attach the hand at the wrist mount frame (merges hand bodies/assets/
# tendons/actuators). The hand already uses an rh_ prefix on everything.
mount = arm.frame("wrist_mount")
arm.attach(hand, frame=mount, prefix="")
# Lighten the Shadow-Hand bodies: the stock forearm is 3 kg, far too heavy
# for this demo arm — the position servos can't hold a bent reach pose under
# that cantilever load (the arm falls short of commanded angles). Scale all
# hand body masses/inertias down so the arm can track IK targets.
for b in arm.bodies:
if b.name.startswith("rh_") and b.mass > 0:
b.mass *= 0.15
b.inertia = [v * 0.15 for v in b.inertia]
# grasp site at the palm centre + weld for holding the object during carry.
palm = arm.body("rh_palm")
# site at the measured closed-finger enclosure centre (palm-local frame), so
# IK driving the site onto the object seats it where the fingers actually close.
palm.add_site(name="grasp", pos=[0.009, -0.061, 0.078], size=[0.008, 0.008, 0.008],
rgba=[0, 1, 0, 0.4])
weld = arm.add_equality()
weld.name = "grasp_weld"
weld.type = mujoco.mjtEq.mjEQ_WELD
weld.name1 = "rh_palm"
weld.name2 = "object"
weld.objtype = mujoco.mjtObj.mjOBJ_BODY
weld.active = False
return arm.compile()
def main():
model = build_model()
mujoco.mj_saveModel(model, str(OUT_MJB))
print(f"[build_arm_scene] compiled nq={model.nq} nu={model.nu} "
f"nbody={model.nbody} -> {OUT_MJB.name}")
if __name__ == "__main__":
main()