Inverse kinematics, Jacobians, singularities

Hour 3 — Lab: keyboard-teleop a Panda with DLS IK (75 min)

Step 1 — Install keyboard handling and verify viewer works (10 min)

cd ~/robo47
source .venv/bin/activate
uv pip install pynput

python -c "
import mujoco, mujoco.viewer, time
m = mujoco.MjModel.from_xml_path('mujoco_menagerie/franka_emika_panda/scene.xml')
d = mujoco.MjData(m)
with mujoco.viewer.launch_passive(m, d) as v:
    for _ in range(180):  # 3 seconds at 60 fps
        mujoco.mj_step(m, d)
        v.sync()
        time.sleep(1/60)
print('passive viewer ok')
"

Step 2 — Write the teleop loop (40 min)

"""Day 3: Keyboard-teleop the Franka Panda using Damped Least Squares IK.
Press WASD/QE for translation; arrow keys for orientation; ESC to quit.
"""
from __future__ import annotations
import time
import numpy as np
import mujoco
import mujoco.viewer
import matplotlib.pyplot as plt
from pynput import keyboard

# --- Config ------------------------------------------------------------------
DT = 1.0 / 60.0
LIN_SPEED = 0.15   # m/s
ANG_SPEED = 0.5    # rad/s
DAMPING = 0.05     # DLS lambda
TIMEOUT_SEC = 60.0

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

# --- Keyboard state (shared across threads) ---------------------------------
keys_held: set = set()

def on_press(key):
    try:
        keys_held.add(key.char)
    except AttributeError:
        keys_held.add(key)

def on_release(key):
    try:
        keys_held.discard(key.char)
    except AttributeError:
        keys_held.discard(key)
    if key == keyboard.Key.esc:
        return False  # stop listener

# --- DLS IK ------------------------------------------------------------------

def dls_step(jacp: np.ndarray, jacr: np.ndarray,
             dx: np.ndarray, dr: np.ndarray, lam: float = DAMPING) -> np.ndarray:
    """One step of damped least-squares IK. Returns delta-q for the 7 arm joints."""
    J = np.vstack([jacp, jacr])         # 6 x n_dof
    e = np.concatenate([dx, dr])        # 6
    JJt = J @ J.T
    dq = J.T @ np.linalg.solve(JJt + lam**2 * np.eye(6), e)
    return dq

# --- Main --------------------------------------------------------------------

def main():
    model = mujoco.MjModel.from_xml_path(MODEL_PATH)
    data = mujoco.MjData(model)

    # Home pose for the arm
    data.qpos[:7] = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
    mujoco.mj_forward(model, data)

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

    listener = keyboard.Listener(on_press=on_press, on_release=on_release)
    listener.start()

    print("Teleop ready. WASD = X/Y, QE = Z, arrows = orientation, ESC = quit.")

    cond_history: list = []
    t_history: list = []
    t_start = time.time()

    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running() and (time.time() - t_start) < TIMEOUT_SEC:
            t = time.time() - t_start
            # --- Build commanded EE velocity from keyboard
            v = np.zeros(3)
            w = np.zeros(3)
            if 'w' in keys_held: v[0] += LIN_SPEED
            if 's' in keys_held: v[0] -= LIN_SPEED
            if 'a' in keys_held: v[1] += LIN_SPEED
            if 'd' in keys_held: v[1] -= LIN_SPEED
            if 'q' in keys_held: v[2] += LIN_SPEED
            if 'e' in keys_held: v[2] -= LIN_SPEED
            if keyboard.Key.up    in keys_held: w[0] += ANG_SPEED
            if keyboard.Key.down  in keys_held: w[0] -= ANG_SPEED
            if keyboard.Key.left  in keys_held: w[1] += ANG_SPEED
            if keyboard.Key.right in keys_held: w[1] -= ANG_SPEED

            # Desired EE motion this step
            dx = v * DT
            dr = w * DT

            # --- Jacobian at current state
            jacp = np.zeros((3, model.nv))
            jacr = np.zeros((3, model.nv))
            mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)

            # Restrict to the 7 arm joints (slice columns)
            J_arm = np.vstack([jacp[:, :7], jacr[:, :7]])

            # --- DLS step
            dq = dls_step(jacp[:, :7], jacr[:, :7], dx, dr)
            data.qpos[:7] += dq

            # --- Step physics
            mujoco.mj_forward(model, data)
            viewer.sync()

            # --- Log condition number
            cond = np.linalg.cond(J_arm)
            cond_history.append(cond)
            t_history.append(t)

            time.sleep(max(0, DT - (time.time() - t_start - t)))

    listener.stop()

    # --- Save the condition-number plot
    cond = np.array(cond_history)
    t = np.array(t_history)
    fig, ax = plt.subplots(figsize=(10, 4))
    ax.plot(t, cond)
    ax.axhline(500, color='r', linestyle='--', label='singularity threshold')
    ax.set_xlabel("Time (s)")
    ax.set_ylabel("cond(J_arm)")
    ax.set_title("Jacobian condition number during teleop")
    ax.set_yscale('log')
    ax.legend()
    ax.grid(True, alpha=0.3)
    plt.tight_layout()
    plt.savefig("../figures/day3_jacobian_condition.png", dpi=120)
    print(f"\nWrote figures/day3_jacobian_condition.png")
    print(f"Max condition number reached: {cond.max():.1f}")
    print(f"Time spent above threshold (cond > 500): {((cond > 500).sum() * DT):.1f} s")

if __name__ == "__main__":
    main()

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

• What you should experience:
• An interactive Panda window opens.
• Press W: arm advances in +X. Press S: retreats in −X. A/D: left/right. Q/E: up/down.
• Move the arm in a smooth circle in front of the robotCore ConceptsRobotA physical system with sensors and actuators that can observe the world and take actions. for ~10 seconds. The motion should feel responsive.
• Now deliberately push the arm fully extended forward (hold W). At some point the arm will reach the edge of its workspaceManipulation & TasksWorkspaceThe region of space the robot can reach. and the motion will become jittery — that's a singularity.
• Press ESC to exit.

Teleop ready. WASD = X/Y, QE = Z, arrows = orientation, ESC = quit.
Wrote figures/day3_jacobian_condition.png
Max condition number reached: 1247.3
Time spent above threshold (cond > 500): 2.4 s

Step 3 — Record a screen video (10 min)

Step 4 — Common failures and fixes (10 min)

• `pynput` doesn't capture keys — On macOS, you need to grant terminal accessibility permissions in System Settings → Privacy → Accessibility.
• Arm wiggles wildly — LIN_SPEED is too high. Reduce to 0.05 and retry.
• DLS doesn't help near singularity (still jittery) — Increase DAMPING to 0.1. With more damping, the EE responds more sluggishly but never explodes.
• No condition-number spike at all — The Panda's home pose is well-conditioned. To force a singularity, hold W long enough that the arm fully extends, then press an orientation key. The fully-stretched arm has a wrist singularity.
• `mj_jacSite` segfault — You used the wrong site name. Print [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SITE, i) for i in range(model.nsite)] to list all sites.

Step 5 — Log and commit (5 min)

echo "day3_dls_teleop,$(date -I),3,dls_ik_teleop,$(cd ~/robo47/w1-foundations && git rev-parse --short HEAD),0,Panda,dls_ik,1800,1,max_cond_J,1247,manual_teleop_30s" >> ~/robo47/metrics/metrics.csv

cd ~/robo47/w1-foundations
git add src/day3_dls_ik.py figures/day3_jacobian_condition.png videos/day3_teleop.mp4
git commit -m "Day 3: DLS IK teleop, observed singularity cond ~1200"

Deliverable checklist

• src/day3_dls_ik.py runs and accepts keyboard input
• videos/day3_teleop.mp4 exists, ≤30 s, shows smooth teleopImitation & Reinforcement LearningTeleoperation (teleop)A human remotely controlling the robot, often to collect demonstrations.
• figures/day3_jacobian_condition.png shows cond(J) over time on log scale, with at least one visible spike above 500
• Max condition number reached > 500
• One row in metrics.csv
• Commit pushed

Why this lab matters

Side quest (optional, 30 min)