2D SCARA Kinematics

Kinematics derivation

The mechanism is a planar serial chain with two revolute joints. Let the base origin be $\underset{\sim}{o}_0=[x_0,y_0]^T$, and let each link extend along the local $i$-axis of its joint frame. The first link is oriented by $\theta_1$ relative to the base frame. Because $\theta_2$ is measured relative to link 1, the absolute orientation of link 2 is $\theta_1+\theta_2$.

Forward kinematics

Solving the direct (forward) kinematics of the 2D SCARA is fairly straightforward: given the joint angles, we can determine the location of the gripper in base coordinates.

$$\underset{\sim}{o}_2 = a\vect{i}_0 + b\vect{j}_0$$

Now, just looking at the above figure, we can see two things fairly clearly:

$$\begin{aligned} a &= l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2), \\ b &= l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2). \end{aligned}$$

And so,

$$\underset{\sim}{o}_2 =\underset{\sim}{o}_0+ \begin{bmatrix}a\\b\end{bmatrix} =\underset{\sim}{o}_0+ \begin{bmatrix} l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2) \\ l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2) \end{bmatrix}$$

The key thing here is that link 2 ($\theta_2$) is measured relative to link 1 ($\theta_1$). To get link 2’s absolute orientation, and therefore its $x$ and $y$ components, you need to add $\theta_1$ and $\theta_2$ together.

Now, let’s solve the velocity kinematics.

Velocity kinematics

All we really need to do here is take the derivative of the point $\underset{\sim}{o}_2$ with respect to time. Using basic differentiation rules and trig derivatives, we take the derivative of the equation above:

$$\frac{d}{dt} \begin{bmatrix}a\\b\end{bmatrix} = \begin{bmatrix} -l_1\sin\theta_1\,\dot{\theta}_1-l_2\sin(\theta_1+\theta_2)(\dot{\theta}_1+\dot{\theta}_2) \\ l_1\cos\theta_1\,\dot{\theta}_1+l_2\cos(\theta_1+\theta_2)(\dot{\theta}_1+\dot{\theta}_2) \end{bmatrix}$$

Now, rearranging a bit:

$$\begin{bmatrix}\dot{a}\\\dot{b}\end{bmatrix} = \underbrace{ \begin{bmatrix} -l_1\sin\theta_1-l_2\sin(\theta_1+\theta_2) \\ l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2) \end{bmatrix}}_{\raisebox{-0.3ex}{$\vect{J}_1$}} \dot{\theta}_1 + \underbrace{ \begin{bmatrix} -l_2\sin(\theta_1+\theta_2) \\ l_2\cos(\theta_1+\theta_2) \end{bmatrix}}_{\raisebox{-0.3ex}{$\vect{J}_2$}} \dot{\theta}_2$$ $$\vect{\dot{v}} =\vect{J}_1\dot{\theta}_1+\vect{J}_2\dot{\theta}_2 =\begin{bmatrix}\vect{J}_1&\vect{J}_2\end{bmatrix} \begin{bmatrix}\dot{\theta}_1\\\dot{\theta}_2\end{bmatrix} =J(\vect{q})\vect{\dot{q}}$$

This is the Jacobian matrix, which, in this case, is a $2\times2$ matrix that maps joint velocities to end-effector velocities in the base frame.

Here, it is easy to see how it represents the end-effector velocities, but it will definitely get messy for higher DOFs!

Inverse kinematics

Now for the inverse kinematics, which I find to be a bit more fun. Here, we are given the end-effector position and want to find the corresponding joint angles, $\theta_1$ and $\theta_2$. This is a bit trickier, but we can use the law of cosines to solve for the angles.

Looking back at the figure, the important part is to draw the hypotenuse formed by $a$ and $b$. We can then use cosine and tangent relationships to solve for the various angles. As shown in the figure, we have also labeled $\alpha$ and $\beta$, which are the angles of the drawn triangles. We can solve for these and then use them to determine the joint angles.

First, let’s solve for $\theta_2$, since that one is a bit easier. There are technically two ways to do this (well, 1.5, the second way uses the first, but with a few additional steps): a simple method, and a more involved method using atan2 for better accuracy at small angles, as well as the ability to handle quadrants.

The main trick is to solve for this interior angle and recognize that $\pi-\text{Angle}=\theta_2$.

So, using the law of cosines, we can solve for $\theta_2$:

$$l^2 = l_1^2 + l_2^2 - 2l_1l_2\cos(\pi-\theta_2)$$

The equation above works, but we can also use the trig identity $\cos(\pi-x)=-\cos(x)$ to simplify a bit:

$$l^2 = l_1^2 + l_2^2 + 2l_1l_2\cos(\theta_2)$$

And so:

$$\theta_2=\cos^{-1}\!\left(\frac{l^2-l_1^2-l_2^2}{2l_1l_2}\right)$$

Now, that’s technically all we need. That’s method 1. But to get a little fancy (and improve the accuracy), we can instead use arctangent, or specifically, the quadrant-aware atan2, to retrieve the angle.

$$c_2=\cos\theta_2=\frac{l^2-l_1^2-l_2^2}{2l_1l_2}$$

Since cosine alone does not distinguish between the positive and negative angle branches, we grab the sine:

$$s_2=\sin\theta_2=\pm\sqrt{1-c_2^2}$$

Finally, to retrieve the angle while preserving its sign and quadrant, we use:

$$\theta_2=\operatorname{atan2}(s_2,c_2)$$

Moving on to finding $\theta_1$.

We can find $\alpha$ simply by using the target-origin coordinates:

$$\alpha=\operatorname{atan2}(b,a)$$

For $\beta$, we can also use the law of cosines. For example:

$$l_2^2=l_1^2+l^2-2l_1l\cos\beta$$

But again, we don’t really like the law of cosines, so let’s use atan2 instead. There is actually another right angle hidden in the figure:

And so:

$$\begin{aligned} \beta &=\operatorname{atan2}\!\left(l_2\sin\theta_2,\;l_1+l_2\cos\theta_2\right) \end{aligned}$$

Here, $\theta_2$ is already known.

And finally:

$$\theta_1=\alpha-\beta$$

There we have it: the inverse kinematics for a 2D SCARA arm. Note that the $s_2$ derivation gives us two possible solutions for $\theta_2$, elbow up and elbow down, which also give us two corresponding solutions for $\theta_1$.

Repository structure

The derivation above describes the underlying mathematics. In the repository, the calculation layer lives in kinematics.py. It provides 2D rotation matrices along with forward-, inverse-, and velocity-kinematics functions. Three small applications sit above that shared layer:

1. forward_kinematics.py controls the base position and two joint angles while plotting the link geometry and local coordinate frames.
2. inverse_kinematics.py moves a Cartesian target, compares two analytical formulations, and switches between the positive and negative solution branches.
3. velocity_kinematics.py controls joint position and joint rate, displays the Jacobian, and animates the resulting motion.

Keeping the numerical model separate from the interface makes the equations easier to inspect and reuse while each demonstration stays focused on one kinematics question.

Run locally

Create a Python environment with NumPy and Matplotlib, then launch any demonstration independently:

python forward_kinematics.py
python inverse_kinematics.py
python velocity_kinematics.py