The equations of motion

One equation for the whole arm

One joint on its own obeys $\tau = I\alpha$ (Foundations: Mass, inertia & F = ma). With all four joints free, each joint's motion depends on the others, and the arm obeys one vector equation, its equations of motion:

$$M(q)\,\ddot q + h(q, \dot q) = \tau$$

• $q$, $\dot q$, $\ddot q$ are the four joint angles (rad), velocities (rad/s) and accelerations (rad/s²), ordered [base, shoulder, elbow, wrist]; $\tau$ holds the four motor torques (N·m).
• $M(q)$ is the 4×4 mass matrix (kg·m²). Its diagonal holds each joint's inertia, the $I$ from $\tau = I\alpha$, and it changes with the pose. The off-diagonal entries couple the joints: a torque on the shoulder also swings the elbow.
• $h(q, \dot q) = C(q, \dot q)\,\dot q + g(q)$ holds the bias torques: gravity $g(q)$, plus the Coriolis and centrifugal torques $C\dot q$ that appear when joints move, the way a spinning base flings the forearm outwards.

Forward dynamics

Solve the equation for the accelerations and you can predict the motion:

$$\ddot q = M(q)^{-1}\big(\tau - h(q, \dot q)\big)$$

np.linalg.solve(M, b) finds $x$ with $Mx = b$ without inverting $M$.

Call	Returns
arm.mass_matrix(q)	$M(q)$, a 4×4 array
arm.bias_torques(q, qd)	$h(q, \dot q)$, 4 torques
arm.gravity_torques(q)	$g(q)$, 4 torques

Pass q and qd explicitly: without them these read the live arm, and the grader calls your function on other states.

The program

The arm starts folded, its motors cancelling gravity. It gets a kick on the base, shoulder and elbow for 0.2 s, then the same kick backwards. First your model predicts the next 0.6 s, stepping $\dot q \leftarrow \dot q + \ddot q\,\Delta t$ and $q \leftarrow q + \dot q\,\Delta t$ every $\Delta t$ = 2 ms, and draws the hand's path in yellow. Then the real arm runs, in blue, and Plots compares the angles. Finally it prints the diagonal of $M$ folded and unfolded: which joints got heavier, and why?

Your task

Write forward_dynamics(q, qd, tau). It must match the simulator on 8 states, and your prediction must stay within 0.5° of the real arm for the whole 0.6 s.