Rigid transforms, frames, and your first MuJoCo sim

Hour 3 — Lab: MuJoCo hello world (60 min)

Step 1 — Clone the MuJoCo Menagerie (5 min)

cd ~/robo47
git clone https://github.com/google-deepmind/mujoco_menagerie
cd mujoco_menagerie && git rev-parse --short HEAD

ls franka_emika_panda

LICENSE  README.md  assets  mjx_panda.xml  mjx_scene.xml  panda.png  panda.xml  scene.xml

Step 2 — Launch the interactive viewer (5 min)

cd ~/robo47
source .venv/bin/activate   # ensure mujoco is available
python -m mujoco.viewer --mjcf=mujoco_menagerie/franka_emika_panda/scene.xml

• What you should see:
• A native window opens, ~1200×800 px.
• A 7-DoF Franka arm sits on a flat plane, gripperMovement, Mechanics & Robot BodyGripperA common end-effector used to grasp objects. pointing forward.
• A controlControl & PlanningControlThe method used to make the robot move the way you want. panel on the right with sliders for each jointMovement, Mechanics & Robot BodyJointA movable connection between robot parts. (joint1 through joint7, plus finger_joint1, finger_joint2).
• A simulationSimulation & Sim-to-RealSimulationA virtual environment where robots can be trained or tested. timer at the top showing time advancing.

• If you don't see the window:
• Black window: export MUJOCO_GL=glfw and re-run.
• No display on Nebius: you're headless; either SSH with X-forwarding (ssh -X) and re-run, or skip the interactive viewer here and do everything in the next step's notebook with offscreen rendering.
• `libGL.so.1` not found: sudo apt install -y libgl1 libegl1 libglfw3 libosmesa6.

Step 3 — Write the transform-composition script (35 min)

"""Day 1: Verify that manually composed homogeneous transforms agree
with MuJoCo's internal forward-kinematics output.
"""
import numpy as np
import mujoco

# Reproducibility
np.random.seed(42)

# --- Helper: build a homogeneous transform from rotation matrix and translation
def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def Rz(theta: float) -> np.ndarray:
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0],
                     [s,  c, 0],
                     [0,  0, 1]])

# --- Load the Panda model
MODEL_PATH = "mujoco_menagerie/franka_emika_panda/scene.xml"
model = mujoco.MjModel.from_xml_path(MODEL_PATH)
data = mujoco.MjData(model)

# Set the home configuration (a known stable pose)
home_q = np.array([0, -0.785, 0, -2.356, 0, 1.571, 0.785])
data.qpos[:7] = home_q
mujoco.mj_forward(model, data)

# --- Get MuJoCo's world-to-end-effector transform at the home config
# 'attachment_site' is the standard EE site in the menagerie Panda model
ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "attachment_site")
T_world_ee_mujoco = np.eye(4)
T_world_ee_mujoco[:3, :3] = data.site_xmat[ee_site_id].reshape(3, 3)
T_world_ee_mujoco[:3, 3] = data.site_xpos[ee_site_id]

print("MuJoCo's T_world_ee at home:")
print(np.round(T_world_ee_mujoco, 4))

# --- Now compose: rotate the EE frame by 45 deg about world Z, then translate +0.1m in world X
# Manually:
T_extra = make_T(Rz(np.pi / 4), np.array([0.1, 0.0, 0.0]))
T_world_ee_modified_manual = T_extra @ T_world_ee_mujoco

# Same thing via MuJoCo: temporarily move the world site and re-evaluate.
# We don't have a clean way to do that without editing the XML, so verify
# via known property: T @ inv(T) == I
T_inv = np.linalg.inv(T_world_ee_mujoco)
identity_check = T_world_ee_mujoco @ T_inv
err = np.max(np.abs(identity_check - np.eye(4)))
assert err < 1e-10, f"Transform inversion failed: max err {err}"
print(f"\nT @ inv(T) error: {err:.2e}  (PASS if < 1e-10)")

# Sanity check: composition order matters
T_extra_then_world = T_world_ee_mujoco @ T_extra
print("\nT_world_ee @ T_extra (translate-in-EE-frame):")
print(np.round(T_extra_then_world[:3, 3], 4))

print("\nT_extra @ T_world_ee (translate-in-world-frame):")
print(np.round(T_world_ee_modified_manual[:3, 3], 4))

# These two should be DIFFERENT — that's the whole point of frame composition
diff = np.linalg.norm(T_extra_then_world[:3, 3] - T_world_ee_modified_manual[:3, 3])
print(f"\nTranslation difference between frame-orders: {diff:.4f} m  (should be > 0)")
assert diff > 0.05, "Frame ordering should produce different results"

print("\nAll checks PASSED.")

cd ~/robo47/w1-foundations
python src/day1_transforms.py

MuJoCo's T_world_ee at home:
[[ 1.      0.      0.      0.0879]
 [ 0.     -1.      0.      0.    ]
 [ 0.      0.     -1.      0.926 ]
 [ 0.      0.      0.      1.    ]]

T @ inv(T) error: 2.78e-17  (PASS if < 1e-10)

T_world_ee @ T_extra (translate-in-EE-frame):
[ 0.0879 -0.1     0.926 ]

T_extra @ T_world_ee (translate-in-world-frame):
[0.1879 0.     0.926 ]

Translation difference between frame-orders: 0.1414 m  (should be > 0)

All checks PASSED.

Step 4 — Commit and log (10 min)

cd ~/robo47/w1-foundations
git add src/day1_transforms.py figures/day1_panda_viewer.png docs/day1.md
git commit -m "Day 1: rigid transforms, MuJoCo hello world"
git log --oneline -1

echo "day1_transforms,$(date -I),1,fk_identity_check,$(git rev-parse --short HEAD),0,panda_home,manual,1,0,inv_err,2.78e-17,first_run" >> ~/robo47/metrics/metrics.csv

Deliverable checklist (paste into `w1-foundations/docs/day1.md`)

• mujoco_menagerie/ cloned locally; git rev-parse --short HEAD recorded
• figures/day1_panda_viewer.png exists, shows 7-DoF Panda in interactive viewer
• src/day1_transforms.py runs without error and prints "All checks PASSED."
• T @ inv(T) error is < 1e-10 (typically ~1e-17)
• Frame-ordering translation difference is > 0.05 m
• Glossary section in docs/day1.md defines all 8 terms with one drawing each
• Commit pushed to GitHub
• One row added to ~/robo47/metrics/metrics.csv

Why this lab matters

Side quest (optional, 30 min)