Step 1

PID control

Hold a torque-driven shoulder on target against gravity with P, I and D terms, and keep the integral from winding up.

What it computes

A PID controller turns an error $e = q_{\text{target}} - q$ into a command $u$ , here the shoulder motor's torque:

u = k_p\,e + k_i \int e\,dt + k_d\,\frac{de}{dt}

P pulls towards the target and D damps the motion. Both give zero torque at zero error, but holding the arm up against gravity takes a steady torque. The I term builds it up from the error that's left.

Windup

The output is clamped to $\pm$ limit. While it's clamped, the integral keeps growing without changing $u$ . That's windup: the stored-up torque later throws the arm past the target. Anti-windup by conditional integration skips integrating while the output is saturated. (Back-calculation, which bleeds the integral towards the clamped output, is the other common fix.)

# every dt seconds:
integral_new = integral + error * dt
derivative = (error - prev_error) / dt    # 0 on the first call
u = kp * error + ki * integral_new + kd * derivative
u_out = clip(u, -limit, limit)
if u_out == u:                            # not saturated
    integral = integral_new
prev_error = error
return u_out

The program

Gravity isn't compensated, and the base, elbow and wrist are held for you. The shoulder starts at 0.2 rad with a target of 0.8 rad; at t = 2 s the hand is pushed down with 20 N for 0.1 s. Your PID runs at 500 Hz with kp = 175, ki = 350, kd = 9 and a 12 N·m limit. Leave pid_response under your class: the grader tests PID through it.

See it in context

Pick & Place's first step, Joints, targets & PD control, explains PD control, but there MuJoCo's position servos run it for you.

Your task