Dynamics: Lagrangian, mass matrix, gravity compensation

Hour 3 — Lab: gravity-compensation + joint-space PD on the Panda (75 min)

Step 1 — Understand `mj_rne` (15 min)

• mj_rne(m, d, flg_acc, qfrc) writes jointMovement, Mechanics & Robot BodyJointA movable connection between robot parts. torques into qfrc such that, given current qpos and qvel:
• If flg_acc == 0: qfrc = C(q, q̇) q̇ + g(q) (just bias).
• If flg_acc == 1: qfrc = M(q) q̈ + C(q, q̇) q̇ + g(q) (full inverse dynamicsMovement, Mechanics & Robot BodyDynamicsThe study of motion including forces, torques, mass, and inertia.).

python -c "
import mujoco, numpy as np
m = mujoco.MjModel.from_xml_path('mujoco_menagerie/franka_emika_panda/scene.xml')
d = mujoco.MjData(m)
d.qpos[:7] = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
mujoco.mj_forward(m, d)
print('qfrc_bias[:7]:', np.round(d.qfrc_bias[:7], 3))
"

qfrc_bias[:7]: [ 0.    -19.59   0.    19.96   0.     1.36   0.   ]

Step 2 — Write the controller (35 min)

"""Day 4: Gravity compensation + joint-space PD on the Panda. Compare with and without."""
from __future__ import annotations
import numpy as np
import matplotlib.pyplot as plt
import mujoco

MODEL_PATH = "../mujoco_menagerie/franka_emika_panda/scene.xml"
SIM_TIME_S = 5.0

# Joints we'll control (the 7 arm joints). MuJoCo Panda has 7 arm joints + 2 gripper.
ARM_DOF = 7

# Home configuration
Q_HOME = np.array([0, -0.785, 0, -2.356, 0, 1.571, 0.785])

# PD gains (tuned roughly: ~2 Hz natural freq, critical damping)
KP = np.array([200, 200, 150, 150, 80, 80, 40], dtype=float)
KD = np.array([20, 20, 15, 15, 8, 8, 4], dtype=float)


def run_controller(use_grav_comp: bool, push_force: float = 1.0):
    """Run a 5-second sim. Optionally apply gravity comp. Push the EE for 1s starting at t=2s."""
    model = mujoco.MjModel.from_xml_path(MODEL_PATH)
    data = mujoco.MjData(model)

    # Disable any default control (we compute torque ourselves)
    data.qpos[:ARM_DOF] = Q_HOME.copy()
    data.qvel[:] = 0
    mujoco.mj_forward(model, data)

    # We'll apply torques via data.qfrc_applied (added to each step's force balance)
    n_steps = int(SIM_TIME_S / model.opt.timestep)
    err = np.zeros((n_steps, ARM_DOF))
    t_arr = np.zeros(n_steps)

    ee_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "hand")

    for k in range(n_steps):
        t = k * model.opt.timestep

        # PD around home
        q = data.qpos[:ARM_DOF].copy()
        qd = data.qvel[:ARM_DOF].copy()
        tau_pd = KP * (Q_HOME - q) - KD * qd

        # Gravity comp
        if use_grav_comp:
            tau_gc = data.qfrc_bias[:ARM_DOF].copy()
        else:
            tau_gc = np.zeros(ARM_DOF)

        # Apply torque
        data.qfrc_applied[:] = 0
        data.qfrc_applied[:ARM_DOF] = tau_pd + tau_gc

        # External push on EE for 1 second starting at t=2
        if 2.0 <= t < 3.0:
            data.xfrc_applied[ee_body_id, :3] = [push_force, 0, 0]
        else:
            data.xfrc_applied[ee_body_id, :] = 0

        mujoco.mj_step(model, data)

        err[k] = data.qpos[:ARM_DOF] - Q_HOME
        t_arr[k] = t

    return t_arr, err


def main():
    print("Run 1: PD only (no gravity comp)...")
    t1, err_no_gc = run_controller(use_grav_comp=False)
    print("Run 2: PD + gravity compensation...")
    t2, err_with_gc = run_controller(use_grav_comp=True)

    # Steady-state error: mean over last 1 s
    n = len(t1)
    mask_ss = (t1 > 4.0)
    mean_err_no_gc = np.mean(np.abs(err_no_gc[mask_ss]))
    mean_err_with_gc = np.mean(np.abs(err_with_gc[mask_ss]))
    improvement = mean_err_no_gc / max(mean_err_with_gc, 1e-9)
    print(f"\nMean steady-state |error| no gravity comp: {mean_err_no_gc:.4f} rad")
    print(f"Mean steady-state |error| with gravity comp: {mean_err_with_gc:.4f} rad")
    print(f"Improvement: {improvement:.1f}x")

    # Plot joints 1 and 4 (most affected by gravity)
    fig, axes = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
    axes[0].plot(t1, err_no_gc[:, 1], label='No grav comp', color='C3')
    axes[0].plot(t2, err_with_gc[:, 1], label='With grav comp', color='C0')
    axes[0].axvspan(2, 3, alpha=0.15, color='gray', label='1N push')
    axes[0].set_ylabel("Joint 2 error (rad)")
    axes[0].legend()
    axes[0].grid(alpha=0.3)
    axes[1].plot(t1, err_no_gc[:, 3], label='No grav comp', color='C3')
    axes[1].plot(t2, err_with_gc[:, 3], label='With grav comp', color='C0')
    axes[1].axvspan(2, 3, alpha=0.15, color='gray')
    axes[1].set_xlabel("Time (s)")
    axes[1].set_ylabel("Joint 4 error (rad)")
    axes[1].grid(alpha=0.3)
    fig.suptitle(f"Joint-tracking error: PD only vs PD + gravity comp ({improvement:.1f}× better)")
    plt.tight_layout()
    plt.savefig("../figures/day4_tracking_error.png", dpi=120)
    print("Wrote figures/day4_tracking_error.png")

    return mean_err_no_gc, mean_err_with_gc


if __name__ == "__main__":
    main()

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

Run 1: PD only (no gravity comp)...
Run 2: PD + gravity compensation...

Mean steady-state |error| no gravity comp: 0.1423 rad
Mean steady-state |error| with gravity comp: 0.0079 rad
Improvement: 18.0x
Wrote figures/day4_tracking_error.png

Step 3 — Common failures and fixes (10 min)

• "Improvement: 1.0x" — You're applying gravity comp in both runs. Check that tau_gc is zeros when use_grav_comp=False.
• Both curves explode (NaN) — KP too high or mj_step not stepping at default dt. Print model.opt.timestep (should be 0.002 s).
• No effect from the push — xfrc_applied indexing wrong. Check that ee_body_id is positive (not -1). The body is named "hand" in the standard menagerie Panda; if it changed, run [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i) for i in range(model.nbody)].

Step 4 — Log and commit (5 min)

echo "day4_gc_pd,$(date -I),4,grav_comp_vs_no,$(cd ~/robo47/w1-foundations && git rev-parse --short HEAD),0,Panda,pd_grav,2500,1,improvement_x,18.0,kp200_kd20" >> ~/robo47/metrics/metrics.csv

cd ~/robo47/w1-foundations
git add src/day4_grav_comp.py figures/day4_tracking_error.png
git commit -m "Day 4: gravity compensation + PD, 18x tracking improvement"

Deliverable checklist

• src/day4_grav_comp.py runs without error
• Console reports gravity-comp improvement ≥ 5× (typically ~18×)
• figures/day4_tracking_error.png shows both joints with clear visual gap between red (no grav comp) and blue (with grav comp)
• Steady-state error with grav comp < 0.02 rad
• One row in metrics.csv
• Commit pushed

Why this lab matters

Side quest (optional, 30 min)