Trajectory generation + Week 1 integration + fresh-clone test

Hour 3 — Lab: integrate Week 1 into a pick-and-place pipeline (90 min)

Step 1 — Install Ruckig (5 min)

cd ~/robo47
source .venv/bin/activate
uv pip install ruckig
python -c "import ruckig; print(ruckig.__version__)"

Step 2 — Write the pipeline (50 min)

"""Day 7: integrated pick-and-place pipeline.
- Ruckig joint-space trajectory between two configs.
- DLS IK from Day 3 to convert task-space waypoints.
- Day 6 impedance + Day 4 grav comp for execution.
"""
from __future__ import annotations
import numpy as np
import matplotlib.pyplot as plt
import mujoco
import mujoco.viewer
import time
from ruckig import InputParameter, OutputParameter, Result, Ruckig

MODEL_PATH = "../mujoco_menagerie/franka_emika_panda/scene.xml"
ARM_DOF = 7
Q_HOME = np.array([0, -0.785, 0, -2.356, 0, 1.571, 0.785])

# --- Two task-space waypoints (in world frame) we'll move between
PICK_POS = np.array([0.40,  0.20, 0.40])
PLACE_POS = np.array([0.40, -0.20, 0.40])

# Conservative limits for Ruckig (rad, rad/s, rad/s^2, rad/s^3)
QD_MAX  = np.array([2.0] * ARM_DOF)
QDD_MAX = np.array([3.0] * ARM_DOF)
QDDD_MAX = np.array([10.0] * ARM_DOF)

KP_LIN = 800
KD_LIN = 2 * np.sqrt(KP_LIN)
KQ_DAMP = np.array([5, 5, 5, 5, 2, 2, 2], dtype=float)


def dls_ik(model, data, target_pos, ee_site_id, n_iter=50, lam=0.05, tol=1e-4):
    """Solve IK for desired EE position. Returns the converged joint config."""
    q = data.qpos[:ARM_DOF].copy()
    for _ in range(n_iter):
        data.qpos[:ARM_DOF] = q
        mujoco.mj_forward(model, data)
        x_now = data.site_xpos[ee_site_id].copy()
        err = target_pos - x_now
        if np.linalg.norm(err) < tol:
            break
        jacp = np.zeros((3, model.nv))
        jacr = np.zeros((3, model.nv))
        mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
        J = jacp[:, :ARM_DOF]
        dq = J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(3), err)
        q = q + dq
    return q


def ruckig_trajectory(q_start, q_goal, dt):
    """Generate a jerk-limited joint-space trajectory from q_start to q_goal.
    Returns three N×ARM_DOF arrays: qpos, qvel, qacc.
    """
    rk = Ruckig(ARM_DOF, dt)
    inp = InputParameter(ARM_DOF)
    out = OutputParameter(ARM_DOF)
    inp.current_position = q_start.tolist()
    inp.current_velocity = [0.0] * ARM_DOF
    inp.current_acceleration = [0.0] * ARM_DOF
    inp.target_position = q_goal.tolist()
    inp.target_velocity = [0.0] * ARM_DOF
    inp.target_acceleration = [0.0] * ARM_DOF
    inp.max_velocity = QD_MAX.tolist()
    inp.max_acceleration = QDD_MAX.tolist()
    inp.max_jerk = QDDD_MAX.tolist()

    qs, qvs, qas = [], [], []
    while True:
        res = rk.update(inp, out)
        qs.append(np.array(out.new_position))
        qvs.append(np.array(out.new_velocity))
        qas.append(np.array(out.new_acceleration))
        if res == Result.Finished:
            break
        out.pass_to_input(inp)

    return np.array(qs), np.array(qvs), np.array(qas)


def execute_trajectory(model, data, q_traj, qd_traj):
    """Track the trajectory using PD + gravity compensation in joint space."""
    KP = np.array([200, 200, 150, 150, 80, 80, 40], dtype=float)
    KD = np.array([20, 20, 15, 15, 8, 8, 4], dtype=float)
    history = np.zeros((len(q_traj), 3))  # EE positions
    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "attachment_site")

    for k in range(len(q_traj)):
        q_des = q_traj[k]
        qd_des = qd_traj[k]
        q = data.qpos[:ARM_DOF]
        qd = data.qvel[:ARM_DOF]
        tau_pd = KP * (q_des - q) + KD * (qd_des - qd)
        tau_gc = data.qfrc_bias[:ARM_DOF]
        data.qfrc_applied[:] = 0
        data.qfrc_applied[:ARM_DOF] = tau_pd + tau_gc

        mujoco.mj_step(model, data)
        history[k] = data.site_xpos[ee_site_id]

    return history


def main(visualize: bool = False):
    model = mujoco.MjModel.from_xml_path(MODEL_PATH)
    data = mujoco.MjData(model)
    data.qpos[:ARM_DOF] = Q_HOME.copy()
    mujoco.mj_forward(model, data)

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

    # 1. Solve IK for pick and place poses
    q_pick = dls_ik(model, data, PICK_POS, ee_site_id)
    print(f"q_pick: {np.round(q_pick, 3)}")
    data.qpos[:ARM_DOF] = q_pick
    mujoco.mj_forward(model, data)
    print(f"  resulting EE pos: {np.round(data.site_xpos[ee_site_id], 3)}, target {PICK_POS}")

    q_place = dls_ik(model, data, PLACE_POS, ee_site_id)
    print(f"q_place: {np.round(q_place, 3)}")
    data.qpos[:ARM_DOF] = q_place
    mujoco.mj_forward(model, data)
    print(f"  resulting EE pos: {np.round(data.site_xpos[ee_site_id], 3)}, target {PLACE_POS}")

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

    dt = model.opt.timestep

    # 2. Plan home -> pick
    print("\nPlanning home -> pick ...")
    q1, qd1, _ = ruckig_trajectory(Q_HOME, q_pick, dt)
    print(f"  trajectory length: {len(q1)} steps ({len(q1)*dt:.2f} s)")

    # 3. Plan pick -> place (no real grasp, just move)
    print("Planning pick -> place ...")
    q2, qd2, _ = ruckig_trajectory(q_pick, q_place, dt)
    print(f"  trajectory length: {len(q2)} steps ({len(q2)*dt:.2f} s)")

    # 4. Execute both
    h1 = execute_trajectory(model, data, q1, qd1)
    h2 = execute_trajectory(model, data, q2, qd2)

    # 5. Plot the resulting EE path
    full = np.vstack([h1, h2])
    fig, ax = plt.subplots(figsize=(8, 6))
    ax.plot(full[:, 0], full[:, 1], 'b-', label='EE path')
    ax.scatter(*PICK_POS[:2], color='g', s=80, label='pick', zorder=5)
    ax.scatter(*PLACE_POS[:2], color='r', s=80, label='place', zorder=5)
    ax.set_xlabel("X (m)")
    ax.set_ylabel("Y (m)")
    ax.set_title("Pick-and-place EE path")
    ax.set_aspect('equal')
    ax.legend()
    ax.grid(alpha=0.3)
    plt.tight_layout()
    plt.savefig("../figures/day7_pickplace_path.png", dpi=120)
    print("Wrote figures/day7_pickplace_path.png")

    # 6. Optional: visualize live
    if visualize:
        data.qpos[:ARM_DOF] = Q_HOME.copy()
        mujoco.mj_forward(model, data)
        with mujoco.viewer.launch_passive(model, data) as v:
            for q_traj, qd_traj in [(q1, qd1), (q2, qd2)]:
                for k in range(len(q_traj)):
                    q_des = q_traj[k]
                    qd_des = qd_traj[k]
                    q = data.qpos[:ARM_DOF]
                    qd = data.qvel[:ARM_DOF]
                    KP = np.array([200, 200, 150, 150, 80, 80, 40], dtype=float)
                    KD = np.array([20, 20, 15, 15, 8, 8, 4], dtype=float)
                    tau = KP * (q_des - q) + KD * (qd_des - qd) + data.qfrc_bias[:ARM_DOF]
                    data.qfrc_applied[:] = 0
                    data.qfrc_applied[:ARM_DOF] = tau
                    mujoco.mj_step(model, data)
                    v.sync()
                    time.sleep(dt)


if __name__ == "__main__":
    import sys
    main(visualize=("--viz" in sys.argv))

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

q_pick: [ 0.452 -0.193 -0.025 -2.05  -0.012  1.86   0.785]
  resulting EE pos: [0.4 0.2 0.4], target [0.4 0.2 0.4]
q_place: [-0.452 -0.193  0.025 -2.05   0.012  1.86   0.785]
  resulting EE pos: [ 0.4 -0.2  0.4], target [ 0.4 -0.2  0.4]

Planning home -> pick ...
  trajectory length: 612 steps (1.22 s)
Planning pick -> place ...
  trajectory length: 478 steps (0.96 s)
Wrote figures/day7_pickplace_path.png

python src/day7_pick_and_place.py --viz

Step 3 — Write the Makefile (10 min)

.PHONY: install reproduce clean

install:
	uv venv --python 3.12 .venv
	. .venv/bin/activate && uv pip install -r requirements.txt

reproduce:
	. ../.venv/bin/activate && \
	  python src/day1_transforms.py && \
	  python src/day2_fk.py && \
	  python src/day4_grav_comp.py && \
	  python src/day5_cartpole_lqr.py && \
	  python src/day7_pick_and_place.py

clean:
	rm -rf figures/*.png videos/*.mp4 __pycache__ src/__pycache__

mujoco==3.7.*
numpy
scipy
matplotlib
ruckig
pynput

Step 4 — Fresh-clone test (15 min)

# Push your work
cd ~/robo47/w1-foundations
git add -A && git commit -m "Day 7: integrated pick-place pipeline + Makefile"
git push origin main   # adjust branch name if needed

# Now do the fresh-clone test
cd /tmp
rm -rf w1-test
git clone <your-github-url-for-w1-foundations> w1-test
cd w1-test
cp -r ~/robo47/mujoco_menagerie .   # menagerie not in repo (gitignored)

# Make a fresh venv
uv venv --python 3.12 .venv
source .venv/bin/activate
uv pip install -r requirements.txt

# Run the headless pipeline
python src/day5_cartpole_lqr.py 2>&1 | tee fresh_run.log

# Week 1 retro

## Reproducibility
- Original mean settling time: 1.84 s
- Fresh-clone mean settling time: 1.83 s
- Delta: 0.5%, within ±1% target.

## What I learned
1. Frame ordering: `T_a @ T_b` and `T_b @ T_a` are different; the difference IS the geometry.
2. PoE is cleaner than DH but the screw-axis sign convention is still where bugs hide.
3. Singular Jacobians don't usually crash code — they make controllers oscillate. Watch the condition number.
4. Gravity compensation is worth ~18× tracking improvement. It's not optional on a 7-DoF arm.
5. LQR linearization happens about an equilibrium; the controller works in *delta-state*, not absolute state.

## What still confuses me
- Why does Ruckig need `max_jerk`? In what units, and how was 10 rad/s³ chosen?

Step 5 — Log and commit (5 min)

cd ~/robo47/w1-foundations
git add Makefile requirements.txt RETRO.md src/day7_pick_and_place.py figures/day7_pickplace_path.png videos/day7_pickplace.mp4
git commit -m "Day 7: pick-place pipeline + Makefile + Week 1 retro"
git push

Deliverable checklist

• src/day7_pick_and_place.py runs and prints expected outputs
• videos/day7_pickplace.mp4 shows smooth pick-place motion
• figures/day7_pickplace_path.png shows EE path with pick/place markers
• Makefile has reproduce target that runs successfully end-to-end
• Fresh-clone test passes; Day 5 settling time reproduces within ±1%
• RETRO.md documents reproducibilityEvaluation & ResearchReproducibilityWhether others can reliably get the same result. delta and 5 learnings
• Repo pushed; HEAD SHA recorded

Why this lab matters

Side quest (optional, 60 min)