Kinematics

Abstract

This chapter presents the formulation of robot manipulator kinematics within the geometric algebra framework. In this algebraic system, the 3D Euclidean motion of points, lines, and planes can be advantageously represented using the algebra of motors. The computational complexity of direct and indirect kinematics and other problems concerning robot manipulators are dependent on the robot’s degrees of freedom as well as its geometric characteristics. Our approach makes possible a direct algebraic formulation of the concrete problem in such a way that it reflects the underlying geometric structure. This is achieved by describing parts of the problem based on motor representations of points, lines, planes, circles, and spheres where necessary. The chapter presents the formulation and computation of closed-form solutions of direct and indirect kinematics for standard robot manipulators and a simple example of a grasping task. The flexible method presented here widens the current standard point or line representation–based approaches for the treatment of problems related to robot manipulators. We present the computation of the inverse kinematics in motor algebra using points, lines, and planes, and as an extension we solve the same problem by using conformal geometric algebra that also allows us to represent the use of circles and spheres.

1 Introduction

This chapter presents the formulation of robot manipulator kinematics within the geometric algebra framework. In this algebraic system, the 3D Euclidean motion of points, lines, and planes can be advantageously represented using the algebra of motors. The computational complexity of direct and indirect kinematics and other problems concerning robot manipulators are dependent on the robot’s degrees of freedom as well as its geometric characteristics. Our approach makes possible a direct algebraic formulation of the concrete problem in such a way that it reflects the underlying geometric structure. This is achieved by describing parts of the problem based on motor representations of points, lines, planes, circles, and spheres where necessary. The chapter presents the formulation and computation of closed-form solutions of direct and indirect kinematics for standard robot manipulators and a simple example of a grasping task. The flexible method presented here widens the current standard point or line representation–based approaches for the treatment of problems related to robot manipulators. We present the computation of the inverse kinematics in motor algebra using points, lines, and planes, and as an extension we solve the same problem by using conformal geometric algebra that also allows us to represent the use of circles and spheres.

2 Elementary Transformations of Robot Manipulators

The study of the rigid motion of objects in 3D space plays an important role in robotics. In order to linearize the rigid motion of the Euclidean space, homogeneous coordinates are normally utilized. That is why in the geometric algebra framework we choose the special or degenerated geometric algebra to extend the algebraic system from 3D Euclidean space to 4D space. In this system, we can nicely model the motion of points, lines, and planes with computational advantages and geometric insight. Let us start with a description of the basic elements of robot manipulators in terms of the special or degenerated geometric algebra ${\mathcal{G}}_{3,0,1}^{+}$ or motor algebra. The most basic parts of a robot manipulator are revolute joints, prismatic joints, connecting links, and the end-effectors. In the following sections, we will treat the kinematics of the prismatic and revolute manipulator parts using the 4D geometric algebra ${\mathcal{G}}_{3,0,1}^{+}$ and will illustrate an end-effector grasping task.

2.1 The Denavit–Hartenberg Parameterization

The computation of direct or inverse kinematics requires both an exact description of the robot manipulator’s structure and its configuration. The descriptive approach used most often is known as the Denavit–Hartenberg (DH) procedure [48]. It is based on a uniform description of the position of the reference coordinate system of a particular joint relative to the next joint in consideration. Figure 11.3a shows how coordinate frames are attached to a joint of a robot manipulator. Table 11.1 presents the specifications of two robot manipulators, the SCARA and the Stanford manipulators,as shown in Figs. 11.1 and 11.2, respectively.

Fig. 11.1

SCARA-type manipulator according to DH parameters given in Table 11.1 (variable parameters are circled)

Fig. 11.2

Stanford-type manipulator according to DH parameters in Table 11.1 (variable parameters are encircled)

Fig. 11.3

Motor description of a manipulator joint: (a) ith joint of a robot manipulator and attached coordinate frames according to Denavit–Hartenberg; the encircled θ _i is the variable parameter; (b) transformation from frame ${\mathcal{F}}_{i}$ to ${\mathcal{F}}_{i-1}$ (motor ^{i − 1}​M _i consists of two screw transformations, ${\textit{\textbf{M}}}_{\hat{{\alpha }}_{i}}^{x}$ and ${\textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}$)

Table 11.1 Kinematic configuration of two robotic manipulators

This tells us whether the joint is for rotation (revolute) or for translation (prismatic). The transformation of the reference coordinate system between two joints will be called a jointtransition. Figure 11.3b shows the involved screws in a jointtransition according to the Denavit–Hartenberg parameters. The frame, or reference coordinate system, related to the ith joint is attached at the end of this link and it is notated ${\mathcal{F}}_{i}$.

In Table 11.1, parameter “v” indicates that the involved variable of the joint is variable, and parameter “c” indicates that it is constant.

The position and orientation of the end-effector in relation to the reference coordinate system of the robot basis can be computed by linking all jointtransitions. In this way, we are able to get straightforwardly the direct kinematics.

Conversely, in the case of the inverse kinematics, given the position and orientation of the end-effector, we must find values of the variable parameters of the jointtransitions that satisfy this requirement. In the following sections, we will provide more details for the computation of the direct and inverse kinematics of robot manipulators.

2.2 Representations of Prismatic and Revolute Transformations

The transformation of any point, line, or plane between coordinate systems ${\mathcal{F}}_{i-1}$ and ${\mathcal{F}}_{i}$ is revolute when the involved degree of freedom is only a variable angle θ _i , and it is prismatic when the degree of freedom is only a variable length d _i . The transformation motor ^{i − 1}​M _i between ${\mathcal{F}}_{i}$ and ${\mathcal{F}}_{i-1}$ consists of a sequence of two screw transformations, one fixed (i.e., ${\textit{\textbf{M}}}_{\hat{{\alpha }}_{i}}^{x}$) and another variable (i.e., ${\textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}$) (see Fig. 11.3b). Note that we use dual angles (see [11]):

$$\hat{{\theta }}_{i} = {\theta }_{i} + I{d}_{i},$$

(11.1)

$$\hat{{\alpha }}_{i} = {\alpha }_{i} + I{l}_{i}.$$

(11.2)

In the revolute case, the latter equation uses as a variable parameter, the angle θ _i , and in the prismatic case, the displacement d _i is the variable parameter. The transformation reads

$$ \begin{array}{rcl} {}^{i-1}\!{\mbox{ $M$}}_{ i}& =&{ \textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{i}}^{x} ={ \textit{\textbf{T}}}_{{ d}_{i}}^{z}{\textit{\textbf{T}}}_{{ \theta }_{i}}^{z}{\textit{\textbf{T}}}_{{ l}_{i}}^{x}{\textit{\textbf{T}}}_{{ \alpha }_{i}}^{x} \\ & =& \left (1 + \frac{I} {2}\left (\begin{array}{c} 0\\ 0 \\ {d}_{i}\\ \end{array} \right )\right ){\textit{\textbf{T}}}_{{\theta }_{i}}^{z}\left (1 + \frac{I} {2}\left (\begin{array}{c} {l}_{i} \\ 0\\ 0\\ \end{array} \right )\right ){\textit{\textbf{T}}}_{{\alpha }_{i}}^{x}.\end{array}$$

(11.3)

For the sake of clarity, the dual bivectors of the translators are given as a column vector, which makes the variable parameters explicit.

Since ${}^{i-1}\!{\textit{\textbf{M}}{}_{i}\,\,}^{i-1}\!\widetilde{{\textit{\textbf{M}}}}_{i} = 1$, we obtain

$$ \begin{array}{rcl} {}^{i}\!{\textit{\textbf{M}}}_{ i-1}& =& \widetilde{{\textit{\textbf{M}}}}_{\hat{{\alpha }}_{i}}^{x}\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z} =\widetilde{{ \textit{\textbf{T}}}}_{{ l}_{i}}^{x}\widetilde{{\textit{\textbf{T}}}}_{{ \alpha }_{i}}^{x}\widetilde{{\textit{\textbf{T}}}}_{{ d}_{i}}^{z}\widetilde{{\textit{\textbf{T}}}}_{{ \theta }_{i}}^{z}\;.\end{array}$$

(11.4)

Be aware that for the rest of the chapter ^j​M _i will denote a motor transformation from ${\mathcal{F}}_{i}$ to ${\mathcal{F}}_{j}$.

We will now give general expressions for the transformation of points, lines, and planes using a single parameter, θ _i or d _i , as the variable and two fixed parameters, α _i and l _i . For the joint depicted in Fig. 11.3b, a revolute transformation will take place only when θ _i varies and a prismatic transformation only when d _i varies. Now, taking a point X represented in the frame ${\mathcal{F}}_{i-1}$, we can describe its transformation from ${\mathcal{F}}_{i-1}$ to ${\mathcal{F}}_{i}$ using the motor algebra [11], with either θ _i or d _i as the variable parameter. We will call this transformation a forward transformation.

The multivector representation of point X related to the frame ${\mathcal{F}}_{i}$ will be expressed as ⁱ​X, with

$$ \begin{array}{rcl} {}^{i}\!\textit{\textbf{X}}& =& {}^{i}\!{\textit{\textbf{M}}{}_{ i-1}\,\,}^{i-1}\!{\textit{\textbf{X}}\,\,}^{i}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ i-1} =\widetilde{{ \textit{\textbf{M}}}}_{\hat{{\alpha }}_{i}}^{x}\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z}\,\,{}^{i-1}\!\textit{\textbf{X}}\,\,{\overline{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z}{\overline{\textit{\textbf{M}}}}_{\hat{{ \alpha }}_{i}}^{x} \\ & =& \widetilde{{\textit{\textbf{T}}}}_{{l}_{i}}^{x}\widetilde{{\textit{\textbf{T}}}}_{{ \alpha }_{i}}^{x}\widetilde{{\textit{\textbf{T}}}}_{{ d}_{i}}^{z}\widetilde{{\textit{\textbf{T}}}}_{{ \theta }_{i}}^{z}\,\,{}^{i-1}\!\textit{\textbf{X}}\,\,{\textit{\textbf{T}}}_{{ \theta }_{i}}^{z}\widetilde{{\textit{\textbf{T}}}}_{{ d}_{i}}^{z}{\textit{\textbf{T}}}_{{ \alpha }_{i}}^{x}\widetilde{{\textit{\textbf{T}}}}_{{ l}_{i}}^{x} \\ & =& 1 + {I\,}^{i}\!\textit{\textbf{x}}\, \end{array}$$

(11.5)

where ⁱ​x is a bivector representing the 3D position of X referred to ${\mathcal{F}}_{i}$. A transformation in the reverse sense will be called a backward transformation. In this case, a point X referenced in the frame ${\mathcal{F}}_{i}$ is transformed to the frame ${\mathcal{F}}_{i-1}$ as follows:

$$ \begin{array}{rcl} {}^{i-1}\!\textit{\textbf{X}}& =& {}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{i}\!{\textit{\textbf{X}}\,\,}^{i-1}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ i} ={ \textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{i}}^{x}\,\,{}^{i}\!\textit{\textbf{X}}\,\,\widetilde{{\overline{\textit{\textbf{M}}}}}_{\hat{{ \alpha }}_{i}}^{x}\,\widetilde{{\overline{\textit{\textbf{M}}}}}_{\hat{{ \theta }}_{i}}^{z} \\ & =& 1 + {I\,}^{i-1}\!\textit{\textbf{x}}\;. \end{array}$$

(11.6)

Note that the motor applied from the right side is not purely conjugated as is the case with the line. The same will also be true for a plane [11].

Consider a line L represented in the frame ${\mathcal{F}}_{i-1}$ by ${}^{i-1}\!\textit{\textbf{L}} {= }^{i-1}\!\textit{\textbf{n}} + {I}^{i-1}\!\textit{\textbf{m}}$, where n and m are bivectors indicating the orientation and moment of the line, respectively. We can write the forward transformation of this line related to the frame ${\mathcal{F}}_{i}$ according to [11]:

$$ \begin{array}{rcl} {}^{i}\!\textit{\textbf{L}}& =& {}^{i}\!{\textit{\textbf{M}}{}_{ i-1}\,\,}^{i-1}\!{\textit{\textbf{L}}\,\,}^{i}\widetilde{{\textit{\textbf{M}}}}_{ i-1} =\widetilde{{ \textit{\textbf{M}}}}_{\hat{{\alpha }}_{i}}^{x}\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z}\,\,{}^{i-1}\!\textit{\textbf{L}}\,\,{\textit{\textbf{M}}}_{\hat{{ \theta }}_{i}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{i}}^{x} \\ & =& {}^{i}\!\textit{\textbf{n}} + {I\,}^{i}\!\textit{\textbf{m}}. \end{array}$$

(11.7)

Its backward transformation reads

$$ \begin{array}{rcl} {}^{i-1}\!\textit{\textbf{L}}& =& {}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{i}\!{\textit{\textbf{L}}\,\,}^{i-1}\widetilde{{\textit{\textbf{M}}}}_{ i} ={ \textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{i}}^{x}\,\,{}^{i}\!\textit{\textbf{L}}\,\,\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \alpha }}_{i}}^{x}\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z} \\ & =& {}^{i-1}\!\textit{\textbf{n}} + {I\,}^{i-1}\!\textit{\textbf{m}}. \end{array}$$

(11.8)

Finally, the forward transformation of a plane H represented in ${\mathcal{F}}_{i-1}$ reads

$$ \begin{array}{rcl} {}^{i}\!\textit{\textbf{H}}& =& {}^{i}\!{\textit{\textbf{M}}{}_{ i-1}\,\,}^{i-1}\!{\textit{\textbf{H}}\,\,}^{i}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ i-1} =\widetilde{{ \textit{\textbf{M}}}}_{\hat{{\alpha }}_{i}}^{x}\widetilde{{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z}\,\,{}^{i-1}\!\textit{\textbf{H}}\,\,{\overline{\textit{\textbf{M}}}}_{\hat{{ \theta }}_{i}}^{z}{\overline{\textit{\textbf{M}}}}_{\hat{{ \alpha }}_{i}}^{x} \\ & =& {}^{i}\!\textit{\textbf{n}} + {I\,}^{i}{d}_{ H}, \end{array}$$

(11.9)

and, as above, its backward transformation equation is

$$ \begin{array}{rcl} {}^{i-1}\!\textit{\textbf{H}}& =& {}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{i}\!{\textit{\textbf{H}}\,\,}^{i-1}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ i} ={ \textit{\textbf{M}}}_{\hat{{\theta }}_{i}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{i}}^{x}\,\,{}^{i}\!\textit{\textbf{H}}\,\,\widetilde{{\overline{\textit{\textbf{M}}}}}_{\hat{{ \alpha }}_{i}}^{x}\,\widetilde{{\overline{\textit{\textbf{M}}}}}_{\hat{{ \theta }}_{i}}^{z} \\ & =& {}^{i-1}\!\textit{\textbf{n}} + {I\,}^{i-1}{d}_{ H}. \end{array}$$

(11.10)

2.3 Grasping by Using Constraint Equations

In this section we illustrate grasping as a manipulation-related task, grasping operation, that involves the positioning of a two-finger grasper in front of a static object. Figure 11.4 shows the grasper and the considered object O. The manipulator must move the grasper near the object, and together they must fulfill some conditions in order to grasp the object firmly. To determine the overall transformation, ⁰​M _n , which moves the grasper into an appropriate grasping position, we claim that ⁰​M _n must fulfill the three constraints below. In order to formulate these constraints, we can take advantages of the point, line, and plane representations of the motor algebra. In the following explanation, we assume that the representations of geometric entities attached to the object O in frame ${\mathcal{F}}_{0}$ are known. The grasping movement of the two fingers should be in the reference plane H _O of O, that is, the yz-plane of the end-effector frame ${\mathcal{F}}_{n}$ should be equal to the reference plane H _O . The attitude condition can be simply formulated in terms of a plane equation, as follows:

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{M}}{}_{ n}\,}^{n}\!{\textit{\textbf{H}}}_{ n}^{yz}\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ n} {-}^{0}\!{\textit{\textbf{H}}}_{ O}& \approx & 0\, \\ \end{array} $$

(11.11)

where ${}^{n}\!{\textit{\textbf{H}}}_{n}^{yz} = {(1,0,0)}^{T} + I\;0 = {(1,0,0)}^{T}$ (see Fig. 11.4).

Fig. 11.4

Two-finger grasper approaching an object

After the application of the motor ⁰​M _n , the grasper and the object should be in a parallel alignment, that is, the direction of the y-axis and the line L _O should be the same. The alignment condition can be simply expressed in terms of a line equation,

$$ \begin{array}{rcl} \langle {}^{0}\!{\textit{\textbf{M}}{}_{ n}\,}^{n}\!{\textit{\textbf{L}}}_{ n}^{y}\,{}^{0}\!\widetilde{{\textit{\textbf{M}}}{}_{ n}\rangle }_{d} {-\langle }^{0}\!{\textit{\textbf{L}}{}_{ O}\rangle }_{d} \approx 0\, & &\end{array}$$

(11.12)

where ${}^{n}\!{\textit{\textbf{L}}}_{n}^{y} = {(0,1,0)}^{T} + I{(0,0,0)}^{T} = {(0,1,0)}^{T}$ and ⟨L⟩ _d denotes the direction components of line L.

The motion ⁰​M _n should also guarantee that the grasper is in the correct grasping position, that is, the origin, ${\textit{\textbf{P}}}_{n}^{o}$, of the end-effector frame, ${\mathcal{F}}_{n}$, should touch the reference point X _O of O. The formulation of this constraint in our framework is

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{M}}{}_{ n}\,\,}^{n}\!{\textit{\textbf{P}}}_{ n}^{o}\,\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ n} {-}^{0}\!{\textit{\textbf{X}}}_{ O} \approx 0.& &\end{array}$$

(11.13)

With these three conditions, we can obtain constraints for the components of ⁰​M _n , and we can determine ⁰​M _n numerically. The next step is to determine the variable joint parameters of the robot manipulator that lead to the position and orientation of the end-effector frame ${\mathcal{F}}_{n}$ described by ⁰​M _n . This problem is called the inverse kinematics problem of robot manipulators and is treated in Sect. 11.4.

3 Direct Kinematics of Robot Manipulators

The determination of direct kinematics involves the computation of the position and orientation of the end-effector or frame ${\mathcal{F}}_{n}$ given the parameters of the jointtransitions (see Fig. 11.5). In this section, we show how the direct kinematics can be computed when we use as geometric objects points, lines, or planes. The notation we use for points, lines, and planes is shown in Fig. 11.6. The direct kinematics for the general case of a manipulator with n joints can be written as

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{M}}}_{ n} {= }^{0}\!{\textit{\textbf{M}}{}_{ 1}}^{1}\!{\textit{\textbf{M}}{}_{ 2}}^{2}\!{\textit{\textbf{M}}}_{ 3}{\cdots }^{n-1}\!{\textit{\textbf{M}}}_{ n} ={ \prod \nolimits }_{i=1}^{n}{}^{i-1}\!{\textit{\textbf{M}}}_{ i}. \\ \end{array} $$

(11.14)

We can formulate straightforwardly the direct kinematics in terms of point, line, or plane representations as follows:

$$ \begin{array}{rcl}{ }^{0}\!\textit{\textbf{X}}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ n}\,\,}^{n}\!{\textit{\textbf{X}}\,\,}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ n} ={ \prod \nolimits }_{i=1}^{n}{}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{n}\!\textit{\textbf{X}}\,\,{\prod \nolimits }_{i=1}^{n}{}^{n-i}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ n+1-i}\;, \\ {}^{0}\!\textit{\textbf{L}}& =& {\prod \nolimits }_{i=1}^{n}{}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{n}\!\textit{\textbf{L}}\,\,{\prod \nolimits }_{i=1}^{n}{}^{n-i}\widetilde{{\textit{\textbf{M}}}}_{ n+1-i}\;, \\ {}^{0}\!\textit{\textbf{H}}& =& {\prod \nolimits }_{i=1}^{n}{}^{i-1}\!{\textit{\textbf{M}}{}_{ i}\,\,}^{n}\!\textit{\textbf{H}}\,\,{\prod \nolimits }_{i=1}^{n}{}^{n-i}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ n+1-i}. \\ \end{array} $$

(11.15)

Now, let us write the motor ⁰​M ₄ for the direct kinematics for points, lines, and planes in the form of equation (11.15) for the SCARA manipulator specified by the Denavit–Hartenberg parameters of Table 11.1. First, using equation (11.14) with n = 4, we can easily express the required motor ⁰​M ₄, as follows:

$$\begin{array}{rcl} {}^{0}\!{\textit{\textbf{M}}}_{ 4}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 1}}^{1}\!{\textit{\textbf{M}}{}_{ 2}}^{2}\!{\textit{\textbf{M}}{}_{ 3}}^{3}\!{\textit{\textbf{M}}}_{ 4} = ({\textit{\textbf{M}}}_{\hat{{\theta }}_{1}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{1}}^{x})\cdots ({\textit{\textbf{M}}}_{\hat{{ \theta }}_{4}}^{z}{\textit{\textbf{M}}}_{\hat{{ \alpha }}_{4}}^{x}) \\ & =& ({\textit{\textbf{T}}}_{{d}_{1}}^{z}{\textit{\textbf{T}}}_{{ \theta }_{1}}^{z}{\textit{\textbf{T}}}_{{ l}_{1}}^{x}{\textit{\textbf{T}}}_{{ \alpha }_{1}}^{x})\cdots ({\textit{\textbf{T}}}_{{ d}_{4}}^{z}{\textit{\textbf{T}}}_{{ \theta }_{4}}^{z}{\textit{\textbf{T}}}_{{ l}_{4}}^{x}{\textit{\textbf{T}}}_{{ \alpha }_{4}}^{x}) \\ & =& \left (1 + \frac{I} {2}\left (\begin{array}{c} 0\\ 0 \\ {d}_{1}\\ \end{array} \right )\right ){\textit{\textbf{T}}}_{{\theta }_{1}}^{z}\left (1 + \frac{I} {2}\left (\begin{array}{c} {l}_{1} \\ 0\\ 0\\ \end{array} \right )\right )\left (1 + \frac{I} {2}\left (\begin{array}{c} 0\\ 0 \\ {d}_{2}\\ \end{array} \right )\right) \\ & &{ \textit{\textbf{T}}}_{{\theta }_{2}}^{z}\left (1 + \frac{I} {2}\left (\begin{array}{c} {l}_{2} \\ 0\\ 0\\ \end{array} \right )\right ){\textit{\textbf{T}}}_{{\theta }_{3}}^{z}\left (1 + \frac{I} {2}\left (\begin{array}{c} 0\\ 0 \\ {d}_{4}\\ \end{array} \right )\right ). \\ \end{array} $$

(11.16)

Note that translators with zero translation and rotors with zero angle become 1. Applying the motor ⁰​M ₄ from the left and ${}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{4}$ from the right for point and plane equations, and the motor ⁰​M ₄ from the left and ${}^{0}\widetilde{{\textit{\textbf{M}}}}_{4}$ from the right for line equations, as indicated by Eq. 11.15, we derive the direct kinematics equations of points, lines, and planes for the SCARA robot manipulator.

Fig. 11.5

Direct and inverse kinematics

Fig. 11.6

Notations for origin, coordinate axis, and coordinate planes for frame-specific entities

3.1 MAPLE Program for Motor Algebra Computations

Since the nature of our approach requires symbolic computation, we chose the MAPLE computer package to develop a comfortable program for computations in the frame of different geometric algebras. To employ the program, we simply have to specify the vector basis of the selected geometric algebra. The program also employs a variety of useful algebraic operators to carry out computations involving reversion, Clifford conjugations, inner and wedge operations, rotations, translations, motors, extraction of the i-blade of a multivector, etc. Here, as a first illustration, we use the program for computations in the motor algebra framework ${\mathcal{G}}_{3,0,1}^{+}$ to compute the direct kinematic equation of the origin ${\textit{\textbf{P}}}_{4}^{o}$ of ${\mathcal{F}}_{4}$ for the SCARA manipulator specified by the Denavit–Hartenberg parameters of Table 11.1. Figure 11.7 shows the frames and the point ${\textit{\textbf{P}}}_{4}^{o}$ referred to ${\mathcal{F}}_{0}$. The final result of the computer calculation is

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{P}}}_{ 4}^{o}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 4}\,\,}^{4}\!{\textit{\textbf{P}}}_{ 4}^{o}\,\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 4} {= }^{0}\!{\textit{\textbf{M}}}_{ 4}\,\,{\left (1 + I\left (\begin{array}{c} 0\\ 0 \\ 0\\ \end{array} \right )\right )\,\,}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 4} \\ & =& 1 + I\left (\begin{array}{c} {l}_{2}\cos ({\theta }_{1} + {\theta }_{2}) + {l}_{1}\cos ({\theta }_{1}) \\ {l}_{2}\sin ({\theta }_{1} + {\theta }_{2}) + {l}_{1}\sin ({\theta }_{1}) \\ {d}_{1} + {d}_{2} + {d}_{4} \end{array} \right ). \end{array} $$

(11.17)

Fig. 11.7

Representation ${}^{0}\!{\textit{\textbf{P}}}_{4}^{o}$ of ${\textit{\textbf{P}}}_{4}^{o}$ in frame ${\mathcal{F}}_{0}$ is computed using ⁰​M ₄

4 Inverse Kinematics of Robot Manipulators Using Motor Algebra

Since calculations for the inverse kinematics are more complex than those for direct kinematics, our aim should be to find a systematic approach that will exploit the point, line, and plane motor algebra representations. Unfortunately, the procedure for the computation of the inverse kinematics is not amenable to a general formulation as in the case of the direct kinematics equation (11.14). That is why we are compelled to choose a real robot manipulator and compute its inverse kinematics in order to show all the characteristics of the computational assumptions.

The Stanford robot manipulator is well known among researchers concerned with the design of strategies for the symbolic computation of the inverse kinematics. According to Table 11.1, the variable parameters to be computed are θ₁, θ₂, θ₄, θ₅, θ₆, and d ₃. Using the Stanford robot manipulator, we will show that the motor algebra approach gives us the freedom to switch between the point, line, or plane representation, according to the geometrical circumstances. This is one of the most important advantages of our motor algebra approach.

Mechanically, the Stanford manipulator can be divided into two basic components. The first comprises joints 1, 2, and 3 and is dedicated to the task of general positioning. The second comprises joints 4, 5, and 6, and is dedicated to the wrist-like orientation of the end-effector. Since the philosophy of our approach relies on the application of point, line, or plane representation where it is needed, we must first evaluate, for any given case, which of these three representations is suitable for the jointtransitions. In this way, a better geometric insight is guaranteed and the solution method is more easily developed. The first three joints of the Stanford manipulator are used to position the origin of the coordinate frame ${\mathcal{F}}_{3}$. Therefore, we apply a point representation to describe this part of the problem. The last three joints are used to achieve the desired orientation of the end-effector frame. For the formulation of this subproblem, we use a line and a plane representation because these entities allow us to model orientations.

4.1 The Rendezvous Method

The next important step is to represent the motor transformations from the beginning of a chain of jointtransitions to their completion, and vice versa, as depicted in Fig. 11.8. As a result, we gain a set of equations for each meeting point. In each of these points, the forward equation is equivalent to the backward equation. We use these equalities as a guide in the computation of the unknowns. We will call this procedure the rendezvous method. This simple idea has proved to be very useful as a strategy for the solution of the inverse kinematics.

Fig. 11.8

Rendezvous method: If ⁱ​X and ^j​X are known, we can compute ^k​X for each i ≤ k ≤ j in one of two different ways: (1) by successive forward transformations of ⁱ​X, and (2) by successive backward transformations of ^j​X

4.2 Computing θ₁, θ₂, and d ₃ Using a Point

In the case of the Stanford manipulator, the orientation and position of frame ${\mathcal{F}}_{6}$ uniquely determines the position of frame ${\mathcal{F}}_{3}$. An explanation for this relation follows.

The position of frame ${\mathcal{F}}_{3}$ with respect to ${\mathcal{F}}_{0}$ is described by the multivector representation ⁰​P ₃ ^o of ${\textit{\textbf{P}}}_{3}^{o}$ in ${\mathcal{F}}_{0}$. By a successive forward transformation applied on ${}^{3}\!{\textit{\textbf{P}}}_{3}^{o} = 1$, we find the representation ${}^{6}\!{\textit{\textbf{P}}}_{3}^{o}$ of ${\textit{\textbf{P}}}_{3}^{o}$ in ${\mathcal{F}}_{6}$ by

$${ }^{6}\!{\textit{\textbf{P}}}_{ 3}^{o} {= }^{6}\!{\textit{\textbf{M}}{}_{ 3}\,\,}^{3}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{6}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 3} = 1-I\left (\begin{array}{c} 0\\ 0 \\ {d}_{6}\\ \end{array} \right).$$

(11.18)

Now we can compute ${}^{0}\!{\textit{\textbf{P}}}_{3}^{o}$ by

$$ \begin{array}{rcl}{ }^{0}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 6}\,\,}^{6}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6} {= }^{0}\!{\textit{\textbf{M}}}_{ 6}\,\,{\left (1 - I\left (\begin{array}{c} 0\\ 0 \\ {d}_{6}\\ \end{array} \right )\right )\,\,}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6} \\ & =& 1 + I\left (\begin{array}{c} {P}_{x} \\ {P}_{y} \\ {P}_{z}\\ \end{array} \right ). \end{array}$$

(11.19)

Note that ⁰​M ₆ is given. The vector (P _x , P _y , P _z )^T describes the position of the origin ${\textit{\textbf{P}}}_{3}^{o}$ of frame ${\mathcal{F}}_{3}$ in frame ${\mathcal{F}}_{0}$ for a given overall transformation ⁰​M ₆. Now we can apply the rendezvous method, since we know the representation of ${\textit{\textbf{P}}}_{3}^{o}$ in the two different frames ${\mathcal{F}}_{0}$ and ${\mathcal{F}}_{3}$ (see Fig. 11.9).

Fig. 11.9

The rendezvous method applied to ${\textit{\textbf{P}}}_{3}^{o}$ in order to determine the equations shown in Table 11.2 (the equations of the rendezvous frame ${\mathcal{F}}_{1}$ are chosen to compute the variable parameters θ₁, θ₂, and d ₃)

Table 11.2 Rendezvous equations obtained for ${\textit{\textbf{P}}}_{3}^{o}$ regarding frames ${\mathcal{F}}_{0}$, ${\mathcal{F}}_{1}$, ${\mathcal{F}}_{2}\mbox{, and }{\mathcal{F}}_{3}$

By applying successive forward transformations, we obtain

$$ \begin{array}{rcl}{ }^{1}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{1}\!{\textit{\textbf{M}}{}_{ 0}\,\,}^{0}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{1}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 0}, \\ {}^{2}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{2}\!{\textit{\textbf{M}}{}_{ 1}\,\,}^{1}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{2}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 1}, \\ {}^{3}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{3}\!{\textit{\textbf{M}}{}_{ 2}\,\,}^{2}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{3}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 2}.\end{array}$$

(11.20)

These computations were carried out with our MAPLE program, which calculated the left-hand sides of the four groups of equations in Table 11.2.

On the other hand, by applying successive backward transformations to the origin of ${\mathcal{F}}_{3}$ given by

$$ \begin{array}{rcl} {}^{3}\!{\textit{\textbf{P}}}_{ 3}^{o} = 1 + I\left (\begin{array}{c} 0\\ 0 \\ 0\\ \end{array} \right) = 1\, & &\end{array}$$

(11.21)

we get

$$ \begin{array}{rcl}{ }^{2}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{2}\!{\textit{\textbf{M}}{}_{ 3}\,\,}^{3}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{2}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 3} = 1 + I\left (\begin{array}{c} 0\\ 0 \\ {d}_{3}\\ \end{array} \right ), \\ {}^{1}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{1}\!{\textit{\textbf{M}}{}_{ 2}\,\,}^{2}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{1}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 2} = 1 + I\left (\begin{array}{c} {d}_{3}\sin ({\theta }_{2}) \\ - {d}_{3}\cos ({\theta }_{2}) \\ {d}_{2}\\ \end{array} \right ), \\ {}^{0}\!{\textit{\textbf{P}}}_{ 3}^{o}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 1}\,\,}^{1}\!{\textit{\textbf{P}}}_{ 3}^{o}\,\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 1} = 1 + I\left (\begin{array}{c} {d}_{3}\sin ({\theta }_{2})\cos ({\theta }_{1}) - {d}_{2}\sin ({\theta }_{1}) \\ {d}_{3}\sin ({\theta }_{2})\sin ({\theta }_{1}) + {d}_{2}\cos ({\theta }_{1}) \\ {d}_{3}\cos ({\theta }_{2}) + {d}_{1}\\ \end{array} \right )\!.\qquad \end{array}$$

(11.22)

These equations correspond to the right-hand sides of the four groups of equations in Table 11.2. For simplicity, we have used the abbreviation s _i for sin(θ _i ) and c _i for cos(θ _i ).

Using the third equation of the rendezvous frame ${\mathcal{F}}_{1}$, we compute

$${\theta }_{1} {=\arctan }_{2}({x}_{1/2},{y}_{1/2}),$$

(11.23)

where

$${x}_{1/2} = \frac{{d}_{2} - {P}_{y}{y}_{1/2}} {-{P}_{x}}, {y}_{1/2} = \frac{{P}_{y}{d}_{2} \pm {P}_{x}\sqrt{{P}_{x }^{2 } + {P}_{y }^{2 } - {d}_{2 }^{2}}} {{P}_{x}^{2} + {P}_{y}^{2}}, $$

(11.24)

and

$${ \arctan }_{2}(x,y) = \left \{\begin{array}{r@{\quad :\quad }l} \arctan (\frac{x} {y})\quad : \quad &y > 0 \\ \frac{\pi } {2} \quad : \quad &y = 0\,\,\mbox{ and}\,\,x > 0\\ \mbox{ undefined} \quad : \quad &y = 0\,\,\mbox{ and} \,\,x = 0 \\ -\frac{\pi } {2} \quad : \quad &y = 0\,\,\mbox{ and}\,\,x < 0 \\ \arctan (\frac{x} {y}) + \pi \quad : \quad &y < 0. \end{array} \right.$$

(11.25)

This gives two values for θ₁. Now, let us look for d ₃ and θ₂. For that, we consider the first and second equations of the rendezvous frame ${\mathcal{F}}_{1}$. With ${a}_{1/2} = {P}_{y}{x}_{1/2} + {P}_{x}{y}_{1/2}$ and $b = {P}_{z} - {d}_{1}$, we obtain two values for d ₃. Since for the Stanford manipulator d ₃ must be positive, we choose

$$\begin{array}{rcl}{ d}_{{3}_{1/2}} = \sqrt{{a}_{1/2 }^{2 } + {b}^{2}}\;.& &\end{array}$$

(11.26)

Using this value in Eqs. (11.21) and (11.22), we compute straightforwardly,

$$\begin{array}{rcl}{ \theta }_{2} {=\arctan }_{2}\left (\frac{{a}_{1/2}} {{d}_{{3}_{1/2}}}, \frac{b} {{d}_{{3}_{1/2}}} \right ).& &\end{array}$$

(11.27)

4.3 Computing θ₄ and θ₅ Using a Line

These variables will be computed using the jointtransition from ${\mathcal{F}}_{3}$ to ${\mathcal{F}}_{6}$. Given the geometric characteristics of the manipulator, an appealing option is to use the line representation to set up an appropriate equation system. The representation ⁰​L ₆ ^z of the line L ₆ ^z in frame ${\mathcal{F}}_{0}$ can be computed using ⁰​M ₆:

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 6}\,\,}^{6}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{0}\widetilde{{\textit{\textbf{M}}}_{ 6}} {= }^{0}\!{\textit{\textbf{M}}}_{ 6}{\left (\left (\begin{array}{c} 0\\ 0 \\ 1\\ \end{array} \right) + I\left (\begin{array}{c} 0\\ 0 \\ 0\\ \end{array} \right )\right )}^{0}\widetilde{{\textit{\textbf{M}}}_{ 6}}.\end{array}$$

(11.28)

Since the z-axis of the ${\mathcal{F}}_{6}$ frame crosses the origin of ${\mathcal{F}}_{3}$, we can see that the z-axis line related to this frame has zero moment. Thus, we can claim that L ₆ ^z in the ${\mathcal{F}}_{3}$ frame is

$$ \begin{array}{rcl} {}^{3}\!{\textit{\textbf{L}}}_{ 6}^{z} {= }^{3}\!{\textit{\textbf{M}}{}_{ 0}\,\,}^{0}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{3}\widetilde{{\textit{\textbf{M}}}_{ 0}} = \left (\begin{array}{c} {A}_{x} \\ {A}_{y} \\ {A}_{z}\\ \end{array} \right) + I\left (\begin{array}{c} 0\\ 0 \\ 0\\ \end{array} \right ).& &\end{array}$$

(11.29)

Note that ³​M ₀ is known since we have already computed θ₁, θ₂, and d ₃.

Now, applying successive forward transformations, as follows

$$ \begin{array}{rcl}{ }^{4}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{4}\!{\textit{\textbf{M}}{}_{ 3}\,\,}^{3}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{4}\widetilde{{\textit{\textbf{M}}}_{ 3}}, \\{ }^{5}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{5}\!{\textit{\textbf{M}}{}_{ 4}\,\,}^{4}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{5}\widetilde{{\textit{\textbf{M}}}_{ 4}}, \\ {}^{6}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{6}\!{\textit{\textbf{M}}{}_{ 5}\,\,}^{5}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{6}\widetilde{{\textit{\textbf{M}}}_{ 5}},\end{array}$$

(11.30)

we obtain the left-hand sides of the four groups of equations in Table 11.3. The z-axis line L ₆ ^z of ${\mathcal{F}}_{6}$ represented in ${\mathcal{F}}_{6}$ has zero moment, and can therefore be expressed as

$$ \begin{array}{rcl} {}^{6}\!{L}_{ 6}^{z} = \left (\begin{array}{c} 0\\ 0 \\ 1\\ \end{array} \right) + I\left (\begin{array}{c} 0\\ 0 \\ 0\\ \end{array} \right ).& &\end{array}$$

(11.31)

Next, applying successive backward transformations, we obtain

$$ \begin{array}{rcl}{ }^{5}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{5}\!{\textit{\textbf{M}}{}_{ 6}\,\,}^{6}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{5}\widetilde{{\textit{\textbf{M}}}}_{ 6}, \\ {}^{4}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{4}\!{\textit{\textbf{M}}{}_{ 5}\,\,}^{5}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{4}\widetilde{{\textit{\textbf{M}}}}_{ 5}, \\ {}^{3}\!{\textit{\textbf{L}}}_{ 6}^{z}& =& {}^{3}\!{\textit{\textbf{M}}{}_{ 4}\,\,}^{4}\!{\textit{\textbf{L}}}_{ 6}^{z}\,\,{}^{3}\widetilde{{\textit{\textbf{M}}}}_{ 4}.\end{array}$$

(11.32)

Using our MAPLE program, we then compute the right-hand sides of the four groups of equations in Table 11.3.

Table 11.3 Rendezvous equations obtained for L ₆ ^z regarding frames ${\mathcal{F}}_{3},{\mathcal{F}}_{4},{\mathcal{F}}_{5}$, and ${\mathcal{F}}_{6}$

Finally, we will consider the equations of rendezvous frame ${\mathcal{F}}_{4}$. Using the third equations of Table 11.3, we compute

$$\begin{array}{rcl}{ \theta }_{4} {=\arctan }_{2}({x}_{1/2},{y}_{1/2})\;,& &\end{array}$$

(11.33)

where

$$\begin{array}{rcl}{ x}_{1/2} = -\frac{{A}_{y}{y}_{1/2}} {-Ax} = \pm \frac{{A}_{y}} {\sqrt{{A}_{x }^{2 } + {A}_{y }^{2}}}\;,{y}_{1/2} = \pm \frac{{A}_{x}} {\sqrt{{A}_{x }^{2 } + {A}_{y }^{2}}}\;.& &\end{array}$$

(11.34)

This results in two values for θ₄, which when substituted into the first and second equations of Table 11.3 helps us to find two solutions for θ₅:

$$\begin{array}{rcl}{ \theta }_{5} {=\arctan }_{2}({s}_{5},{c}_{5}) {=\arctan }_{2}\Big{(}(-{A}_{y}{s}_{4} - {A}_{x}{c}_{4}),-{A}_{z}\Big{)}\;.& &\end{array}$$

(11.35)

4.4 Computing θ₆ Using a Plane Representation

Since θ₁, θ₂, d ₃, θ₄, and θ₅ are now known, we can compute the motor ⁵​M ₀. The yz-plane ${\textit{\textbf{H}}}_{6}^{yz}$ represented in ${\mathcal{F}}_{6}$ has a Hesse distance of zero; thus,

$$ \begin{array}{rcl} {}^{6}\!{\textit{\textbf{H}}}_{ 6}^{yz} = \left (\begin{array}{c} 1\\ 0 \\ 0\\ \end{array} \right) + I0 = \left (\begin{array}{c} 1\\ 0 \\ 0\\ \end{array} \right ).& &\end{array}$$

(11.36)

The transformation of this plane to ${\mathcal{F}}_{0}$ reads

$$ \begin{array}{rcl} {}^{0}\!{\textit{\textbf{H}}}_{ 6}^{yz}& =& {}^{0}\!{\textit{\textbf{M}}{}_{ 6}\,\,}^{6}\!{\textit{\textbf{H}}}_{ 6}^{yz}\,\,{}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6} {= }^{0}\!{\textit{\textbf{M}}}_{ 6}\,\,{\left (\begin{array}{c} 1\\ 0 \\ 0\\ \end{array} \right )\,\,}^{0}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6}.\end{array}$$

(11.37)

Now we compute ${}^{5}\!{\textit{\textbf{H}}}_{6}^{yz}$:

$$ \begin{array}{rcl} {}^{5}\!{\textit{\textbf{H}}}_{ 6}^{yz} {= }^{5}\!{\textit{\textbf{M}}{}_{ 0}\,\,}^{0}\!{\textit{\textbf{H}}}_{ 6}^{yz}\,\,{}^{5}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 0}& =& \left (\begin{array}{c} {N}_{x} \\ {N}_{y} \\ {N}_{z}\\ \end{array} \right) + {I\,}^{5}{d}_{{ H}_{6}^{yz}}.\end{array}$$

(11.38)

The orientation bivector (N _x , N _y , N _z )^T describes the orientation of the yz-plane of frame ${\mathcal{F}}_{6}$ in frame ${\mathcal{F}}_{5}$, given the values of the joint variables θ₁, θ₂, θ₄, θ₅, and d ₃. By applying forward transformation from ${\mathcal{F}}_{5}$ to ${\mathcal{F}}_{6}$, we obtain

$${ }^{6}\!{\textit{\textbf{H}}}_{ 6}^{yz} {= }^{6}\!{\textit{\textbf{M}}{}_{ 5}\,\,}^{5}\!{\textit{\textbf{H}}}_{ 6}^{yz}\,\,{}^{6}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 5}\;.$$

(11.39)

Then, using our MAPLE program, we calculate the left-hand sides of the two groups of equations in Table 11.4. Since the values for θ₁, θ₂, d ₃, θ₄, and θ₅ are not unique, we will obtain different values for the equations. By applying ⁵​M ₆ to ⁶​H ₆ ^yz, we next obtain the right-hand sides of the two groups of equations in Table 11.4:

$$ \begin{array}{rcl} {}^{5}\!{\textit{\textbf{H}}}_{ 6}^{yz}& =& {}^{5}\!{\textit{\textbf{M}}{}_{ 6}\,\,}^{6}\!{\textit{\textbf{H}}}_{ 6}^{yz}\,\,{}^{5}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6}\, =\, {}^{5}\!{\textit{\textbf{M}}}_{ 6}\,\,{\left (\begin{array}{c} 1\\ 0 \\ 0\\ \end{array} \right )\,\,}^{5}\widetilde{{\overline{\textit{\textbf{M}}}}}_{ 6} \\ & =& \left (\begin{array}{c} \sin ({\theta }_{6}) \\ \cos ({\theta }_{6}) \\ 0\\ \end{array} \right ). \end{array}$$

(11.40)

Table 11.4 Rendezvous equations obtained for ${\textit{\textbf{H}}}_{6}^{yz}$ regarding frames ${\mathcal{F}}_{5}$ and ${\mathcal{F}}_{6}$

Finally, we will consider the equations of the rendezvous frame ${\mathcal{F}}_{5}$. Using the first and second equations in Table 11.4, we can compute θ₆ by

$$\begin{array}{rcl}{ \theta }_{6} {=\arctan }_{2}({s}_{6},{c}_{6}) {=\arctan }_{2}({N}_{x},{N}_{y})\;.& &\end{array}$$

(11.41)

Note that since there are two values for θ₄ and two values for θ₅, there is more than one solution for θ₆.

5 Inverse Kinematics Using the 3D Affine Plane

In this section we compute the inverse kinematics of a robot manipulator using the framework of the 3D affine plane (see Chap. 5). On the one hand, one can use a geometric algebra ${\mathcal{G}}_{p,q,0}$ with a Minkowski metric for computations of projective geometry and the algebra of incidence, as is the case for computer vision problems (see Chap. 9). On the other hand, one can use a degenerated algebra for computations involving rigid motions, as is often the case in the field of robotics (see, e.g., our use of the motor algebra ${\mathcal{G}}_{3,0,1}^{+}$ in Sect. 11.4 or the Clifford algebra ${\mathcal{G}}_{3,0,1}$ in [169]). In a more general way, the 3D affine plane allows calculations that can involve both 3D rigid transformations and the meet and join operations of the algebra of incidence.

Note that in this section we assume that the projection P _A back to the affine plane is always carried out, and thus we need not make it explicit in the formulas. In the procedure, after the equations to compute the angles θ _i have been found, simple dual trigonometric relations can then be used to determine the angles. In order to show the methodology at a symbolic level, we will avoid showing these trigonometric computation details.

The transformation M _t of a robot manipulator that takes the end-effector from its home position to a configuration determined by the n-degrees of freedom of the joint angles θ₁, θ₂, …, θ _n is given by

$$\begin{array}{rcl}{ M}_{t} = {M}_{1}{M}_{2}{M}_{3}\cdots {M}_{n},& &\end{array}$$

(11.42)

where the screw versor of a joint M _i = T _i R _i is dependent on the angle θ _i .

The task of inverse kinematics is to calculate the angles θ _i for a given final configuration of the end-effector. Robot manipulators are equipped with a single parallel revoluted axis as well as intersecting axes. The latter can be located at the end-effector or at the home position. Two typical configurations are illustrated in Fig. 11.10a,b. The mechanical characteristics of the robot manipulators can be used to simplify the computations – by considering either the invariant plane ϕ^h, in the case of three parallel revoluted line axes (see Fig. 11.10a), or the invariant point p ^h, in the case of the intersecting revoluted line axis (see Fig. 11.10b).

Fig. 11.10

Robot manipulators: (a) (top) intersecting revoluted line axes at the end-effector; or (b) (bottom) at the home position

We can solve the inverse kinematics problem by breaking it up into a series of separate equations, using the strategy suggested by Selig [169] (see Chap. 11). We will illustrate the procedure for a robot with 6 DOF. First, we rearrange the terms of Eq. 11.42:

$$\begin{array}{rcl}{ M}_{2}{M}_{3}{M}_{4} =\widetilde{ {M}}_{1}{M}_{t}\widetilde{{M}}_{6}\widetilde{{M}}_{5}.& &\end{array}$$

(11.43)

In the case of three parallel joints, we can isolate them by considering the common perpendicular plane ϕ^h that satisfies the equation

$$\begin{array}{rcl}{ \phi }^{h} = {M}_{ 2}{M}_{3}{M}_{4}\,{\phi }^{h}\,\widetilde{{M}}_{ 4}\widetilde{{M}}_{3}\widetilde{{M}}_{2} =\widetilde{ {M}}_{1}{M}_{t}(\widetilde{{M}}_{6}(\widetilde{{M}}_{5}\,{\phi }^{h}{M}_{ 5}){M}_{6})\widetilde{{M}}_{t}{M}_{1}.\qquad & &\end{array}$$

(11.44)

In the case of a meeting point p ^h, we can isolate the three coincident joints as follows:

$$\begin{array}{rcl}{ p}^{h} = {M}_{ 2}{M}_{3}{M}_{4}\,{p}^{h}\,\widetilde{{M}}_{ 4}\widetilde{{M}}_{3}\widetilde{{M}}_{2} =\widetilde{ {M}}_{1}{M}_{t}\widetilde{{M}}_{6}\widetilde{{M}}_{5}\,{p}^{h}\,{M}_{ 5}\widetilde{{M}}_{6}\widetilde{{M}}_{t}{M}_{1}.\quad & &\end{array}$$

(11.45)

In this way, we have separated the problem into two systems of equations:

$$\widetilde{{M}}_{t}{M}_{1}\,{\phi }^{h}\,\widetilde{{M}}_{ 1}{M}_{t} =\widetilde{ {M}}_{6}\widetilde{{M}}_{5}\,{\phi }^{h}{M}_{ 5}{M}_{6}$$

(11.46)

$$\begin{array}{rcl} & & \mbox{ or} \\ & & \widetilde{{M}}_{t}{M}_{1}\,{p}^{h}\,\widetilde{{M}}_{ 1}{M}_{t} =\widetilde{ {M}}_{6}\widetilde{{M}}_{5}\,{p}^{h}{M}_{ 5}{M}_{6},\end{array}$$

(11.47)

$${M}_{2}{M}_{3}{M}_{4} = {M}_{1}{M}_{t}\widetilde{{M}}_{6}\widetilde{{M}}_{5} = M'.$$

(11.48)

We can now compute for θ₁, θ₅, θ₆ with the help of either Eq. 11.46 (see Fig. 11.10a) or Eq. 11.47 (see Fig. 11.10b). Then, by using these results and Eq. 11.48, we can solve for θ₂, θ₃, θ₄.

Let us demonstrate how the procedure works for the case of three intersecting revoluted joint axes in the common plane at the end-effector (see Fig. 11.10a). When the plane ${\phi }_{b}^{h}$ (perpendicular to the line axes l ₂, l ₃, and l ₄) is rotated about the end-joint, the point p _i ^h on the line axis of the revoluted end-joint l ₆ ^h remains invariant. Using a meet operation and Eq. 11.46, the angle θ₆ can be eliminated:

$$\begin{array}{rcl}{ p}_{i}^{h}& =& (\widetilde{{M}}_{ t}{M}_{1}\,{\phi }^{h}\,\widetilde{{M}}_{ 1}{M}_{t}) \cap {l}_{6}^{h} = (\widetilde{{M}}_{ 6}\widetilde{{M}}_{5}\,{\phi }^{h}{M}_{ 5}{M}_{6}) \cap {l}_{6}^{h} \\ & =& (\widetilde{{M}}_{5}\,{\phi }^{h}\,{M}_{ 5}) \cap {l}_{6}^{h}. \end{array}$$

(11.49)

In the case of a robot manipulator in the home position, the revoluted joint axes are the manipulator basis. Equation 11.47 shows that the point p ^h is an invariant for the parallel fourth and fifth line axes. Thus, we can use the equation

$$\begin{array}{rcl}{ M}_{6}\widetilde{{M}}_{t}\,{p}^{h}\,{M}_{ t}\widetilde{{M}}_{6} =\widetilde{ {M}}_{5}\widetilde{{M}}_{4}\,{p}^{h}{M}_{ 4}{M}_{5}& &\end{array}$$

(11.50)

to solve for the angles θ₄ and θ₅. Using the line l ₅ and p ^h, we get the invariant plane,

$$\begin{array}{rcl}{ \phi }_{i}^{h} = {M}_{ 6}\widetilde{{M}_{t}}\,{p}^{h}{M}_{ t}\widetilde{{M}}_{6}\! \wedge \! {l}_{5}.& &\end{array}$$

(11.51)

The 3D coordinates of this plane are known in advance and correspond to the plane x − z or e ₃₂; thus, Eq. 11.51 allows us to solve for the angle θ₆. Having determined angle θ₆, we can now use Eq. 11.50 to complete the calculation of θ₄ and θ₅.

Now consider the three coincident line axes ${l}_{1}^{h},\,{l}_{2}^{h},\,{l}_{3}^{h}$ given in Fig. 11.10b. We can isolate the angle θ₂ by considering the invariant relation based on the meet of two of these lines. Thus,

$$\begin{array}{rcl} ({M}_{2}\,{l}_{3}^{h}\,\widetilde{{M}_{ 2}}) \cap {l}_{1}^{h} = (M'\,{l}_{ 3}^{h}\,\widetilde{M}') \cap {l}_{ 1}^{h} = {p}_{ 0}^{h},& &\end{array}$$

(11.52)

where M′ = M ₁ M ₂ M ₃ and p ₀ ^h is the invariant intersecting point. When the lines are parallel, as shown in Fig. 11.10a, we can use the same invariant relation, but now we consider the intersecting point to be at infinity, so that M′ = M ₂ M ₃ M ₄.

6 Inverse Kinematic Using Conformal Geometric Algebra

In this section, we briefly describe the procedural steps to compute the inverse kinematics of a 5-DOF robot arm using the conformal geometric algebra framework. We have presented above a similar computation using motor algebra ${\mathcal{G}}_{3,0,1}$ and the affine plane and the next conformal geometric algebra ${\mathcal{G}}_{4,1}$. Our purpose is that the reader by studying these examples learns to compute using the geometric algebra and he or she can get a better understanding of the potential and limitations of the different computational framework models of geometric algebra.

Objective Find the joint angles in order to place the robot arm at the point p _t in such a way that the gripper will stay parallel to the plane ϕ _t .

Step 1: Find the position of the point p ₂

$$\begin{array}{rcl}{ \textit{\textbf{l}}}_{y}^{{_\ast}}& =& {e}_{ 2}\textit{\textbf{E}} \\ { \textit{\textbf{s}}}_{t}& =&{ \textit{\textbf{p}}}_{t} -\frac{1} {2}{d}_{3}^{2}{e}_{ \infty } \\ { \textit{\textbf{z}}}_{t}& =&{ \textit{\textbf{s}}}_{t}\! \wedge \! {\pi }_{t} \\ { \textit{\textbf{j}}}_{z}^{{_\ast}}& =&{ \textit{\textbf{z}}}_{ t}\! \wedge \! {e}_{\infty } \\ { \textit{\textbf{l}}}_{d}^{{_\ast}}& =& d({\textit{\textbf{p}}}_{ t},{\textit{\textbf{l}}}_{y}^{{_\ast}})\! \wedge \!{\textit{\textbf{p}}}_{ t}\! \wedge \! {e}_{\infty } \\ {\pi }_{j{l}_{d}}^{{_\ast}}& =&{ \textit{\textbf{j}}}_{ z}^{{_\ast}}\!\wedge \! ({\textit{\textbf{l}}}_{ d}^{{_\ast}}\textit{\textbf{E}}) \\ { \textit{\textbf{P}}}_{{P}_{2}}& =& {\pi }_{j{l}_{d}}\! \wedge \!{\textit{\textbf{z}}}_{t} \\ { \textit{\textbf{p}}}_{2}& =& \frac{{\textit{\textbf{P}}}_{{P}_{2}}^{{_\ast}} + \sqrt{{\textit{\textbf{P} } }_{{P}_{2 } }^{{_\ast} } \cdot { \textit{\textbf{P} } }_{{P}_{2 } }^{{_\ast}}}} {{\textit{\textbf{P}}}_{{P}_{2}}^{{_\ast}}\cdot {e}_{\infty }}, \\ \end{array}$$

where $d({\textit{\textbf{p}}}_{t},{\textit{\textbf{l}}}_{y}^{{_\ast}})$ stands for the directed distance between p _t and ${\textit{\textbf{l}}}_{y}^{{_\ast}}$.

Step 2: Find the point p ₁:

$$\begin{array}{rcl}{ \textit{\textbf{s}}}_{1}& =& {e}_{0} -\frac{{d}_{1}^{2}} {2} {e}_{\infty } \\ { \textit{\textbf{s}}}_{2}& =&{ \textit{\textbf{p}}}_{2} -\frac{{d}_{2}^{2}} {2} {e}_{\infty } \\ {\pi }_{1}^{{_\ast}}& =&{ \textit{\textbf{l}}}_{ y}\! \wedge \!{\textit{\textbf{p}}}_{2} = {e}_{2}\textit{\textbf{E}}\! \wedge \!{\textit{\textbf{p}}}_{2} \\ {\pi }_{2}^{{_\ast}}& =& {e}_{ 3}{I}_{c} \\ {\textit{\textbf{P}}}_{{P}_{1}}& =&{ \textit{\textbf{s}}}_{1}\! \wedge \!{\textit{\textbf{s}}}_{2}\! \wedge \! {\pi }_{1} \\ { \textit{\textbf{p}}}_{1}& =& \frac{{\textit{\textbf{P}}}_{{P}_{1}}^{{_\ast}} + \sqrt{{\textit{\textbf{P} } }_{{P}_{1 } }^{{_\ast} } \cdot { \textit{\textbf{P} } }_{{P}_{1 } }^{{_\ast}}}} {{\textit{\textbf{P}}}_{{P}_{1}}^{{_\ast}}\cdot {e}_{\infty }} \end{array}.$$

Step 3: Find the point p ₀:

$$\begin{array}{rcl} {\pi }_{1}^{{_\ast}}& =&{ \textit{\textbf{l}}}_{ y}^{{_\ast}}\!\wedge \!{\textit{\textbf{p}}}_{ 2} \\ { \textit{\textbf{s}}}_{0}& =& {e}_{0} -\frac{{d}_{0}^{2}} {2} {e}_{\infty } \\ {\pi }_{0}& =& {e}_{2} - h{e}_{\infty } \\ { \textit{\textbf{z}}}_{0}& =&{ \textit{\textbf{s}}}_{0}\! \wedge \! {\pi }_{0} \\ {\textit{\textbf{P}}}_{{P}_{0}}& =&{ \textit{\textbf{z}}}_{0}\! \wedge \! {\pi }_{1} \\ { \textit{\textbf{p}}}_{0}& =& \frac{{\textit{\textbf{P}}}_{{P}_{0}}^{{_\ast}} + \sqrt{{\textit{\textbf{P} } }_{{P}_{0 } }^{{_\ast} } \cdot { \textit{\textbf{P} } }_{{P}_{0 } }^{{_\ast}}}} {{\textit{\textbf{P}}}_{{P}_{0}}^{{_\ast}}\cdot {e}_{\infty }} \end{array}.$$

Step 4: Compute the lines l ₁, l ₂, and l ₃:

$$\begin{array}{rcl}{ \textit{\textbf{l}}}_{1}^{{_\ast}}& =&{ \textit{\textbf{p}}}_{ 0}\! \wedge \!{\textit{\textbf{p}}}_{1}\! \wedge \! {e}_{\infty } \\ {\textit{\textbf{l}}}_{2}^{{_\ast}}& =&{ \textit{\textbf{p}}}_{ 1}\! \wedge \!{\textit{\textbf{p}}}_{2}\! \wedge \! {e}_{\infty } \\ {\textit{\textbf{l}}}_{3}^{{_\ast}}& =&{ \textit{\textbf{p}}}_{ 2}\! \wedge \!{\textit{\textbf{p}}}_{1}\! \wedge \! {e}_{\infty }.\end{array}$$

(11.53)

Step 5: Compute the angles

$$\begin{array}{rcl} \cos ({\theta }_{1})& =& \frac{{\pi }_{1}^{{_\ast}}\cdot {\pi }_{2}^{{_\ast}}} {\vert {\pi }_{1}^{{_\ast}}\vert \vert {\pi }_{2}^{{_\ast}}\vert },\,\,\,\cos ({\theta }_{2}) = \frac{{\textit{\textbf{l}}}_{1}^{{_\ast}}\cdot {\textit{\textbf{l}}}_{y}^{{_\ast}}} {\vert {\textit{\textbf{l}}}_{1}^{{_\ast}}\vert \vert {\textit{\textbf{l}}}_{y}^{{_\ast}}\vert } \\ \cos ({\theta }_{3})& =& \frac{{\textit{\textbf{l}}}_{1}^{{_\ast}}\cdot {\textit{\textbf{l}}}_{3}^{{_\ast}}} {\vert {\textit{\textbf{l}}}_{1}^{{_\ast}}\vert \vert {\textit{\textbf{l}}}_{3}^{{_\ast}}\vert },\,\,\,\cos ({\theta }_{4}) = \frac{{\pi }_{1}^{{_\ast}}\cdot {\pi }_{3}^{{_\ast}}} {\vert {\pi }_{1}^{{_\ast}}\vert \vert {\pi }_{3}^{{_\ast}}\vert } \\ \cos ({\theta }_{5})& =& \frac{{\textit{\textbf{l}}}_{3}^{{_\ast}}\cdot {\textit{\textbf{l}}}_{2}^{{_\ast}}} {\vert {\textit{\textbf{l}}}_{3}^{{_\ast}}\vert \vert {\textit{\textbf{l}}}_{2}^{{_\ast}}\vert }\end{array}.$$

Next, we present a procedure involving the computing of the inverse kinematics of a pan-tilt unit (PTU).

Objective Find the joint angles of the PTU unit in order to orient its principal ray towards the target point p _t .

Step 1: Find p ₂

$$\begin{array}{rcl}{ \textit{\textbf{s}}}_{1}& =&{ \textit{\textbf{p}}}_{1} -\frac{1} {2}{d}_{2}^{2}{e}_{ \infty } \\ D& =& d({\textit{\textbf{p}}}_{1},{\textit{\textbf{p}}}_{t}) \\ { \textit{\textbf{s}}}_{2}& =&{ \textit{\textbf{p}}}_{1} -\frac{1} {2}(D - {d}_{2}^{2}){e}_{ \infty } \\ { \textit{\textbf{l}}}_{y}^{{_\ast}}& =&{ \textit{\textbf{p}}}_{ 0}\! \wedge \!{\textit{\textbf{p}}}_{1}\! \wedge \! {e}_{\infty } \\ {\pi }_{1}^{{_\ast}}& =&{ \textit{\textbf{l}}}_{ y}^{{_\ast}}\!\wedge \!{\textit{\textbf{p}}}_{ t} \\ \textit{\textbf{P}}{\textit{\textbf{P}}}_{2}& =&{ \textit{\textbf{s}}}_{1}\! \wedge \!{\textit{\textbf{s}}}_{2}\! \wedge \! {\pi }_{1}\end{array}.$$

Step 2: Compute the line and the plane:

$$\begin{array}{rcl}{ \textit{\textbf{l}}}_{2}^{{_\ast}}& =&{ \textit{\textbf{p}}}_{ 1}\! \wedge \!{\textit{\textbf{p}}}_{2}\! \wedge \! {e}_{\infty } \\ {\pi }_{2}^{{_\ast}}& =&{ \textit{\textbf{l}}}_{ y}^{{_\ast}}\!\wedge \! {e}_{ 3}, \\ \end{array}$$

Step 3: Compute the tilt and pan angles:

$$\begin{array}{rcl} \cos ({\theta }_{\mathit{tilt}})& =& \frac{{\textit{\textbf{l}}}_{y} \cdot {\textit{\textbf{l}}}_{2}} {\vert {\textit{\textbf{l}}}_{y}\vert \vert {\textit{\textbf{l}}}_{2}\vert } \\ \cos ({\theta }_{\mathit{pan}})& =& \frac{{\pi }_{1} \cdot {\pi }_{2}} {\vert {\pi }_{1}\vert \vert {\pi }_{2}\vert }, \\ \end{array}$$

where d(p ₁, p _t ) stands for the directed distance between p ₁ and p _t .

7 Conclusion

This chapter presented the application of the algebra of motors for the treatment of the direct and inverse kinematics of robot manipulators. When dealing with 3D rigid motion, it is usual to use homogeneous coordinates in the 4D space to linearize this nonlinear 3D transformation. With the same effect, we model the prismatic and revolute motion of points, lines, and planes using motors that are equivalent to screws. In our approach, we can also use the representation of planes, and this expands the useful geometric language for the treatment of robotic problems. In addition, we illustrate the use of the 3D affine plane for computing inverse kinematics, taking advantage of incidence algebra operations to simplify computations that use intersections and unions of points, lines, and planes under rigid motion.

The chapter has shown the flexibility of the motor algebra approach for the solution of the direct and inverse kinematics of robot manipulators. To solve for the robot’s inverse kinematics, we have shown that, depending on our needs, we can use representations either of points, lines, or planes, which help to enormously reduce the complexity of the computation without losing geometric insight. Similarly, the use of incidence algebra in the affine plane framework allows us to define geometric constraints, or flags, by taking advantage of meet and joint operations, thereby allowing easy identification of the most important geometric objects involved.

For the computation of inverse kinematics, we have also used the conformal geometric algebra which besides points, lines, and planes uses circles and spheres as well; the use of this broad set of geometric entities helps us greatly to simplify the complex computation of the inverse kinematics.

The main contribution of this chapter has been to show that our approach offers more flexibility while at the same time preserving geometric insight during computation. The author believes that the versatility of the geometric algebra framework offers an alternative approach to the increasingly complex of application of multilink mechanisms.