Control II: operational-space, impedance, contact

Hour 3 — Lab: impedance circle-tracing on Panda with simulated push (75 min)

Step 1 — Add an impedance controller (50 min)

"""Day 6: Task-space impedance control on the Panda EE, with a 1s push perturbation."""
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 = 8.0
ARM_DOF = 7

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

# Trajectory: circle in xy-plane at z=0.7
CIRCLE_RADIUS = 0.10
CIRCLE_FREQ_HZ = 0.25
CIRCLE_CENTER = np.array([0.4, 0.0, 0.5])

# Joint-space damping for residual modes
KQ_DAMP = np.array([5, 5, 5, 5, 2, 2, 2], dtype=float)


def task_space_target(t: float):
    omega = 2 * np.pi * CIRCLE_FREQ_HZ
    x = CIRCLE_CENTER + np.array([
        CIRCLE_RADIUS * np.cos(omega * t),
        CIRCLE_RADIUS * np.sin(omega * t),
        0.0,
    ])
    xd = np.array([
        -CIRCLE_RADIUS * omega * np.sin(omega * t),
         CIRCLE_RADIUS * omega * np.cos(omega * t),
         0.0,
    ])
    return x, xd


def run(kp_lin: float, push_force: float = 30.0, push_window=(3.0, 4.0)):
    """One run with given task-space stiffness Kp_lin (N/m).
    Damping uses critical damping rule: Kd = 2*sqrt(Kp * m_eff). We use a fixed m_eff~1.
    """
    model = mujoco.MjModel.from_xml_path(MODEL_PATH)
    data = mujoco.MjData(model)

    data.qpos[:ARM_DOF] = Q_HOME.copy()
    data.qvel[:] = 0
    mujoco.mj_forward(model, data)

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

    Kp = kp_lin * np.eye(3)
    Kd = 2 * np.sqrt(kp_lin) * np.eye(3)

    n_steps = int(SIM_TIME_S / model.opt.timestep)
    history_pos = np.zeros((n_steps, 3))
    history_target = np.zeros((n_steps, 3))
    history_force = np.zeros(n_steps)  # commanded EE force magnitude
    t_arr = np.zeros(n_steps)

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

        # --- Task-space target
        x_des, xd_des = task_space_target(t)

        # --- Current EE pose and velocity
        x_now = data.site_xpos[ee_site_id].copy()
        jacp = np.zeros((3, model.nv))
        jacr = np.zeros((3, model.nv))
        mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
        xd_now = jacp[:, :ARM_DOF] @ data.qvel[:ARM_DOF]

        # --- Impedance: F = Kp*(x_des - x) + Kd*(xd_des - xd)
        F = Kp @ (x_des - x_now) + Kd @ (xd_des - xd_now)
        history_force[k] = np.linalg.norm(F)

        # --- Map F to joint torques: τ = J.T · F (translational only)
        tau_task = jacp[:, :ARM_DOF].T @ F

        # --- Joint-space damping for residual / null-space (small)
        tau_null = -KQ_DAMP * data.qvel[:ARM_DOF]

        # --- Gravity comp
        tau_gc = data.qfrc_bias[:ARM_DOF].copy()

        # --- Apply
        data.qfrc_applied[:] = 0
        data.qfrc_applied[:ARM_DOF] = tau_task + tau_null + tau_gc

        # --- External push
        if push_window[0] <= t < push_window[1]:
            data.xfrc_applied[ee_body_id, :3] = [push_force, 0, 0]
        else:
            data.xfrc_applied[ee_body_id, :] = 0

        mujoco.mj_step(model, data)

        history_pos[k] = data.site_xpos[ee_site_id].copy()
        history_target[k] = x_des
        t_arr[k] = t

    return t_arr, history_pos, history_target, history_force


def main():
    print("Run 1: STIFF (Kp = 1500 N/m)")
    t_s, pos_s, tgt_s, force_s = run(kp_lin=1500.0)
    print("Run 2: COMPLIANT (Kp = 200 N/m)")
    t_c, pos_c, tgt_c, force_c = run(kp_lin=200.0)

    # Tracking error during push window (t in 3–4s)
    push_mask_s = (t_s >= 3.0) & (t_s < 4.0)
    push_mask_c = (t_c >= 3.0) & (t_c < 4.0)
    err_s = np.linalg.norm(pos_s - tgt_s, axis=1)
    err_c = np.linalg.norm(pos_c - tgt_c, axis=1)

    peak_disp_s = err_s[push_mask_s].max()
    peak_disp_c = err_c[push_mask_c].max()
    peak_force_s = force_s[push_mask_s].max()
    peak_force_c = force_c[push_mask_c].max()

    print(f"\nStiff:     peak disp {peak_disp_s:.3f} m, peak |F| {peak_force_s:.1f} N")
    print(f"Compliant: peak disp {peak_disp_c:.3f} m, peak |F| {peak_force_c:.1f} N")

    # --- Plot circles in xy
    fig, axes = plt.subplots(1, 2, figsize=(11, 5))
    for ax, (pos, tgt, label) in zip(axes,
        [(pos_s, tgt_s, f"Stiff (Kp=1500), peak disp {peak_disp_s*1000:.0f} mm"),
         (pos_c, tgt_c, f"Compliant (Kp=200), peak disp {peak_disp_c*1000:.0f} mm")]):
        ax.plot(tgt[:, 0], tgt[:, 1], 'k--', alpha=0.4, label='target')
        ax.plot(pos[:, 0], pos[:, 1], 'b-', alpha=0.7, label='actual')
        # Highlight push window
        push_mask = (t_s >= 3.0) & (t_s < 4.0)
        ax.plot(pos[push_mask, 0], pos[push_mask, 1], 'r-', linewidth=2, label='during push')
        ax.set_xlabel("X (m)")
        ax.set_ylabel("Y (m)")
        ax.set_title(label)
        ax.legend(fontsize=9)
        ax.set_aspect('equal')
        ax.grid(alpha=0.3)

    plt.tight_layout()
    plt.savefig("../figures/day6_circle_tracking.png", dpi=120)
    print("Wrote figures/day6_circle_tracking.png")

    # --- Bar plot of peak disp vs peak force
    fig2, ax2 = plt.subplots(figsize=(7, 4))
    x = np.arange(2)
    width = 0.35
    ax2.bar(x - width/2, [peak_disp_s * 1000, peak_disp_c * 1000], width, label='peak disp (mm)', color='C0')
    ax2.bar(x + width/2, [peak_force_s, peak_force_c], width, label='peak |F| (N)', color='C3')
    ax2.set_xticks(x)
    ax2.set_xticklabels(['Stiff Kp=1500', 'Compliant Kp=200'])
    ax2.set_ylabel("Magnitude")
    ax2.set_title("Stiffness trade-off under 1s, 30N push")
    ax2.legend()
    ax2.grid(alpha=0.3, axis='y')
    plt.tight_layout()
    plt.savefig("../figures/day6_force_vs_disp.png", dpi=120)
    print("Wrote figures/day6_force_vs_disp.png")

    return peak_disp_s, peak_disp_c, peak_force_s, peak_force_c


if __name__ == "__main__":
    main()

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

Run 1: STIFF (Kp = 1500 N/m)
Run 2: COMPLIANT (Kp = 200 N/m)

Stiff:     peak disp 0.018 m, peak |F| 24.3 N
Compliant: peak disp 0.071 m, peak |F| 14.2 N
Wrote figures/day6_circle_tracking.png
Wrote figures/day6_force_vs_disp.png

• Expected figures:
• day6_circle_tracking.png: Two side-by-side circles. Both show a target dashed black circle (radius 0.10 m). The stiff blue trace stays nearly on top of it, with a small red bump (peak ~2 cm) during the push. The compliant blue trace is visibly less round overall and during the push the red bump is much larger (~7 cm), showing the EE getting shoved out by ~7 cm.
• day6_force_vs_disp.png: Bar chart with stiff peak disp ~18 mm and stiff peak force ~24 N versus compliant peak disp ~71 mm and compliant peak force ~14 N. The trade-off is visible: stiff = small motion, large force; compliant = large motion, small force.

Step 2 — Common failures and fixes (15 min)

• EE shoots away during push (compliant case) — Kp_lin=200 is fine; if you set it to 50 the EE genuinely flies. Keep ≥ 100.
• Stiff circle is also wobbly — JointMovement, Mechanics & Robot BodyJointA movable connection between robot parts. damping KQ_DAMP too low. Raise to [10, 10, 10, 10, 4, 4, 4].
• Both runs identical — You forgot to pass kp_lin through run(). Print Kp inside the loop on first step to verify.
• `mj_jacSite` returns garbage — Confirm the site name "attachment_site" exists.

Step 3 — Log and commit (10 min)

echo "day6_imped_stiff,$(date -I),6,impedance_stiff,$(cd ~/robo47/w1-foundations && git rev-parse --short HEAD),0,Panda,impedance,4000,1,peak_disp_m,0.018,kp1500" >> ~/robo47/metrics/metrics.csv
echo "day6_imped_comp,$(date -I),6,impedance_compliant,$(cd ~/robo47/w1-foundations && git rev-parse --short HEAD),0,Panda,impedance,4000,1,peak_disp_m,0.071,kp200" >> ~/robo47/metrics/metrics.csv

cd ~/robo47/w1-foundations
git add src/day6_impedance.py figures/day6_circle_tracking.png figures/day6_force_vs_disp.png
git commit -m "Day 6: task-space impedance, stiff-vs-compliant trade-off"

Deliverable checklist

• src/day6_impedance.py runs without error
• Console reports two peak disp values; stiff < compliant by ≥ 3×
• figures/day6_circle_tracking.png shows clear visual difference between stiff and compliant under push
• figures/day6_force_vs_disp.png shows the stiffness-vs-force trade-off
• Two rows in metrics.csv
• Commit pushed

Why this lab matters

Side quest (optional, 45 min)