Forward kinematics (source code)

= Forward kinematics

<Forward kinematics> computes an <end effector>'s configuration from a mechanism's joint coordinates and geometric model. For a serial <robot arm>, each joint contributes a relative displacement or rotation, and composing these transformations gives the tool's position and orientation relative to the base. This is a geometric calculation; the <forces> needed to realize the motion belong to dynamics.<Modern Robotics: Mechanics, Planning, and Control by Kevin M. Lynch and Frank C. Park>[Chapter 4, author supplement §4.1.1, base and end-effector frames]{ref}

For a two-link planar arm, with relative joint angles $\theta_1,\theta_2$ and lengths $l_1,l_2$,
$$
\begin{pmatrix}x\\y\end{pmatrix}=l_1\begin{pmatrix}\cos\theta_1\\\sin\theta_1\end{pmatrix}+l_2\begin{pmatrix}\cos(\theta_1+\theta_2)\\\sin(\theta_1+\theta_2)\end{pmatrix}.
$$
For $l_1=l_2=1\,\mathrm m$, $\theta_1=\pi/2$ and $\theta_2=0$, the tool is at $(0,2)\,\mathrm m$ and points at angle $\pi/2$. Using $\theta_2$ alone for the second link's absolute angle would give the wrong result. Comparing this predicted position with an external measurement tests the link lengths, zero-angle calibration and rigid-link approximation.