Forward and inverse kinematics, frames, rotations

Forward vs inverse kinematics

Forward kinematics is used to establish the position of the effector based on the configuration of the robot. The individual angles of the arms are parameters that we know and can control.

Inverse kinematics is used in the case where we need to move the robot’s arm from a starting position that we know, to a final one, for which we need to calculate the new angle values.

In case $𝜑$ is not known, inverse kinematics returns 2 possible arm configurations to reach the same point:

$$\begin{array}{c}{𝑞}_2=\pm arccos\left(\frac{{𝑝}_{𝑥}^2+{𝑝}_{𝑦}^2-{𝑙}_1^2-{𝑙}_2^2}{2{𝑙}_1{𝑙}_2}\right)\\ {𝑞}_1=atan2\left({𝑝}_{𝑦},{𝑝}_{𝑥}\right)-atan2\left({𝑙}_{2}sin\left({𝑞}_2\right),{𝑙}_1+{𝑙}_{2}cos\left({𝑞}_2\right)\right)\end{array}$$

Generally speaking, given a manipulator with $𝑛$ joints, a vector of joint variables $𝒒$, and the position $𝒑$ of the effector:

• Forward kinematics is about finding $𝒑=𝑓\left(𝒒\right)$;
• Inverse kinematics is about finding $𝒒={𝑓}^{-1}\left(𝒑\right)$

Reference frames

From now on, we will use the following convention: given reference frame $0$, the point ${𝒑}^0$′s coordinates will be relative to that reference frame (the superscript denotes the frame).

The same point $𝒑$ can of course be associated with different coordinates relative to another reference frame, and in that case it will be denoted ${𝒑}^1$.

A frame is uniquely identified by the position of its origin and by the coordinates of the three unit vectors.

Note

It is common to change reference frames, for example to account for the different positions of different sensors.

Rotation matrices

Let’s take 2 reference frames, with the same origin and one rotated by an angle $𝛼$ with respect to the other.

Rotated frame in blue

We will eventually find that:

$$\begin{array}{c}{𝑝}_{𝑥}^{\prime }={𝑝}_{𝑥}cos\left(𝛼\right)+{𝑝}_{𝑦}sin\left(𝛼\right)\\ {𝑝}_{𝑦}^{\prime }=-{𝑝}_{𝑥}sin\left(𝛼\right)+{𝑝}_{𝑦}cos\left(𝛼\right)\end{array}$$

which means:

$${𝒑}^{\prime }=\left[\begin{array}{cc}cos\left(𝛼\right)& sin\left(𝛼\right)\\ -sin\left(𝛼\right)& cos\left(𝛼\right)\end{array}\right]\times 𝒑$$

This matrix, which is orthogonal (the columns have unit norm and are mutually orthogonal), is easily invertible, since the transpose is equal to the inverse.

Generalization of rotation matrices

Frame $𝑂$, frame ${𝑂}^{\prime }$ and object $𝒑$.

Let’s consider a point belonging to the body. Let $𝒑$ be the vector joining the origin $\mathrm{𝟎}$ with the point in a reference frame with basis $\left\{𝒙,𝒚,𝒛\right\}$, and ${𝒑}^{\prime }$ the vector joining ${\mathrm{𝟎}}^{\prime }$ with the point on a reference frame with basis $\left\{{𝒙}^{\prime },{𝒚}^{\prime },{𝒛}^{\prime }\right\}$. Let $\mathrm{𝟎}{\mathrm{𝟎}}^{\prime }$ be the vector joining the two origins.

We have:

$$\begin{array}{c}{𝑝}_{𝑥}=𝒑\cdot 𝒙={𝒑}^{\prime }\cdot 𝒙+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑥}^{\prime }={𝑝}_{𝑥}^{\prime }\left({𝒙}^{\prime }\cdot 𝒙\right)+{𝑝}_{𝑦}^{\prime }\left({𝒚}^{\prime }\cdot 𝒙\right)+{𝑝}_{𝑧}^{\prime }\left({𝒛}^{\prime }\cdot 𝒙\right)+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑥}^{\prime }\\ {𝑝}_{𝑦}=𝒑\cdot 𝒚={𝒑}^{\prime }\cdot 𝒚+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑦}^{\prime }={𝑝}_{𝑥}^{\prime }\left({𝒙}^{\prime }\cdot 𝒚\right)+{𝑝}_{𝑦}^{\prime }\left({𝒚}^{\prime }\cdot 𝒚\right)+{𝑝}_{𝑧}^{\prime }\left({𝒛}^{\prime }\cdot 𝒚\right)+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑦}^{\prime }\\ {𝑝}_{𝑧}=𝒑\cdot 𝒛={𝒑}^{\prime }\cdot 𝒛+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑧}^{\prime }={𝑝}_{𝑥}^{\prime }\left({𝒙}^{\prime }\cdot 𝒛\right)+{𝑝}_{𝑦}^{\prime }\left({𝒚}^{\prime }\cdot 𝒛\right)+{𝑝}_{𝑧}^{\prime }\left({𝒛}^{\prime }\cdot 𝒛\right)+\mathrm{𝟎}{\mathrm{𝟎}}_{𝑧}^{\prime }\end{array}$$

Which in more compact form is written:

$$\left[\begin{array}{c}{𝑝}_{𝑥}\\ {𝑝}_{𝑦}\\ {𝑝}_{𝑧}\end{array}\right]=𝑹\left[\begin{array}{c}{𝑝}_{𝑥}^{\prime }\\ {𝑝}_{𝑦}^{\prime }\\ {𝑝}_{𝑧}^{\prime }\end{array}\right]+\left[\begin{array}{c}\mathrm{𝟎}{\mathrm{𝟎}}_{𝑥}^{\prime }\\ \mathrm{𝟎}{\mathrm{𝟎}}_{𝑦}^{\prime }\\ \mathrm{𝟎}{\mathrm{𝟎}}_{𝑧}^{\prime }\end{array}\right]$$

where:

$$𝑹=\left[\begin{array}{ccc}{𝑥}_{𝑥}^{\prime }& {𝑦}_{𝑥}^{\prime }& {𝑧}_{𝑥}^{\prime }\\ {𝑥}_{𝑦}^{\prime }& {𝑦}_{𝑦}^{\prime }& {𝑧}_{𝑦}^{\prime }\\ {𝑥}_{𝑧}^{\prime }& {𝑦}_{𝑧}^{\prime }& {𝑧}_{𝑧}^{\prime }\end{array}\right]=\left[\begin{array}{ccc}{𝒙}^{\prime }\cdot 𝒙& {𝒚}^{\prime }\cdot 𝒙& {𝒛}^{\prime }\cdot 𝒙\\ {𝒙}^{\prime }\cdot 𝒚& {𝒚}^{\prime }\cdot 𝒚& {𝒛}^{\prime }\cdot 𝒚\\ {𝒙}^{\prime }\cdot 𝒛& {𝒚}^{\prime }\cdot 𝒛& {𝒛}^{\prime }\cdot 𝒛\end{array}\right]$$

If we compute $𝑹{𝑹}^{𝑇}$ we obtain $𝐼$, which means that ${𝑹}^{𝑇}={𝑹}^{-1}$. So the rotation matrix is an orthogonal matrix with $det\left(𝑹\right)=1$ for right-handed frames and $det\left(𝑹\right)=-1$ for left-handed frames.

Properties of rotation matrices

• Composing (e.g. multiplying) two rotation matrices produces another rotation matrix.
• Rotations are associative: ${𝑹}_1\left({𝑹}_2{𝑹}_3\right)=\left({𝑹}_1{𝑹}_2\right){𝑹}_3$
• Every rotation matrix has a unique inverse matrix.
• Rotations are not commutative.

$SO\left(3\right)$ is the set of all rotation matrices.