Rotation and Orientation

Abstract

Rotation about an arbitrary axis is described by the use of Rodrigues’s formula. Orientation of a coordinate frame with respect to another frame is expressed with the rotation matrix. Orientation of a robot gripper is determined by the use of rotation matrix, RPY and Euler angles, and quaternions. A brief introduction to quaternions is also given in this chapter.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

2.1 Rotation

Rotation represents circular movement about an axis [1]. The point P$_{1}$ is rotated for an angle $\vartheta$ in positive direction about an arbitrary axis, running through the origin of a fixed coordinate frame (Fig. 2.1). Positive rotation around a selected axis in a cartesian frame is defined by the right-hand rule (the thumb is placed in direction of the axis, while the index of the right hand is rotated towards the palm). In a right-handed frame the positive rotations are counter-clockwise. When determining the direction of rotation we must look from the positive end of the axis towards the origin of the frame. The direction of running of athletes on a stadium is also an example of positive rotation. After positive rotation the point comes into a new position P$_{2}$.

Fig. 2.1

Rotation of a point about arbitrary axis

The position of the point P$_{1}$ can be denoted by the vector:

$$\begin{aligned} \mathbf{r }_{1}=\mathrm{\overline{O P_{1}} } \end{aligned}$$

After rotation the point comes into position P$_{2}$:

$$\begin{aligned} \mathbf{r }_{2}=\mathrm{\overline{O P_{2}} } \end{aligned}$$

The direction of rotation is denoted by the unit vector s:

$$\begin{aligned} \mathbf{s }=[\mathrm{s }_{x}, \mathrm{s }_{y}, \mathrm{s }_{z}]^\mathrm{{T}} \end{aligned}$$

The vector s describes the axis of rotation. By equating the following two scalar products, we have:

$$\begin{aligned} \mathbf{r }_{1}^\mathrm{{T}}\mathbf{s }=\mathrm{r }_{1}\cos \alpha = \mathbf{r }_{2}^\mathrm{{T}}\mathbf{s }=\mathrm{r }_{2}\cos \alpha = |\mathrm{\overline{O S_{p}}}| \end{aligned}$$

(2.1)

In Eq. (2.1) $\alpha$ represents the angle between the vectors $\mathbf r _{1}$ and $\mathbf s$ or $\mathbf r _{2}$ and $\mathbf s$. The following difference of the vectors can be seen from Fig. 2.1:

$$\begin{aligned} \mathrm{\overline{S_{p}P_{1}} }= \mathbf{r_{1} }-\mathrm{\overline{O S_{p}} } \end{aligned}$$

From where we can write the Eq. (2.2):

$$\begin{aligned} \mathrm{\overline{S_{p}P_{1}} }&= \mathbf{r }_{1}-(\mathbf{r_{1}^\mathrm{T} }\mathbf{s })\mathbf{s } \\ \mathrm{\overline{S_{p}P_{2}} }&= \mathbf{r }_{2}-(\mathbf{r_{2}^\mathrm{T} }\mathbf{s })\mathbf{s }\nonumber \end{aligned}$$

(2.2)

The relation between the unit vector s and the vector r $_{1}$, describing the absolute value of the cross product, can also be found from Fig. 2.1:

$$\begin{aligned} |\mathbf{s }\times \mathbf{r }_{1}|=|\mathrm{\overline{S_{p}P_{1}} }|=\mathrm{r }_{1}\sin \alpha \end{aligned}$$

Let us now look at the plane, where the rotation between the points P$_{1}$ and P$_{2}$ took place. It is perpendicular to the rotation axis (Fig. 2.2). The point, where the plane and the rotation axis intersect, will be denoted as S$_\mathrm{p }$. There also holds:

Fig. 2.2

The plane perpendicular to the rotation axis

$$\begin{aligned} \mathrm{{\overline{NP_{2}}}} \perp \mathrm{{\overline{S_{p}P_{1}}}} \end{aligned}$$

As the points P$_{1}$ and P$_{2}$ are on the same circular line, we have:

$$\begin{aligned} \mathrm{{|\overline{S_{p}P_{1}}}}| = \mathrm{{ |\overline{S_{p}P_{2}}}}| \end{aligned}$$

from Fig. 2.2 we can see:

$$\begin{aligned} \mathrm{|{\overline{S_{p}N}}}|=\mathrm{|\overline{S_{p}P_{2}}}|\textit{c}\vartheta =\mathrm{|\overline{S_{p}P_{1}}}|\textit{c}\vartheta \end{aligned}$$

In robotics we prefer shorter notation of trigonometric functions $c\vartheta =\cos \vartheta$ and $s\vartheta = \sin \vartheta$. As the vectors $\mathrm{{\overline{S_{p}N}}}$ and $\mathrm{{\overline{S_{p}P_{1}}}}$ have the same direction, we can write also the following vectorial equation:

$$\begin{aligned} \mathrm{\overline{S_{p}N} }=\mathrm{\overline{S_{p}P_{1}} }c\vartheta \end{aligned}$$

(2.3)

In similar way we can see from Fig. 2.2:

$$\begin{aligned} \mathrm{{|\overline{NP_{2}}}}|=\mathrm{{|\overline{S_{p}P_{2}}}}|\textit{s}\vartheta =\mathrm{{|\overline{S_{p}P_{1}}}}|\textit{s}\vartheta =|(\mathbf{s }\times \mathbf{r_{1} })|\textit{s}\vartheta \end{aligned}$$

As the vectors $\mathrm{\overline{NP_{2}} }$ and $(\mathbf{s }\times \mathbf{r_{1} })$ have the same direction, we can write also the following vectorial equation:

$$\begin{aligned} \mathrm{\overline{NP_{2}} }=(\mathbf{s }\times \mathbf{r_{1} })s\vartheta \end{aligned}$$

(2.4)

It is our aim to find the relation between the vectors $\mathbf r _{1}$ and $\mathbf r _{2}$. Let us first write the following sum of vectors:

$$\begin{aligned} \overline{\mathrm{{S}}_{\mathrm{{p}}} \mathrm{{P}}_{\mathrm{2}} }=\overline{\mathrm{{S}}_{\mathrm{{p}}} \mathrm{{N}}}+\overline{\mathrm{{NP}}_{\mathrm{{2}}} } \end{aligned}$$

(2.5)

By inserting the relations (2.2), (2.3) and (2.4) into Eq. (2.5), we obtain:

$$\begin{aligned} \mathbf r {}_{2} - (\mathbf r {}_{2}^\mathrm{T}\mathbf s )\mathbf s = [\mathbf r {}_{1} - (\mathbf r {}_{1}^\mathrm{T}\mathbf s )\mathbf s ] c\vartheta + (\mathbf s \times \mathbf r {}_{1}) s\vartheta \end{aligned}$$

As the vectors r ${}_{1}$ and r ${}_{2}$ are of equal lengths, we can write:

$$\begin{aligned} \mathbf r {}_{1}^\mathrm{T}\mathbf s = \mathbf r {}_{2}^\mathrm{T}\mathbf s \end{aligned}$$

and after rearrangement:

$$\begin{aligned} \mathbf r {}_{2 }= \mathbf r {}_{1} c\vartheta + (\mathbf s \times \mathbf r {}_{1}) s\vartheta + \mathbf s (\mathbf r {}_{1}^\mathrm{T}\mathbf s )(1 - c\vartheta ) \end{aligned}$$

(2.6)

The above equation is known as Rodrigues’s formula in vectorial form. When rewriting the equation for the components, the Rodrigues’s formula can be presented in matrix form:

$$\begin{aligned} \mathbf r_{{2}} =\mathbf{Rr}_{\mathbf{1}} \end{aligned}$$

(2.7)

The matrix $\mathbf R$ describes the rotation about an arbitrary axis. By inserting the components of all three vectors s = [s${}_{x}$, s${}_{y}$, s${}_{z}$]${}^\mathrm{T}$, r ${}_{1}$ = [r${}_{1x}$, r${}_{1y}$, r${}_{1z}$]${}^\mathrm{T}$, and r ${}_{2}$ = [r${}_{2x}$, r${}_{2y}$, r${}_{2z}$]${}^\mathrm{T}$ into the Rodrigues’s formula (2.6) and after calculating the cross and dot products, the following rotation matrixR is obtained:

$$\begin{aligned} \mathbf R = \left[ \begin{array}{ccc} \mathrm s _{x}^{2}v\vartheta +c\vartheta&\mathrm s _{x}\mathrm s _{y}v\vartheta -\mathrm s _{z}s\vartheta&\mathrm s _{x}\mathrm s _{z}v\vartheta +\mathrm s _{y}s\vartheta \\ \\ \mathrm s _{x}\mathrm s _{y}v\vartheta +\mathrm s _{z}s\vartheta&\mathrm s _{y}^{2}v\vartheta +c\vartheta&\mathrm s _{y}\mathrm s _{z}v\vartheta -\mathrm s _{x}s\vartheta \\ \\ \mathrm s _{x}\mathrm s _{z}v\vartheta -\mathrm s _{y}s\vartheta&\mathrm s _{y}\mathrm s _{z}v\vartheta +\mathrm s _{x}s\vartheta&\mathrm s _{z}^{2}v\vartheta +c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.8)

In Eq. (2.8) the following shorter notation of trigonometric function $(1-\cos \vartheta )=v\vartheta$ was used.

The matrix, describing the rotation about an arbitrary axis, is often used in computer graphics or in development of virtual environments. In robotics, however, we always place one of the axes of the cartesian coordinate frame along the axis of the rotational joint. In this way we only use the rotation matrices about the $x$, $y$ and $z$ axes. The rotation matrix $\mathbf R _{x}$, describing the rotation about the $x$ axis, is obtained by inserting the corresponding unit vector $\mathbf s = [1,0,0]^\mathrm{T}$ into Eq. (2.6). We have the following cross product:

$$\begin{aligned} \left(\mathbf s \times r_{{1}} \right) = [0, -r{}_{1z}, r{}_{1y}]{}^\mathrm{T} \end{aligned}$$

and dot product:

$$\begin{aligned} \mathbf r {}_{1}^\mathrm{T}\mathbf s =r{}_{1x} \end{aligned}$$

After inserting the vectors s, r ${}_{1}$ and r ${}_{2}$ together with both products into Eq. (2.6), we obtain the rotation matrix about the $x$ axis:

$$\begin{aligned} \mathbf R _{x}=\left[ \begin{array}{ccc} 1&0&0 \\ 0&c\vartheta&-s\vartheta \\ 0&s\vartheta& \quad c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.9)

When inserting the unit vector s = [0,1,0]${}^\mathrm{T}$, running along the $y$ axis, into the Rodrigues’s formula (2.6), we have:

$$\begin{aligned} \mathbf R _{y}=\left[ \begin{array}{ccc} c\vartheta&0&s\vartheta \\ 0&1&0 \\ -s\vartheta&0&c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.10)

The rotation about the $z$ axis is described by the matrix:

$$\begin{aligned} \mathbf R _{z}=\left[ \begin{array}{ccc} c\vartheta&-s\vartheta&0 \\ s\vartheta&\quad c\vartheta&0 \\ 0&0&1 \\ \end{array} \right] \end{aligned}$$

(2.11)

Let us consider also the inverse problem. The matrix R given, it is our aim to determine the direction of the rotational axis s and the angle of the rotation $\vartheta$. We shall write the rotation matrix in the following general form:

$$\begin{aligned} \mathbf R =\left[\begin{array}{ccc} {r_{11} }&{r_{12} }&{r_{13} } \\ {r_{21} }&{r_{22} }&{r_{23} } \\ {r_{31} }&{r_{32} }&{r_{33} } \end{array}\right] \end{aligned}$$

When summing up the diagonal elements:

$$\begin{aligned} trace(\mathbf R )=r_{11} +r_{22} +r_{33} =1+2c\vartheta \end{aligned}$$

we obtain:

$$\begin{aligned} \vartheta =\arccos \left(\frac{trace(\mathbf R )-1}{2} \right) \end{aligned}$$

(2.12)

The above solution is not uniquely defined. The resulting angle can be also $\vartheta \pm 2\pi n$ and $-\vartheta \pm 2\pi n$. To continue we find the following differences of the off-diagonal elements:

$$\begin{aligned} {r}_{32}- {r}_{23}&= 2\mathrm{s }_{x}s\vartheta \\ r_{13}- r_{31}&= 2\mathrm s _{y}s\vartheta \\ r_{21}- r_{12}&= 2\mathrm s _{z}s\vartheta \end{aligned}$$

When there is $\vartheta$ $\ne$ 0, the rotational axis is given in the following form:

$$\begin{aligned} \mathbf s =\frac{1}{2s\vartheta } \left[\begin{array}{c} {r_{32} -r_{23} } \\ {r_{13} -r_{31} } \\ {r_{21} -r_{12} } \end{array}\right] \end{aligned}$$

(2.13)

When $2\pi - \vartheta$ was selected as the angle of rotation, the rotational axis has a negative sign, i.e. $-\mathbf s$. The axis obtained is called also the equivalent rotational axis.

When the vector r ${}_{1}$ was rotated in the frame $x, y, z$ about the axis s, we obtained the rotated vector r ${}_{2}$ after multiplication with the rotation matrix. When using in Eq. (2.7) another notation for the rotation matrix, we have:

$$\begin{aligned} \mathbf r {}_{2} = \mathbf R {}_{21} \mathbf r {}_{1} \end{aligned}$$

(2.14)

Let us now assume that there is another axis in the same coordinate frame $x, y, z$. After rotating the vector r ${}_{2}$ about this new axis, the vector r ${}_{3}$ is obtained:

$$\begin{aligned} \mathbf r {}_{3} = \mathbf R {}_{32} \mathbf r {}_{2} \end{aligned}$$

(2.15)

After inserting Eq. (2.14) into (2.15), we have:

$$\begin{aligned} \mathbf r {}_{3} = \mathbf R {}_{32}\mathbf R {}_{21} \mathbf r {}_{1} \end{aligned}$$

(2.16)

Successive rotations in the same coordinate frame are described by premultiplication of the rotation matrices.

Let us now consider an example where we shall make use of all knowledge gathered in this chapter. Three successive rotations were performed in the same coordinate frame: first the rotation for $270^{\circ }$ about the $z$ axis, afterwards the rotation for $180^{\circ }$ about the $y$ axis and finally the rotation for $90^{\circ }$ about the $x$ axis. This can be written by the following multiplication of the matrices:

$$\begin{aligned} \mathbf R&= \mathbf R {}_{x,90} \mathbf R {}_{y,180} \mathbf R {}_{z,270}\\&=\left[ \begin{array}{ccc} 1&0&\quad 0 \\ 0&0&-1 \\ 0&1&\quad 0 \\ \end{array} \right] \left[ \begin{array}{lcl} -1&0& \quad 0 \\ \quad 0&1&\quad 0 \\ \quad 0&0&-1 \\ \end{array} \right] \left[ \begin{array}{lcc} \quad 0&1&0 \\ -1&0&0 \\ \quad 0&0&1 \\ \end{array} \right]= \left[ \begin{array}{ccc} \quad 0&-1&0 \\ \quad 0&\quad 0&1 \\ -1&\quad 0&0 \\ \end{array} \right] \end{aligned}$$

The three above rotations can be replaced by a single rotation for a corresponding angle about the equivalent axis. This angle can be calculated from Eq. (2.12). As $trace(\mathbf R )$ in our case equals 0, we have:

$$\begin{aligned} \vartheta =120^{\circ } \end{aligned}$$

After finding the angle of rotation, we calculate the unit vector (2.13) along the rotational axis:

$$\begin{aligned}\mathbf s =\frac{1}{\sqrt{3}}\left[ \begin{array}{c} -1 \\ \quad1 \\ \quad 1 \\ \end{array} \right] \end{aligned}$$

We shall verify the correctness of the calculation by finding the matrix describing the rotation about the equivalent axis. We shall make use of the Rordrigues’s formula (2.6) and again we shall first calculate the cross product:

$$\begin{aligned} (\mathbf s \times \mathbf r_{1} )=\frac{1}{\sqrt{3}}\left[ \begin{array}{c} \mathrm r _{1z}-\mathrm r _{1y} \\ \mathrm r _{1x}+\mathrm r _{1z} \\ -\mathrm r _{1y}-\mathrm r _{1x} \\ \end{array} \right] \end{aligned}$$

and afterwards also the dot product:

$$\begin{aligned} \mathbf r {}_{1}^\mathrm{T}\mathbf s =\frac{1}{\sqrt{3}}(-\mathrm r {}_{1x} + \mathrm r {}_{1y} + \mathrm r {}_{1z}) \end{aligned}$$

The following rotation matrix is obtained:

$$\begin{aligned} \mathbf R =\left[ \begin{array}{ccc} 1-\frac{2}{3}v\vartheta&-\frac{1}{\sqrt{3}}s\vartheta -\frac{1}{3}v\vartheta&\frac{1}{3}s\vartheta -\frac{1}{3}v\vartheta \\ \frac{1}{\sqrt{3}}s\vartheta -\frac{1}{3}v\vartheta&1-\frac{2}{3}v\vartheta&\frac{1}{\sqrt{3}}s\vartheta +\frac{1}{3}v\vartheta \\ -\frac{1}{\sqrt{3}}s\vartheta -\frac{1}{3}v\vartheta&-\frac{1}{\sqrt{3}}s\vartheta +\frac{1}{3}v\vartheta&1-\frac{2}{3}v\vartheta \\ \end{array} \right] \end{aligned}$$

After inserting $\vartheta = 120^{\circ }$ in the above matrix, we obtain the matrix from the beginning of this example:

$$\begin{aligned} \mathbf R =\left[ \begin{array}{ccc} \quad 0&-1&0 \\ \quad 0&\quad 0&1 \\ -1&\quad 0&0 \\ \end{array} \right] \end{aligned}$$

Let us consider another short example. The unit vector i should be rotated for 2$\pi$/3 about the axis running through the origin of the frame and the point [1,1,1]${}^\mathrm{T}$. The unit vector s, which is obtained by normalizing the three components of equal length, is placed along the axis:

$$\begin{aligned} \mathbf s = \frac{1}{\sqrt{3}}[1, 1, 1]^\mathrm{T} \end{aligned}$$

Apart from vector s, we insert into the Rodrigues’s formula (2.6) also: r ${}_{1}$ = [1,0,0]${}^\mathrm{T}$, $c120^{\circ } = -1/2$, and $s120^{\circ } = \sqrt{3}/2$. We write:

$$\begin{aligned} \left[ \begin{array}{c} \mathrm r _{2x} \\ \mathrm r _{2y} \\ \mathrm r _{2z} \\ \end{array} \right]&= \left[ \begin{array}{c} 1 \\ 0 \\ 0 \\ \end{array} \right]\left(-\frac{1}{2}\right)+\frac{1}{\sqrt{3}}\left[ \begin{array}{ccc} \mathbf i&\mathbf j&\mathbf k \\ 1&1&1 \\ 1&0&0 \\ \end{array} \right]\frac{\sqrt{3}}{2}\\&\quad \;+\frac{1}{3}\left[\begin{array}{c} 1 \\ 1 \\ 1 \\ \end{array}\right]\left([\begin{array}{ccc} 1&0&0 \end{array}]\left[\begin{array}{c} 1 \\ 1 \\ 1 \\ \end{array}\right]\right)\left(1+\frac{1}{2}\right) \end{aligned}$$

and the result is:

$$\begin{aligned} \mathbf r _{2} = [0, 1, 0]{}^\mathrm{T} = \mathbf j . \end{aligned}$$

Fig. 2.3

Orientation of the coordinate frame $x{}_{1}, y{}_{1}, z{}_{1}$ with respect to the reference coordinate frame $x{}_{0}, y{}_{0}, z{}_{0}$

2.2 Orientation

Orientation describes in geometrical terms how one object is rotated with respect to the other or how an object is aligned with respect to the reference, usually cartesian, coordinate frame [2, 3]. As a reference frame we shall select the rectangular frame $x{}_{0}, y{}_{0}, z{}_{0}$. Unit vectors ${}^{0}$ i, ${}^{0}$ j, and ${}^{0}$ k describe the selected coordinate frame (Fig. 2.3). In Fig. 2.3 also the rotated coordinate frame $x{}_{1} ,y{}_{1}, z{}_{1}$ with unit vectors ${}^{1}$ i, ${}^{1}$ j, and ${}^{1}$ k is shown. Both coordinate frames coincide in the same origin. Also shown is the point P, which is connected to the frame origin either by vector ${}^{0}$ p, expressed in the frame $x{}_{0}, y{}_{0}, z{}_{0}$, or vector ${}^{1}$ p, expressed in the frame $x{}_{1}, y{}_{1}, z{}_{1}$. Let us describe the position of the point P in the frame $x{}_{0}, y{}_{0}, z{}_{0}$ by the use of vector ${}^{0}$ p with the following equation:

$$\begin{aligned} {}^{0}\mathbf p = {}^{0}\mathrm p {}_{x}{}^{0}\mathbf i + {}^{0}\mathrm p {}_{y}{}^{0}\mathbf j + {}^{0}\mathrm p {}_{z} {}^{0}\mathbf k \end{aligned}$$

Vector ${}^{1}\mathbf p$ belongs to the same point, however in the frame $x{}_{1}, y{}_{1}, z{}_{1}$:

$$\begin{aligned} {}^{1}\mathbf p = {}^{1}\mathrm p {}_{x} {}^{1}\mathbf i + {}^{1}\mathrm p {}_{y}{}^{ 1}\mathbf j + {}^{1}\mathrm p {}_{z} {}^{1}\mathbf k \end{aligned}$$

It is obvious that vectors ${}^{0}$ p and ${}^{1}$ p are equal, as they connect the same origin with the same point P. We shall make use of this property in order to demonstrate the relation between the axes of the coordinate frames $x_{0}, y_{0}, z_{0}$ and $x_{1}, y_{1}, z_{1}$. It is, therefore, our aim to describe the orientation of the frame $x_{1}, y_{1}, z_{1}$ with respect to the frame $x_{0}, y_{0}, z_{0}$. The mathematical relation between the frames $x_{0}, y_{0}, z_{0}$ and $x_{1}, y_{1}, z_{1}$ is obtained by expressing a selected component of vector ${}^{0}$ p in the frame $x_{0}, y_{0}, z_{0}$ by the use of the components of vector ${}^{1}$ p, which is given in the frame $x_{1}, y_{1}, z_{1}$. Let us select first the component ${}^{0}$p${}_{x}$:

$$\begin{aligned} {}^{0}\mathrm p {}_{x} = {}^{0}\mathbf p \,{}^{0}\mathbf i = {}^{1}\mathbf p \, {}^{0}\mathbf i = {}^{1}\mathrm p {}_{x} \,{}^{1}\mathbf i \,{}^{0}\mathbf i + \,{}^{1}\mathrm p {}_{y}\,{}^{1}\mathbf j \,{}^{0}\mathbf i + {}^{1}\mathrm p {}_{z} \,{}^{1}\mathbf k \,{}^{0}\mathbf i \end{aligned}$$

In general we have equivalent expressions also for the components ${}^{0}$p${}_{y}$ and ${}^{0}$p${}_{z}$:

$$\begin{aligned} {}^{0}\mathrm p {}_{y}&= {}^{1}\mathrm p {}_{x}\, {}^{1}\mathbf i \, {}^{0}\mathbf j + {}^{1}\mathrm p {}_{y}\,{}^{1}\mathbf j \, {}^{0}\mathbf j + {}^{1}\mathrm p {}_{z} \,{}^{1}\mathbf k \,{}^{0}\mathbf j \\ {}^{0}\mathrm p {}_{z}&= {}^{1}\mathrm p {}_{x} \,{}^{1}\mathbf i \, {}^{0}\mathbf k + \,{}^{1}\mathrm p {}_{y}\,{}^{1}\mathbf j \,{}^{0}\mathbf k + {}^{1}\mathrm p {}_{z} \,{}^{1}\mathbf k \,{}^{0}\mathbf k \end{aligned}$$

The relation between both coordinate frames, given by the above three equations, can be written in more compact matrix form:

$$\begin{aligned} {}^{0} \mathbf p ={}^{0} \mathbf R _{1} {}^{1} \mathbf p \end{aligned}$$

(2.17)

where ${}^{0}\mathbf p = [{}^{0}$p${}_{x}$, ${}^{0}$p${}_{y}$, ${}^{0}$p${}_{z}$]${}^\mathrm{T}$ and ${}^{1}\mathbf p = [{}^{1}$p${}_{x}$, ${}^{1}$p${}_{y}$, ${}^{1}$p${}_{z}$]${}^\mathrm{T}$. The matrix ${}^{0} \mathbf R _{1}$ is given as follows:

$$\begin{aligned} {}^{\mathrm{0}} \mathbf R _{\mathrm{1}} =\left[ \begin{array}{ccc} {{}^{\mathrm{1}} \mathbf i {}^{\mathrm{0}} \mathbf i }&{{}^{\mathrm{1}} \mathbf j {}^{\mathrm{0}} \mathbf i }&{{}^{\mathrm{1}} \mathbf k {}^{\mathrm{0}} \mathbf i } \\ {{}^{\mathrm{1}} \mathbf i {}^{\mathrm{0}} \mathbf j }&{{}^{\mathrm{1}} \mathbf j {}^{\mathrm{0}} \mathbf j }&{{}^{\mathrm{1}} \mathbf k {}^{\mathrm{0}} \mathbf j } \\ {{}^{\mathrm{1}} \mathbf i {}^{\mathrm{0}} \mathbf k }&{{}^{\mathrm{1}} \mathbf j {}^{\mathrm{0}} \mathbf k }&{{}^{\mathrm{1}} \mathbf k {}^{\mathrm{0}} \mathbf k } \end{array}\right] \end{aligned}$$

(2.18)

The matrix has the dimension $3\times 3$ and represents the transformation of the point P or the corresponding vector ${}^{1}$ p, expressed in the frame $x{}_{1}, y{}_{1}, z{}_{1}$, into the coordinates of the frame $x{}_{0}, y{}_{0}, z{}_{0}$. The above expression describes the orientation of the frame $x{}_{1}, y{}_{1}, z{}_{1}$ with respect to the frame $x{}_{0}, y{}_{0}, z{}_{0}$. As we are dealing with the unit vectors, the elements of the so called rotation matrix are simply the cosines of the angles appertaining to each pair of axes:

$$\begin{aligned} {}^{0} \mathbf R _{1} =\left[\begin{array}{ccc} {\cos \vartheta _{{}^{1} i{}^{0} i} }&{\cos \vartheta _{{}^{1} j{}^{0} i} }&{\cos \vartheta _{{}^{1} k{}^{0} i} } \\ \\ {\cos \vartheta _{{}^{1} i{}^{0} j} }&{\cos \vartheta _{{}^{1} j{}^{0} j} }&{\cos \vartheta _{{}^{1} k{}^{0} j} } \\ \\ {\cos \vartheta _{{}^{1} i{}^{0} k} }&{\cos \vartheta _{{}^{1} j{}^{0} k} }&{\cos \vartheta _{{}^{1} k{}^{0} k} } \end{array}\right] \end{aligned}$$

(2.19)

In a similar way we can determine the position of the point P in the coordinate frame $x{}_{1}, y{}_{1}, z{}_{1}$ from the known coordinates of the same point expressed in the frame $x{}_{0}, y{}_{0}, z{}_{0}$:

$$\begin{aligned} {}^{1}\mathrm p {}_{x} = {}^{1}\mathbf p \, {}^{1}\mathbf i = {}^{0}\mathbf p \, {}^{1}\mathbf i = {}^{0}\mathrm p {}_{x}\, {}^{0}\mathbf i \,{}^{1}\mathbf i + {}^{0}\mathrm p {}_{y} \,{}^{0}\mathbf j \, {}^{1}\mathbf i + {}^{0}\mathrm p {}_{z}\, {}^{0}\mathbf k \,{}^{1}\mathbf i \end{aligned}$$

Similarly, we can write also the expressions for ${}^{1}$p${}_{y}$ and ${}^{1}$p${}_{z}$, so that we have the following matrix equation:

$$\begin{aligned}&{}^{1} \mathbf p ={}^{1} \mathbf R _{0} {}^{0} \mathbf p \\&{}^{1} \mathbf R _{0} = \left[\begin{array}{ccc} { {}^{{0}} \mathbf i {}^{1} \mathbf i }&{{}^{{0}} \mathbf j {}^{1} \mathbf i }&{{}^{{0}} \mathbf k {}^{1} \mathbf i } \\ {{}^{{0}} \mathbf i {}^{1} \mathbf j }&{{}^{{0}} \mathbf j {}^{1} \mathbf j }&{{}^{{0}} \mathbf k {}^{1} \mathbf j } \\ {{}^{{0}} \mathbf i {}^{1} \mathbf k }&{{}^{{0}} \mathbf j {}^{1} \mathbf k }&{{}^{{0}} \mathbf k {}^{1} \mathbf k } \end{array}\right] \end{aligned}$$

The transformation, described by the matrix ${}^{1} \mathbf R _{0}$, is inverse transformation of the matrix ${}^{0} \mathbf R _{1}$. This matrix represents the orientation of the frame $x{}_{0}, y{}_{0}, z{}_{0}$ with respect to the frame $x{}_{1}, y{}_{1}, z{}_{1}$. As the dot product is commutative (e.g. ${}^{0}$ i ${}^{1}$ j = ${}^{1}$ j ${}^{0}$ i), we can write the following equality:

$$\begin{aligned} {}^{1} \mathbf R _{0} =({}^{0} \mathbf R _{1} )^{-1} =({}^{0} \mathbf R _{1} )^\mathrm{T} \end{aligned}$$

(2.20)

The matrix, whose inverse matrix is equal to its transposed matrix, is called orthogonal matrix. The transformation matrix ${}^{1} \mathbf R _{0}$ will be therefore called orthogonal transformation matrix. As the determinants of the matrices ${}^{1} \mathbf R _{0}$ and ${}^{0} \mathbf R _{1}$ are equal $\det {}^{0} \mathbf R _{1} =\det ({}^{0} \mathbf R _{1} )^\mathrm{T}$ and their product equals 1, also both determinants are either $+1$ or $-1$. In the right-handed coordinate frame the determinant is equal to $+1$. The orthogonal matrices with the value of the determinant $+1$ or $-1$ are named rotation matrices.

Fig. 2.4

Two coordinate frames rotated about the x${}_{0}$ axis

Let us consider the example from Fig. 2.4 and calculate the rotation matrix representing orientation of the frame $x{}_{1}, y{}_{1}, z{}_{1}$, which is rotated for the angle $+\vartheta$ with respect to the frame $x{}_{0}, y{}_{0}, z{}_{0}$. We are dealing with the following non-zero products of the unit vectors:

$$\begin{aligned} {}^{0}\mathbf i \, {}^{1}\mathbf i&= 1\\ {}^{0}\mathbf j \, {}^{1}\mathbf j&= \cos \vartheta \\ {}^{0}\mathbf k \, {}^{1}\mathbf k&= \cos \vartheta \\ {}^{0}\mathbf j \, {}^{1}\mathbf k&= - \sin \vartheta \\ {}^{0}\mathbf k \, {}^{1}\mathbf j&= \sin \vartheta \end{aligned}$$

The rotation matrix can be written in the following form:

$$\begin{aligned} \mathbf R _{x}=\left[ \begin{array}{ccc} 1&0&\quad 0 \\ 0&c\vartheta&-s\vartheta \\ 0&s\vartheta&\quad c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.21)

In the same way as we determined the matrix describing the orientation obtained after the rotation about the $x$ axis, we shall calculate the rotation matrix about the $y$ axis:

$$\begin{aligned} \mathbf R _{y}=\left[ \begin{array}{lcc} \quad c\vartheta&0&s\vartheta \\\quad 0&1&0 \\ -s\vartheta&0&c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.22)

and finally the rotation matrix about the $z$ axis:

$$\begin{aligned} \mathbf R _{z}=\left[ \begin{array}{clc} c\vartheta&-s\vartheta&0 \\ s\vartheta&\quad c\vartheta&0 \\ 0&\quad 0&1 \\ \end{array} \right] \end{aligned}$$

(2.23)

We can see that the matrices describing the orientation of two coordinate frames rotated about $x$, $y$, or $z$ axis of cartesian coordinate frame are the same as the matrices describing rotation, obtained in previous chapter.

Let us imagine point P expressed in three different coordinate frames by the use of vectors ${}^{0}$ p, ${}^{1}$ p, and ${}^{2}$ p. The relation between particular vectors is given with the following equations:

$$\begin{aligned} {}^{0} \mathbf p&={}^{0} \mathbf R _{1} {}^{1} \mathbf p \\ {}^{1} \mathbf p&={}^{1} \mathbf R _{2} {}^{2} \mathbf p \end{aligned}$$

The coordinate frames $x{}_{0}, y{}_{0}, z{}_{0}$, $\;x{}_{1}, y{}_{1}, z{}_{1}$, and $x{}_{2}, y{}_{2}, z{}_{2}$ have the origin in the same point, however they are rotated one with respect to the other. After inserting vector ${}^{1}$ p from the second into the first equation, we obtain the equation describing relative position of vector ${}^{2}$ p with respect to the frame $x{}_{0}, y{}_{0}, z{}_{0}$:

$$\begin{aligned} {}^{\mathrm{0}} \mathbf p&= {}^{\mathrm{0}} \mathbf R _{\mathrm{1}} {}^{\mathrm{1}} \mathbf R _{\mathrm{2}} {}^{\mathrm{2}} \mathbf p \\ \nonumber {}^{\mathrm{0}} \mathbf R _{\mathrm{2}}&= {}^{\mathrm{0}} \mathbf R _{\mathrm{1}} {}^{\mathrm{1}} \mathbf R _{\mathrm{2}} \end{aligned}$$

(2.24)

This is different from the previous chapter, where we considered consecutive rotations about different axes of the same coordinate frame. The consecutive orientations of several coordinate frames are described by the postmultiplication of the rotation matrices. We must have in mind, that consecutive orientations are related to the previous (relative) coordinate frame.

Fig. 2.5

Orientation of robot gripper

The notion of orientation is in robotics mostly related to the orientation of the robot gripper. A coordinate frame with three unit vectors n, s, and a, describing the orientation of the gripper, is placed between both fingers (Fig. 2.5). The $z$ axis vector lays in the direction of the approach of the gripper to the object. It is denoted by vector a (approach). Vector, which is aligned with $y$ axis, describes the direction of sliding of the fingers and is denoted as s (slide). The third vector completes the right-handed coordinate frame and is called normal. There is $\mathbf n =\mathbf{s}\times \mathbf{a}$. The matrix describing the orientation of the gripper with respect to the reference frame $x{}_{0}, y{}_{0}, z{}_{0}$ has the following form:

$$\begin{aligned} \mathbf R =\left[ \begin{array}{ccc} \mathrm n _{x}&\mathrm s _{x}&\mathrm a _{x} \\ \mathrm n _{y}&\mathrm s _{y}&\mathrm a _{y} \\ \mathrm n _{z}&\mathrm s _{z}&\mathrm a _{z} \\ \end{array} \right] \end{aligned}$$

(2.25)

The element $\mathrm n {}_{x}$ of the matrix (2.25) denotes the projection of the unit vector n on the $x{}_{0}$ axis of the reference frame or, when considering the matrix (2.19), the cosine of the angle between the axes $x$ and $x{}_{0}$. The same is valid for the eight other elements of the orientation matrixR. To describe the orientation of an object we do not need all nine elements of the matrix. The left column vector is the cross product of vectors s and a. The vectors s and a are unit vectors which are perpendicular with respect to each other, so that we have:

$$\begin{aligned} \mathbf s \cdot \mathbf s&=1\\ \mathbf a \cdot \mathbf a&=1\\ \mathbf s \cdot \mathbf a&=0 \end{aligned}$$

Three elements are, therefore, sufficient to describe the orientation. The orientation is often described by the following sequence of rotations:

$$\begin{aligned}&\mathrm {R}: \text{ roll---about}\, z \,\text{ axis}\\&\mathrm {P}: \text{ pitch---about}\, y \,\text{ axis}\\&\mathrm {Y}: \text{ yaw---about}\, x \,\text{ axis} \end{aligned}$$

This description is mostly used with orientation of a ship or airplane. Let us imagine that the airplane flies along $z$ axis and that the coordinate frame is positioned into the center of the airplane. Then, R represents the rotation $\varphi$ about $z$ axis, P belongs to the rotation $\vartheta$ about $y$ axis and Y to the rotation $\psi$ about $x$ axis, as shown in Fig. 2.6. All rotations are performed with respect to a fixed reference frame.

Fig. 2.6

RPY angles for the case of an airplane

Fig. 2.7

RPY angles for the case of robot gripper

The meaning of RPY angles for the case of robot gripper is shown in Fig. 2.7. As it can be realized from Figs. 2.6 and 2.7, the RPY orientation is defined with respect to a fixed coordinate frame. In Sect. 2.1 we learned, that consecutive rotations about different axes of the same coordinate frame can be described by the premultiplication of the rotation matrices, or with another words the rotations are performed in the reverse order. We start with the rotation $\varphi$ about $z$ axis, continue with rotation $\vartheta$ about $y$ axis and finish with the rotation $\psi$ about $x$ axis. The reverse order of rotations is evident also from the name of RPY angles. The orientation matrix, which belongs to the RPY angles, is obtained by the following multiplication of the rotation matrices:

$$\begin{aligned} \mathbf{RPY }(\varphi ,\vartheta ,\psi )&=Rot(z,\varphi ) Rot(y,\vartheta ) Rot(x,\psi )\nonumber \\&= \left[ \begin{array}{clc} c\varphi&-s\varphi&0 \\ s\varphi&\quad c\varphi&0 \\ 0&\quad 0&1 \\ \end{array} \right] \left[ \begin{array}{lcc} \quad c\vartheta&0&s\vartheta \\ \quad 0&1&0 \\ -s\vartheta&0&c\vartheta \\ \end{array} \right] \left[ \begin{array}{ccl} 1&0&\quad 0 \\ 0&c\psi&-s\psi \\ 0&s\psi&\quad c\psi \\ \end{array} \right]\nonumber \\&=\left[ \begin{array}{ccc} c\varphi c\vartheta&c\varphi s\vartheta s\psi -s\varphi c\psi&c\varphi s\vartheta c\psi +s\varphi s\psi \\ s\varphi s\vartheta&s\varphi s\vartheta s\psi +c\varphi c\psi&s\varphi s\vartheta c\psi -c\varphi c\psi \\ -s\vartheta&c\vartheta s\psi&c\vartheta c\psi \\ \end{array} \right] \end{aligned}$$

(2.26)

Equation (2.26) calculates the rotation matrix from the corresponding RPY angles. Let us consider also the inverse problem, i.e. an example of calculating the RPY angles from a given matrix. Let us assume that the matrix (2.27) describes the orientation of a gripper in the reference coordinate frame which is attached to the base of a robot manipulator. It was calculated by the use of geometric robot model which will be studied in the following chapters of the textbook. The matrix is obtained by inserting the readings of the joint angles obtained from joint sensors, while the robot is in a selected pose. Let us assume the following simple matrix:

$$\begin{aligned} \mathbf R =\left[ \begin{array}{ccc} 1&0&0 \\ 0&0.5&0.866 \\ 0&-0.866&0.5 \\ \end{array} \right] \end{aligned}$$

(2.27)

It is our aim to calculate the RPY angles of the gripper with respect to the reference frame. We shall first write a general form for the orientation matrix of the gripper and equate it to the RPY matrix:

$$\begin{aligned} \left[\begin{array}{ccc} {\mathrm{r}_{{1x}} }&{\mathrm{r}_{{2x}} }&{\mathrm{r}_{{3x}} } \\ {\mathrm{r}_{{1y}} }&{\mathrm{r}_{{2y}} }&{\mathrm{r}_{{3y}} } \\ {\mathrm{r}_{{1z}} }&{\mathrm{r}_{{2z}} }&{\mathrm{r}_{{3z}} } \end{array}\right]=\left[\begin{array}{ccc} {\mathrm{c}\varphi \mathrm{c}\vartheta }&{\mathrm{c}\varphi \mathrm{s}\vartheta \mathrm{s\psi }-\mathrm{s}\varphi \mathrm{c\psi }}&{\mathrm{c}\varphi \mathrm{s}\vartheta \mathrm{c\psi }+\mathrm{s}\varphi \mathrm{s\psi }} \\ {\mathrm{s}\varphi \mathrm{c}\vartheta }&{\mathrm{s}\varphi \mathrm{s}\vartheta \mathrm{s\psi }+\mathrm{c}\varphi \mathrm{c\psi }}&{\mathrm{s}\varphi \mathrm{s}\vartheta \mathrm{c\psi }-\mathrm{c}\varphi \mathrm{s\psi }} \\ {-\mathrm{s}\vartheta }&{\mathrm{c}\vartheta \mathrm{s\psi }}&{\mathrm{c}\vartheta \mathrm{c\psi }} \end{array}\right] \end{aligned}$$

When using the most simple solutions for the elements r${}_{1z}$, r${}_{2z}$, and r${}_{3z}$, in many cases singularities or inaccurate results are obtained. The accuracy of calculation of an angle depends on its magnitude. Specially inappropriate is dividing by small angle values. Let us first find the equation for the angle $\vartheta$:

$$\begin{aligned} \sin \vartheta&= -\mathrm{r}_{1z}\nonumber \\ {\cos }^\mathrm{2} \vartheta&= \frac{\mathrm{r}^{2}_{1x} +\mathrm{r}^{2}_{1y}+\mathrm{r}^{2}_{2z} + \mathrm{r}^{2}_{3z}}{2} \nonumber \\ \vartheta&= {\arctan }\frac{-\mathrm{r}_{{1z}} }{\sqrt{\frac{{1}}{{2}} \left(\mathrm{r}^{{2}} _{{1x}} +\mathrm{r}^{{2}} _{{1y}} +\mathrm{r}^{{2}} _{{2z}} +\mathrm{r}^{{2}} _{{3z}} \right)} } \end{aligned}$$

(2.28)

When calculating the angle $\varphi$, we make use of the following trigonometrical expressions:

$$\begin{aligned} \mathrm{r}_{{2z}} \mathrm{r}_{{3x}}&= \mathrm{c}\vartheta {s\psi } \left({s}\varphi {s\psi }+{c}\varphi {s}\vartheta {c\psi }\right)\nonumber \\ \mathrm{r}_{{3z}} \mathrm{r}_{{2x}}&= {c}\vartheta {c\psi } \left(-{s}\varphi {c\psi }+{c}\varphi {s}\vartheta {s\psi }\right)\nonumber \\ \mathrm{r}_{{2z}} \mathrm{r}_{{3x}} -\mathrm{r}_{{3z}} \mathrm{r}_{{2x}}&= {s}\varphi {c}\vartheta \nonumber \\ \mathrm{r}_{{3z}} \mathrm{r}_{{2y}}&= {c}\vartheta {c\psi } \left({c}\varphi {c\psi }+{s}\varphi {s}\vartheta {s\psi }\right)\nonumber \\ \mathrm{r}_{{2z}} \mathrm{r}_{{3y}}&= {\cos }\vartheta {s\psi } \left(-{c}\varphi {s\psi }+{s}\varphi {s}\vartheta {c\psi }\right)\nonumber \\ \mathrm{r}_{{3z}} \mathrm{r}_{{2y}} -\mathrm{r}_{{2z}} \mathrm{r}_{{3y}}&= {c}\varphi {c}\vartheta \nonumber \\&\nonumber \\ \varphi&= {\arctan }\frac{\mathrm{r}_{{2z}} \mathrm{r}_{{3x}} -\mathrm{r}_{{3z}} \mathrm{r}_{{2x}} }{\mathrm{r}_{{3z}} \mathrm{r}_{{2y}} -\mathrm{r}_{{2z}} \mathrm{r}_{{3y}} } \end{aligned}$$

(2.29)

In a similar way we find also the angle $\psi$:

$$\begin{aligned} \mathrm{r}_{{1y}} \mathrm{r}_{{3x}}&= {s}\varphi {c\psi } \left({s}\varphi {s\psi }+{c}\varphi {s}\vartheta {c\psi }\right)\nonumber \\ \mathrm{r}_{{1x}} \mathrm{r}_{{3y}}&= {c}\varphi {c}\vartheta \left(-{c}\varphi {s\psi }+{s}\varphi {s}\vartheta {c\psi }\right)\nonumber \\ \mathrm{r}_{{1y}} \mathrm{r}_{{3x}} -\mathrm{r}_{{1x}} \mathrm{r}_{{3y}}&= {c}\vartheta {s\psi }\nonumber \\ \mathrm{r}_{{1x}} \mathrm{r}_{{2y}}&= {c}\varphi {c}\vartheta \left({c}\varphi {c\psi }+{s}\varphi {s}\vartheta {s\psi }\right)\nonumber \\ \mathrm{r}_{{1y}} \mathrm{r}_{{2x}}&= {s}\varphi {c}\vartheta \left(-{s}\varphi {c\psi }+{c}\varphi {s}\vartheta {s\psi }\right)\nonumber \\ \mathrm{r}_{{1x}} \mathrm{r}_{{2y}} - \mathrm{r}_{{1y}} \mathrm{r}_{{2x}}&= {c}\vartheta {c\psi }\nonumber \\&\nonumber \\ \mathrm{\psi }&= {\arctan }\frac{\mathrm{r}_{{1y}} \mathrm{r}_{{3x}} -\mathrm{r}_{{1x}} \mathrm{r}_{{3y}} }{\mathrm{r}_{{1x}} \mathrm{r}_{{2y}} -\mathrm{r}_{{1y}} \mathrm{r}_{{2x}} } \end{aligned}$$

(2.30)

Fig. 2.8

Orientation of robot gripper

Let us go back to the numerical example where the matrix (2.27) represents the orientation of the gripper. When calculating the value of the angle $\vartheta$, we can notice, that the numerator (r${}_{1z}$) equals zero, while the denominator is non-zero, therefore $\vartheta = 0$. The same is valid for the angle $\varphi = 0$, while the angle $\psi = - 60^{\circ }$. The orientation of the gripper with respect to the reference frame is shown in Fig. 2.8. The gripper lays in the $y{}_{0}, z{}_{0}$ plane. From the figure we can read the angles between the axes of the reference and gripper coordinate frame:

• n${}_{x} = \cos 0^{\circ }$, s${}_{x} = \cos 90^{\circ }$, a${}_{x} = \cos 90^{\circ }$

• n${}_{y} = \cos 90^{\circ }$, s${}_{y} = \cos 60^{\circ }$, a${}_{y} = \cos 30^{\circ }$

• n${}_{z} = \cos 90^{\circ }$, s${}_{z} = \cos 150^{\circ }$, a${}_{z} = \cos 60^{\circ }$

We can see that this is the original matrix (2.27).

The orientation can be described also by the help of the Euler angles, where we first perform the rotation $\varphi$ about the $z$ axis, afterwards the rotation $\vartheta$ about the new $y$ axis and finally the rotation $\psi$ about the momentary $z$ axis (Fig. 2.9). As now the rotations were performed about the axes of the momentary coordinate frame, we make use of postmultiplications. The Euler matrix is obtained as follows:

$$\begin{aligned} \mathbf{Euler }(\varphi ,\vartheta ,\psi )&=Rot(z,\varphi ) Rot(y^{\prime },\vartheta ) Rot(z^{\prime \prime },\psi )\nonumber \\&= \left[ \begin{array}{clc} c\varphi&-s\varphi&0 \\ s\varphi&\quad c\varphi&0 \\ 0&\quad 0&1 \\ \end{array} \right] \left[ \begin{array}{lcc} \quad c\vartheta&0&s\vartheta \\ \quad 0&1&0 \\ -s\vartheta&0&c\vartheta \\ \end{array} \right] \left[ \begin{array}{clc} c\psi&-s\psi&0 \\ s\psi&\quad c\psi&0 \\ 0&\quad 0&1 \\ \end{array} \right]\nonumber \\&=\left[ \begin{array}{ccc} c\varphi c\vartheta c\psi - s\varphi s\psi&-c\varphi c\vartheta s\psi -s\varphi c\psi&c\varphi s\vartheta \\ s\varphi c\vartheta c\psi + c\varphi s\psi&-s\varphi c\vartheta s\psi +c\varphi c\psi&s\varphi s\vartheta \\ -s\vartheta c\psi&s\vartheta s\psi&c\vartheta \\ \end{array} \right] \end{aligned}$$

(2.31)

The orientation described by the matrix (2.31), is called also Z-Y-Z Euler angles. Euler’s theorem says, that two independent orthonormal coordinate frames can be aligned to each other through a sequence of three rotations about the coordinate axes, where two consecutive rotations cannot be made about the same axis. In this way 12 different rotations are possible: X-Y-Z, X-Z-Y, X-Y-X, X-Z-X, Y-Z-X, Y-X-Z, Y-Z-Y, Y-X-Y, Z-X-Y, Z-Y-X, Z-X-Z , and our Z-Y-Z. Twelve different rotations are also possible when describing the rotations in a fixed reference frame, however usually the described RPY angles are used. Finally let us also state that three rotations about the axes of the fixed coordinate frame represent the same orientation as the same three rotations performed in reverse order about the three axes of the momentary or relative coordinate frame.

Fig. 2.9

Euler angles

2.3 Quaternions

We learned that rotation and orientation can be described either by rotation matrices or by RPY and Euler angles. In the first case we need 9 parameters, while only 3 parameters are required in the latter two cases. The matrices are convenient for computations, however they do not provide fast and clear image of e.g. orientation of a robot gripper in the space. RPY and Euler angles nicely present the orientation of a gripper, but they are not appropriate for calculations. In this chapter we shall learn that quaternions are appropriate for either calculation of rotation or description of orientation [4, 5].

Quaternions are represented by four real numbers, with operations of addition and multiplication defined by special rules which we will learn in this chapter. Quaternions are generalization of the complex numbers. Complex numbers enable operations with two-dimensional vectors, while by the use of quaternions four-dimensional vectors can be dealt with.

The quaternions can be written in various ways. The simplest is the following expression:

$$\begin{aligned} q=q_{0} 1+q_{1} \mathbf i +q_{2} \mathbf j +q_{3} \mathbf k \end{aligned}$$

(2.32)

In the above equation $q_{i}$ are real numbers, while $\mathbf i$, $\mathbf j$, $\mathbf k$ correspond to the unit vectors along the axes of the cartesian coordinate frame.

The sum of quaternions is obtained in the following way:

$$\begin{aligned} p+q=\left(p_{0} +q_{0} \right)\; 1+\left(p_{1} +q_{1} \right) \mathbf i +\left(p_{2} +q_{2} \right) \mathbf j +\left(p_{3} +q_{3} \right) \mathbf k , \end{aligned}$$

product of a quaternion and scalar is:

$$\begin{aligned} wq = wq_{0} +wq_{1} \mathbf i +wq_{2} \mathbf j +wq_{3} \mathbf k \end{aligned}$$

Quaternion conjugate has the following form:

$$\begin{aligned} q^{*} = q_{0} 1-q_{1} \mathbf i -q_{2} \mathbf j -q_{3} \mathbf k \end{aligned}$$

(2.33)

The equation reminds us on complex conjugate. Similar observation is true also for the following rules, which we shall use when developing the product of two quaternions:

$$\begin{aligned} \mathbf i ^{2} =\mathbf j ^{2} =\mathbf k ^{2} =\mathbf{ijk }=-1 \end{aligned}$$

The multiplication of quaternions is not commutative. When multiplying two quaternions we shall make use of the following table:

*	1	i	j	k
1	1	i	j	k
i	i	$-1$	k	$-$ j
j	j	$-$ k	$-1$	i
k	k	j	$-$ i	$-1$

Quaternions can be written in another way, where the sum of the scalar part $q_{0}$ and the vector part q is emphasized:

$$\begin{aligned} q=q_{0} +\mathbf q \end{aligned}$$

(2.34)

The vector q can be written in the usual form

$$\begin{aligned} \mathbf q =q_{1} \mathbf i +q_{2} \mathbf j +q_{3} \mathbf k \end{aligned}$$

Let us calculate first the product of two quaternions:

$$\begin{aligned} pq&=\left(p_{0} +p_{1} \mathbf i +p_{2} \mathbf j +p_{3} \mathbf k \right)\; \left(q_{0} +q_{1} \mathbf i +q_{2} \mathbf j +q_{3} \mathbf k \right) \\&=p_{0} q_{0} +q_{0} \left(p_{1} \mathbf i +p_{2} \mathbf j +p_{3} \mathbf k \right)+p_{0} \left(q_{1} \mathbf i +q_{2} \mathbf j +q_{3} \mathbf k \right) \\&\quad \;+p_{1} q_{1} \mathbf i ^{2} +p_{2} q_{1} \mathbf{ji }+p_{3} q_{1} \mathbf{ki } \\&\quad \;+ p_{1} q_{2}\mathbf{ij }+p_{2} q_{2} \mathbf j ^{2} +p_{3} q_{2} \mathbf{kj } \\&\quad \;+ p_{1} q_{3} \mathbf{ik }+p_{2} q_{3} \mathbf{jk }+p_{3} q_{3} \mathbf k ^{2} \end{aligned}$$

After applying the rules, defining the algebra of calculations with quaternions, we obtain:

$$\begin{aligned} pq&=p_{0} q_{0} +p_{0} \mathbf q +q_{0} \mathbf p \\&\quad \;-p_{1} q_{1} -p_{2} q_{2} -p_{3} q_{3}\\&\quad \;+\left(p_{2} q_{3} -p_{3} q_{2} \right)\; \mathbf i \\&\quad \;+\left(p_{3} q_{1} -p_{1} q_{3} \right)\; \mathbf j \\&\quad \;+\left(p_{1} q_{2} -p_{2} q_{1} \right)\; \mathbf k \end{aligned}$$

The second row of the above equation represents a dot product, while the last three rows belong to the cross product of the vectors p and q. In this way we can write the product of two quaternions in the following form:

$$\begin{aligned} pq=p_{0} q_{0} -\mathbf p \cdot \mathbf q +p_{0} \mathbf q +q_{0} \mathbf p +\mathbf p \times \mathbf q \end{aligned}$$

(2.35)

After exchanging the factors, we obtain:

$$\begin{aligned} qp=q_{0} p_{0} -\mathbf q \cdot p +p_{0} \mathbf q +q_{0} \mathbf p +\mathbf q \times \mathbf p \end{aligned}$$

Because of the cross product in the last summand, the multiplication of two quaternions is not commutative. The multiplication of quaternions is sufficiently complex, so that mistakes are quite frequent. We shall develop another formula which is more error resistant. The first two summands in the right side of Eq. (2.35) represent the following scalar:

$$\begin{aligned} r_{0} =p_{0} q_{0} -p_{1} q_{1} -p_{2} q_{2} -p_{3} q_{3} \end{aligned}$$

The other three summands can be written in the following form of columns:

$$\begin{aligned} \left[\begin{array}{c} {r_{1} } \\ {r_{2} } \\ {r_{3} } \end{array}\right]=\left[\begin{array}{c} {p_{0} q_{1} } \\ {p_{0} q_{2} } \\ {p_{0} q_{3} } \end{array}\right]+\left[\begin{array}{c} {q_{0} p_{1} } \\ {q_{0} p_{2} } \\ {q_{0} p_{3} } \end{array}\right]+\left[\begin{array}{c} {p_{2} q_{3} -p_{3} q_{2} } \\ {p_{3} q_{1} -p_{1} q_{3} } \\ {p_{1} q_{2} -p_{2} q_{1} } \end{array}\right] \end{aligned}$$

Both expressions can be transformed into the following matrix form:

$$\begin{aligned} \left[\begin{array}{c} {r_{0} } \\ {r_{1} } \\ {r_{2} } \\ {r_{3} } \end{array}\right]= \left[\begin{array}{clll} {p_{0} }&{-p_{1} }&{-p_{2} }&{-p_{3} } \\ {p_{1} }&\quad {p_{0} }&{-p_{3} }&\quad {p_{2} } \\ {p_{2} }&\quad {p_{3} }&\quad {p_{0} }&{-p_{1} } \\ {p_{3} }&{-p_{2} }&\quad {p_{1} }&\quad{p_{0} } \end{array}\right] \left[\begin{array}{c} {q_{0} } \\ {q_{1} } \\ {q_{2} } \\ {q_{3} } \end{array}\right] \end{aligned}$$

(2.36)

With this kind of multiplying the quaternions there is less chance to make a mistake. As a numerical example let us multiply two quaternions in three different ways. First, we shall only make use of the rules from the table. Because of the risk to make a mistake, we shall multiply step by step:

$$\begin{aligned}&(2 +3\mathbf i - \mathbf j + 5\mathbf k )(3 - 4\mathbf i + 2\mathbf j + \mathbf k )\\&= 6 + 9\mathbf i - 3\mathbf j + 15\mathbf k \\&\quad \; - 8\mathbf i - 12\mathbf i {}^{2} + 4\mathbf{ji } - 20\mathbf{ki }\\&\quad \;+ 4\mathbf j + 6\mathbf{ij } - 2\mathbf j {}^{2} + 10\mathbf{kj }\\&\quad \;+ 2\mathbf k + 3\mathbf{ik } - \mathbf{jk } + 5\mathbf k {}^{2}\\&= 6 + 9\mathbf i - 3\mathbf j + 15\mathbf k \\&\quad \;- 8\mathbf i + 12 - 4\mathbf k - 20\mathbf j \\&\quad \;+ 4\mathbf j + 6\mathbf k + 2 - 10\mathbf i \\&\quad \;+ 2\mathbf k - 3\mathbf j - \mathbf i - 5 = 15 - 10\mathbf i - 22\mathbf j + 19\mathbf k \end{aligned}$$

The same result is obtained by the use of Eq. (2.35):

$$\begin{aligned}&\left(2+\left[ \begin{array}{l} \quad 3 \\ -1 \\ \quad 5 \\ \end{array} \right]\right)\left(3+\left[ \begin{array}{l} -4 \\\quad 2 \\ \quad 1 \\ \end{array} \right]\right)\\&\quad \;=6-\left[ \begin{array}{lll} 3&-1&5 \\ \end{array} \right]\left[ \begin{array}{l} -4 \\ \quad 2 \\ \quad 1 \\ \end{array} \right]+2\left[ \begin{array}{l} -4 \\ \quad 2 \\ \quad 1 \\ \end{array} \right]+3\left[ \begin{array}{l} \quad 3 \\ -1 \\ \quad 5 \\ \end{array} \right]+\left[ \begin{array}{llc} \quad \mathbf{i}& \quad \mathbf{j}& \mathbf {k} \\ \quad 3&-1&5 \\ -4&\quad 2&1 \\ \end{array} \right] \\&\quad \;=6+9+\left[ \begin{array}{c} 1 \\ 1 \\ 17 \\ \end{array} \right]+\left[ \begin{array}{l} -11 \\ -23 \\ \quad 2 \\ \end{array} \right]=15-\left[ \begin{array}{l} -10 \\ -22 \\ \quad 19 \\ \end{array} \right] \end{aligned}$$

Finally, we shall make use of Eq (2.36):

$$\begin{aligned} \left[ \begin{array}{llll} \quad 2&-3&\quad 1&-5 \\ \quad 3&\quad 2&-5&-1 \\ -1&\quad 5&\quad 2&-3 \\ \quad 5&\quad1&\quad 3& \quad2 \\ \end{array} \right] \left[ \begin{array}{l} \quad 3 \\ -4 \\ \quad 2 \\ \quad 1 \\ \end{array} \right]=\left[ \begin{array}{l} \quad 15 \\ -10 \\ -22 \\ \quad 19 \\ \end{array} \right] \end{aligned}$$

Until now we learned how to rotate vector r ${}_{1}$ into a new position r ${}_{2}$ by using Rodrigues’s formula (2.6) or rotation matrix (2.7). Now we shall do the same by the use of quaternions:

$$\begin{aligned} \mathrm{r}_{2}=q \mathrm{r}_{1} q^{*} \end{aligned}$$

(2.37)

The quaternions from Eq. (2.37) have the following meaning:

$$\begin{aligned}&q = q_{0} + \mathbf q \\&r{}_{1} = 0 + \mathbf r {}_{1}\\&q^{*} = q_{0} - \mathbf q \\&r{}_{2} = 0 + \mathbf r {}_{2}\\ \end{aligned}$$

We shall demonstrate that the expression (2.37) is equivalent to the description of rotation with the matrix Eq. (2.7). Let us perform both quaternion multiplications, as required by Eq. (2.37):

$$\begin{aligned} (0 + \mathbf r {}_{2}) = (q_{0} + \mathbf q )(0 + \mathbf r {}_{1}) q^{*} = (- \mathbf q \cdot \mathbf r {}_{1} + (q_{0} \mathbf r {}_{1} + \mathbf q \times \mathbf r {}_{1}))(q_{0} - \mathbf q ) \end{aligned}$$

Before performing the second multiplication, we must know, that the first summand in the first brackets of the above equation is a scalar, while the other two represent a vector. We multiply by the use of Eq. (2.35):

$$\begin{aligned} \mathbf r _{2}&= - \mathbf q \cdot \mathbf r _{1}q_{0}\nonumber \\&\quad \; + q_{0}\mathbf r _{1}\cdot \mathbf q +(\mathbf q \times \mathbf r _{1})\cdot \mathbf q \nonumber \\&\quad \; + (\mathbf q \cdot \mathbf r _{1})\mathbf q +q^{2}_{0}\mathbf r _{1}+q_{0}(\mathbf q \times \mathbf r _{1})\nonumber \\&\quad \; - q_{0}\mathbf r _{1}\times \mathbf q -\mathbf q \times \mathbf r _{1}\times \mathbf q \nonumber \end{aligned}$$

In the above equation we first subtract the first two summands. The third summand is zero. We exchange the factors of the cross product in the seventh summand and add it to the sixth summand. The last summand is expressed according to the formula $-(\mathbf q \cdot \mathbf q ) \cdot \mathbf r {}_{1} + (\mathbf r {}_{1} \cdot \mathbf q ) \cdot \mathbf q$, which can be found in every mathematical reference book. After little rearranging we have:

$$\begin{aligned} \mathbf r _{2}=q^{2}_{0}\mathbf r _{1}-(\mathbf q \cdot \mathbf q )\mathbf r _{1}+2q_{0}(\mathbf q \times \mathbf r _{1}) + 2\mathbf q (\mathbf q \cdot \mathbf r _{1}) \end{aligned}$$

(2.38)

From the above equation we wish to expose r ${}_{1}$. We replace the cross product by the multiplication with a skew symmetric matrix:

$$\begin{aligned} (\mathbf q \times \mathbf r _{1})=\left[ \begin{array}{lll} 0&-q_{3}&q_{2} \\ q_{3}&0&-q_{1} \\ -q_{2}&q_{1}&0 \\ \end{array} \right]\mathbf r _{1} \end{aligned}$$

while in the last summand we perform a dot product:

$$\begin{aligned} \mathbf q \mathbf q ^\mathrm{T}=\left[ \begin{array}{c} q_{1} \\ q_{2} \\ q_{3} \\ \end{array} \right]\left[ \begin{array}{ccc} q_{1}&q_{2}&q_{3} \\ \end{array} \right]\end{aligned}$$

Equation (2.38) can be rewritten into the following form:

$$\begin{aligned} \mathbf r _{2}&= \left\{ (q^{2}_{0}-q^{2}_{1}-q^{2}_{2}-q^{2}_{3})\mathbf I + 2q_{0}\left[ \begin{array}{lll} 0&-q_{3}&q_{2} \\ q_{3}&0&-q_{1} \\ -q_{2}&q_{1}&0 \\ \end{array} \right]\right. \\&\quad \qquad \; \left.+ 2 \left[ \begin{array}{ccc} q^{2}_{1}&q_{1}q_{2}&q_{1}q_{3} \\ q_{1}q_{2}&q^{2}_{2}&q_{2}q_{3} \\ q_{1}q_{3}&q_{2}q_{3}&q^{2}_{3} \\ \end{array} \right] \right\} \mathbf r _{1} \end{aligned}$$

The rotation matrixR expressed with the four elements of quaternion has the following form:

$$\begin{aligned} \left[\begin{array}{ccc} q_{0}^{2} +q_{1}^{2} -q_{2}^{2} -q_{3}^{2}&2\left(q_{1} q_{2} -q_{0} q_{3} \right)&2\left(q_{1} q_{3} +q_{0} q_{2} \right) \\ 2\left(q_{1} q_{2} +q_{0} q_{3} \right)&q_{0}^{2} -q_{1}^{2} +q_{2}^{2} -q_{3}^{2}&2\left(q_{2} q_{3} -q_{0} q_{1} \right)\\ 2\left(q_{1} q_{3} -q_{0} q_{2} \right)&2\left(q_{2} q_{3} +q_{0} q_{1} \right)&q_{0}^{2} -q_{1}^{2} -q_{2}^{2} +q_{3}^{2} \end{array}\right] \end{aligned}$$

(2.39)

The following expression of a quaternion is specially appropriate to describe the rotation or orientation in the space:

$$\begin{aligned} q=\cos \frac{\vartheta }{2}+\sin \frac{\vartheta }{2}\mathbf s \end{aligned}$$

(2.40)

In the above equation s is a unit vector aligned with the rotation axis, while $\vartheta$ is the angle of rotation. Also the quaternion, which is describing rotation, is a unit quaternion:

$$\begin{aligned} q^{2}_{0}+q^{2}_{1}+q^{2}_{2}+q^{2}_{3} = 1 \end{aligned}$$

(2.41)

We will insert the quaternion $q$, written in the form (2.40), into Eq. (2.38). With respect to Eq. (2.34), the quaternion will be split into scalar and vector part:

$$\begin{aligned} q_{0}&=\cos \frac{\vartheta }{2}\\ \mathbf q&=\sin \frac{\vartheta }{2}\mathbf s \end{aligned}$$

The following equation is obtained:

$$\begin{aligned} \mathbf r _{2}=\cos ^{2}\frac{\vartheta }{2}\mathbf r _{1}-\sin ^{2}\frac{\vartheta }{2}(\mathbf s \cdot \mathbf s )\mathbf r _{1}+2\cos \frac{\vartheta }{2}\sin \frac{\vartheta }{2}(\mathbf s \times \mathbf r _{1})+2\sin ^{2}\frac{\vartheta }{2}\mathbf s (\mathbf s \cdot \mathbf r _{1}) \end{aligned}$$

(2.42)

When considering the following trigonometric formulas:

$$\begin{aligned}&2\cos \frac{\vartheta }{2}\sin \frac{\vartheta }{2}=\sin \vartheta \\&\cos ^{2}\frac{\vartheta }{2}-\sin ^{2}\frac{\vartheta }{2}=\cos \vartheta \\&\cos ^{2}\frac{\vartheta }{2}+\sin ^{2}\frac{\vartheta }{2}=1 \end{aligned}$$

and while taking into account the commutative property of the dot product, we can demonstrate that Eq. (2.42) represents the Rodrigues’s formula (2.6).

The rotation about the $z$ axis can be written by the use of the following quaternion:

$$\begin{aligned} q=\cos \frac{\vartheta }{2} +\sin \frac{\vartheta }{2} \left[\begin{array}{l} {0} \\ {0} \\ {1} \end{array}\right] \end{aligned}$$

Individual elements of the quaternion are therefore:

$$\begin{aligned} q_{0}&=\cos \frac{\vartheta }{2} \\ q_{1}&=0 \\ q_{2}&=0 \\ q_{3}&=\sin \frac{\vartheta }{2} \end{aligned}$$

By inserting the above elements into the rotation matrix (2.39), we have:

$$\begin{aligned} \left[ \begin{array}{ccc} {\cos }^{2}\frac{\vartheta }{2}-{\sin }^{2} \frac{\vartheta }{2}&-2\cos \frac{\vartheta }{2}\sin \frac{\vartheta }{2}&0 \\ 2\cos \frac{\vartheta }{2}\sin \frac{\vartheta }{2}&{\cos }^{2}\frac{\vartheta }{2}-{\sin }^{2} \frac{\vartheta }{2}&0 \\ 0&0&{\cos }^{2}\frac{\vartheta }{2}+{\sin }^{2} \frac{\vartheta }{2} \\ \end{array} \right] \end{aligned}$$

The above matrix is the well known matrix describing the rotation about the $z$ axis:

$$\begin{aligned} \left[\begin{array}{clc} \cos \vartheta&-\sin \vartheta&0\\ \sin \vartheta&\quad \cos \vartheta&0\\ 0&\quad 0&1 \end{array}\right] \end{aligned}$$

Let us consider also the inverse problem, where we will determine the equivalent unit quaternion from the elements of the rotation matrix. With different combinations of the diagonal elements of the rotation matrix $r_{11}, r_{22}, r_{33}$ we obtain:

$$\begin{aligned}&q_{0}^{2} =\frac{1}{4} \left(1+r_{11} +r_{22} +r_{33} \right)\\&q_{1}^{2} =\frac{1}{4} \left(1+r_{11} -r_{22} -r_{33} \right)\\&q_{2}^{2} =\frac{1}{4} \left(1-r_{11} +r_{22} -r_{33} \right)\\&q_{3}^{2} =\frac{1}{4} \left(1-r_{11} -r_{22} +r_{33} \right) \end{aligned}$$

When developing the above expressions, one must have in mind that we are dealing with the unit quaternions (2.41). When calculating these quaternions we use the signs, which we have encountered while determining the equivalent axis of rotation (2.13):

$$\begin{aligned} \begin{array}{l} q_{0} = \dfrac{1}{2} \sqrt{1+r_{11} +r_{22} +r_{33} } \\ q_{1} = \dfrac{1}{2}sgn(r_{32} -r_{23} )\sqrt{1+r_{11} -r_{22} -r_{33} } \\ q_{2} = \dfrac{1}{2}sgn(r_{13} -r_{31} )\sqrt{1-r_{11} +r_{22} -r_{33} } \\ q_{3} = \dfrac{1}{2}sgn(r_{21} -r_{12} )\sqrt{1-r_{11} -r_{22} +r_{33} } \end{array} \end{aligned}$$

(2.43)

Let us first consider a simple example, where two consecutive rotations were performed in the same coordinate frame: first rotation for $90^{\circ }$ about the $z$ axis and afterwards the rotation for $90^{\circ }$ about the $y$ axis. This can be written by the use of rotation matrices as follows:

$$\begin{aligned} \mathbf R =\mathbf R _{y,90} \mathbf R _{z,90} = \left[ \begin{array}{lcc} \quad 0&0&1 \\ \quad 0&1&0 \\ -1&0&0 \\ \end{array} \right]\left[ \begin{array}{clc} 0&-1&0 \\ 1&\quad 0&0 \\ 0&\quad 0&10 \\ \end{array} \right]=\left[ \begin{array}{ccc} 0&0&1 \\ 1&0&0 \\ 0&1&0 \\ \end{array}\right] \end{aligned}$$

We will now use the quaternions instead of rotation matrices. Rotation for $90^{\circ }$ about the $y$ axis is according to Eq. (2.40) written as follows:

$$\begin{aligned} p = \cos 45 + \sin 45^{\circ } \left[ \begin{array}{c} 0 \\ 1 \\ 0 \\ \end{array} \right] \end{aligned}$$

or

$$\begin{aligned} p_{0}=\frac{\sqrt{2}}{2} \; \text{ and}\; \mathbf p =\frac{1}{2}\left[ \begin{array}{c} 0 \\ \sqrt{2} \\ 0 \\ \end{array} \right] \end{aligned}$$

In a similar way we describe also the rotation for $90^{\circ }$ about the $z$ axis:

$$\begin{aligned} q_{0}=\frac{\sqrt{2}}{2} \; \text{ and} \; \mathbf q =\frac{1}{2}\left[ \begin{array}{c} 0 \\ 0 \\ \sqrt{2} \\ \end{array} \right] \end{aligned}$$

The product of two quaternions is calculated by the help of (2.35):

$$\begin{aligned} pq&=\frac{1}{2}-\frac{1}{4}\left[ \begin{array}{ccc} 0&\sqrt{2}&0 \\ \end{array} \right]\left[ \begin{array}{c} 0 \\ 0 \\ \sqrt{2} \\ \end{array} \right]+\frac{\sqrt{2}}{4}\left[ \begin{array}{c} 0 \\ 0 \\ \sqrt{2} \\ \end{array} \right]+\frac{\sqrt{2}}{4}\left[ \begin{array}{c} 0 \\ \sqrt{2} \\ 0 \\ \end{array} \right] \\&\quad \; +\frac{1}{4}\left[ \begin{array}{ccc} \mathbf i&\mathbf j&\mathbf k \\ 0&\sqrt{2}&0 \\ 0&0&\sqrt{2} \\ \end{array} \right]= \frac{1}{2}+\frac{1}{2}\mathbf i +\frac{1}{2}\mathbf j +\frac{1}{2}\mathbf k \end{aligned}$$

When inserting the calculated quaternion into the matrix (2.39), the rotation matrix R from the beginning of this example is obtained.

Let us look at another example, which was by the use of Rodrigues’s formula solved already in Sect. 2.1. The unit vector i was rotated for the angle 2$\pi$/3 about the axis running through the origin of the coordinate frame and the point $(1,1,1)^\mathrm{T}$. The axis of rotation is described, as in previous example, by the unit vector s $= 1/\sqrt{3} [1,1,1]^\mathrm{T}$, which we will insert together with $\cos (\pi /3)= 1/2$ and sin($\pi$/3)= $\sqrt{3}$/2 into Eq. (2.40). The following quaternion is obtained:

$$\begin{aligned} q=\frac{1}{2}+\frac{1}{2}\mathbf i +\frac{1}{2}\mathbf j +\frac{1}{2}\mathbf k \end{aligned}$$

We will insert:

$$\begin{aligned} \mathrm r {}_{1} = 0 + \mathbf i \end{aligned}$$

into Eq. (2.37) describing the rotation. The following multiplication must be performed:

$$\begin{aligned} \mathbf r _{2}&= \frac{1}{2}(1+\mathbf i +\mathbf j +\mathbf k )(\mathbf i )\frac{1}{2}(1-\mathbf i -\mathbf j -\mathbf k )\\&= \frac{1}{4}(\mathbf i -1-\mathbf k +\mathbf j )(1-\mathbf i -\mathbf j -\mathbf k )\\&= \frac{1}{4}(\mathbf i -1-\mathbf k +\mathbf j + 1+\mathbf i +\mathbf j +\mathbf k \\&\quad - \mathbf k +\mathbf j -\mathbf i +1+\mathbf j +\mathbf k -1-\mathbf i )\\&= \mathbf j \end{aligned}$$

We obtained the same result as when using the Rodrigues’s formula.

Let us finally study, how to describe by the use of quaternions the orientation of the gripper shown in Fig. 2.8 from the Sect. 2.2. The orientation of the gripper is obtained as result of the geometric model of the robot in the form of rotation matrix (2.27). We calculate the corresponding quaternion by the use of Eq. (2.43):

$$\begin{aligned}&q_{0}=0.866\\&q_{1}=-0.5\\&\quad \; q_{2}=0\\&\quad \; q_{3}=0 \end{aligned}$$

In previous chapter we have found out that the rotation matrix (2.27) belongs to the following RPY angles: $\varphi$ = 0, $\vartheta$ = 0, and $\psi = - 60^{\circ }$. The orientation quaternion can be obtained also from the RPY angles. Rotation R is described by the quaternion:

$$\begin{aligned} q_{z\varphi }=\cos \frac{\varphi }{2}+\sin \frac{\varphi }{2}\mathbf k \end{aligned}$$

(2.44)

The following quaternion belongs to the rotation P:

$$\begin{aligned} q_{y\vartheta }=\cos \frac{\vartheta }{2}+\sin \frac{\vartheta }{2}\mathbf j \end{aligned}$$

(2.45)

while rotation Y can be written as follows:

$$\begin{aligned} q_{x\psi }=\cos \frac{\psi }{2}+\sin \frac{\psi }{2}\mathbf i \end{aligned}$$

(2.46)

After multiplying the above three quaternions:

$$\begin{aligned} \mathrm{{RPY}}(\varphi ,\vartheta ,\psi )=q _{\mathrm{z}\varphi } q _{\mathrm{y}\vartheta } q _{\mathrm{x}\psi } \end{aligned}$$

(2.47)

the resulting quaternion is obtained:

$$\begin{aligned} q_{0}&= c\frac{\varphi }{2}c\frac{\vartheta }{2}c\frac{\psi }{2}+s\frac{\varphi }{2}s\frac{\vartheta }{2}s\frac{\psi }{2} \end{aligned}$$

(2.48)

$$\begin{aligned} q_{1}&= c\frac{\varphi }{2}c\frac{\vartheta }{2}s\frac{\psi }{2}-s\frac{\varphi }{2}s\frac{\vartheta }{2}c\frac{\psi }{2} \end{aligned}$$

(2.49)

$$\begin{aligned} q_{2}&= c\frac{\varphi }{2}s\frac{\vartheta }{2}s\frac{\psi }{2}+s\frac{\varphi }{2}c\frac{\vartheta }{2}s\frac{\psi }{2} \end{aligned}$$

(2.50)

$$\begin{aligned} q_{3}&= s\frac{\varphi }{2}c\frac{\vartheta }{2}s\frac{\psi }{2}-c\frac{\varphi }{2}s\frac{\vartheta }{2}s\frac{\psi }{2} \end{aligned}$$

(2.51)

For the selected RPY angle we have:

$$\begin{aligned} q_{0}&= \frac{\sqrt{3}}{2}\\ q_{1}&= -\frac{1}{2}\\ q_{2}&= 0\\ q_{3}&= 0\nonumber \end{aligned}$$

which is the expected result.