In this section, we describe the equations of kinetic and potential energy in terms of geometric algebra. Based on these equations and using the Lagrange equation, we synthesize the dynamic model of any n-degrees of freedom serial robot.
3.1 Kinetic Energy
We introduce in our analysis the mass center in order to formulate an expression that describes the kinetic energy of a system of particles.
Kinetic Energy of a System of Particles We are considering a system with n particles shown in Fig. 12.1. The total relative kinetic energy, K, of the system is given by
$$\begin{array}{rcl} K = \sum \limits _{i=1}^{n}\frac{1} {2}{m}_{i}{V }_{i}^{2}.& &\end{array}$$
(12.17)
Now we will rewrite Eq. 12.17 to introduce the mass center. Here r
i
represents the distance to the particle, r
c
the distance to the mass center, and ρ
ci
the distance from the mass center to the particle:
$$\begin{array}{rcl}{ r}_{i} = {r}_{c} + {\rho }_{ci}.& &\end{array}$$
(12.18)
The time derivative of Eq. 12.18 is
$$\begin{array}{rcl} \dot{{r}}_{i} =\dot{ {r}}_{c} +\dot{ {\rho }}_{ci}.& &\end{array}$$
(12.19)
Therefore, the velocity equation of the ith particle (\(\dot{{\rho }}_{ci}\)) with respect to the mass center is given by
$$\begin{array}{rcl}{ V }_{i} = {V }_{c} +\dot{ {\rho }}_{ci}.& &\end{array}$$
(12.20)
By substitution of Eq. 12.20 in the expression of kinetic energy (12.17), we obtain
$$\begin{array}{rcl} K& =& \sum \limits _{i=1}^{n}\frac{1} {2}{m}_{i}{({V }_{c} +\dot{ {\rho }}_{ci})}^{2} \\ & =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}{({V }_{c})}^{2} +\sum \limits _{i=1}^{n}{m}_{ i}{V }_{c}\dot{{\rho }}_{ci} + \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}{(\dot{{\rho }}_{ci})}^{2}.\end{array}$$
(12.21)
As V
c
is not related to the sum index i, we can extract it:
$$\begin{array}{rcl} K = \frac{1} {2}{V }_{c}^{2}\left (\sum \limits _{i=1}^{n}{m}_{ i}\right) + {V }_{c} \frac{\mbox{ d}} {\mbox{ d}t}\sum \limits _{i=1}^{n}{m}_{ i}{\rho }_{ci} + \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}\dot{{\rho }}_{ci}^{2}.& &\end{array}$$
(12.22)
As \(m = \sum \limits _{i=1}^{n}{m}_{i}\) is the total mass of the system and considering that ∑
i = 1
n
m
i
ρ
ci
is by definition equal to zero,
$$\begin{array}{rcl} K = \frac{1} {2}m{V }_{c}^{2} + \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}\dot{{\rho }}_{ci}^{2}.& &\end{array}$$
(12.23)
As a conclusion, we see that kinetic energy with respect to a reference system can be considered as the sum of two parts: (1) the kinetic energy of total mass moves with respect to this reference system at the same velocity, plus (2) the kinetic energy of the particles moves with respect to the mass center (momentum of inertia ).
The Kinetic Energy of a Robot Arm We denote by x
i
the mass center i in its initial position and by x
′
i
the mass center as a function of joints variables (see Fig. 12.2). Similarly, we will denote the joints axis i as L
i
and the joints axis i as a function of the joints variables as L
′
i
. Recall the direct kinematics equation (12.1) that relates x
i
to x
′
i
and L to L
′
i
and is written using conformal geometric algebra as
$$ \begin{array}{rcl}{ \mathbf{\mathit{x}}'}_{i}& =&{ \mathbf{\mathit{M}}}_{1}{\mathbf{\mathit{M}}}_{2}\cdots {\mathbf{\mathit{M}}}_{i}{x}_{i}\tilde{{\mathbf{\mathit{M}}}}_{i}\cdots \tilde{{\mathbf{\mathit{M}}}}_{2}\tilde{{\mathbf{\mathit{M}}}}_{1}, \\ \end{array} $$
(12.24)
$$ \begin{array}{rcl}{ \mathbf{\mathit{L}}'}_{i}& =&{ \mathbf{\mathit{M}}}_{1}{\mathbf{\mathit{M}}}_{2}\cdots {\mathbf{\mathit{M}}}_{i-1}{\mathbf{\mathit{L}}}_{i}\tilde{{\mathbf{\mathit{M}}}}_{i-1}\cdots \tilde{{\mathbf{\mathit{M}}}}_{2}\tilde{{\mathbf{\mathit{M}}}}_{1}. \\ \end{array} $$
(12.25)
We have seen that the kinetic energy is equal to the sum of the energy related to the velocity of the mass center and the energy related to the momentum of inertia. So the kinetic energy of the link i is computed as
$$\begin{array}{rcl}{ K}_{i} = \frac{1} {2}{m}_{i}\dot{{\mathbf{\mathit{x}}'{}_{i}}}^{2} + \frac{1} {2}{I}_{i}{\left (\sum \limits _{j=1}^{i}\dot{{q}}_{ j}\right )}^{2},& &\end{array}$$
(12.26)
where I
i
is the inertia of the link i and \(\dot{{\mathbf{\mathit{x}}'}_{i}}\) represents the velocity of the mass center x
′
i
. The velocity of the mass center is computed using the equation of differential kinematics (12.15), explained in Sect. 12.2:
$$\begin{array}{rcl} \dot{{\mathbf{\mathit{x}}'}}_{i} ={ \mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right ).& &\end{array}$$
(12.27)
Replacing Eq. 12.27 in (12.26), we have the expression of kinetic energy in conformal geometric algebra:
$$\begin{array}{rcl}{ K}_{i} = \frac{1} {2}{m}_{i}{\left [{\mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right )\right ]}^{2} + \frac{1} {2}{I}_{i}{\left (\sum \limits _{j=1}^{i}\dot{{q}}_{ j}\right )}^{2}.& &\end{array}$$
(12.28)
The total kinetic energy on the arm is given by the expression \(\sum \nolimits _{i=1}^{n} K_i\), where n is the number of degrees of freedom. In order to simplify the explanation, we will separate the kinetic energy \(K = {K}_{v} + {K}_{I}\) in two components, K
v
and K
I
, defined as
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}{\left [{\mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right )\right ]}^{2},\end{array}$$
(12.29)
$$\begin{array}{rcl}{ K}_{I}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{I}_{ i}{\left (\sum \limits _{j=1}^{i}\dot{{q}}_{ j}\right )}^{2}.\end{array}$$
(12.30)
We will attend first to K
v
and later to K
I
. The objective is to simplify the expression of total kinetic energy in the arm:
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}{\left [{\mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right )\right ]}^{2}.\end{array}$$
(12.31)
The square of the velocity’s magnitude is equal to the dot product of the vector with itself:
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}\left [{\mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right )\right ] \cdot \left [{\mathbf{\mathit{x}}'}_{i} \cdot \left (\sum \limits _{j=1}^{i}{\mathbf{\mathit{L}}'}_{ j}\dot{{q}}_{j}\right )\right ],\end{array}$$
(12.32)
introducing the term x
′
i
into the sum,
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}\left (\sum \limits _{j=1}^{i}({\mathbf{\mathit{x}}'}_{ i} \cdot {\mathbf{\mathit{L}}'}_{j})\dot{{q}}_{j}\right) \cdot \left (\sum \limits _{j=1}^{i}({\mathbf{\mathit{x}}'}_{ i} \cdot {\mathbf{\mathit{L}}'}_{j})\dot{{q}}_{j}\right ).\end{array}$$
(12.33)
Evaluating the sums for j from 1 to i gives
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\sum \limits _{i=1}^{n}{m}_{ i}\left ({\mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} + \cdots +{ \mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{i}\dot{{q}}_{i}\right) \\ & & \cdot \left ({\mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} + \cdots +{ \mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{i}\dot{{q}}_{i}\right ). \end{array}$$
(12.34)
Evaluating the sum for i from 1 to n gives
$$\begin{array}{rcl}{ K}_{v}& =& \frac{1} {2}\bigg{[}{m}_{1}({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1}) + \cdots \\ & & {m}_{2}({\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} +{ \mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\dot{{q}}_{2}) \cdot ({\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} +{ \mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\dot{{q}}_{2}) + \cdots \\ & & \ \ \ \ \ \ \vdots \\ & & {m}_{n}\left ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} + \cdots +{ \mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n}\dot{{q}}_{n}\right) \cdot \left ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}\dot{{q}}_{1} + \cdots +{ \mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n}\dot{{q}}_{n}\right )\bigg{]}.\end{array}$$
(12.35)
Reorganizing the terms and extracting \(\dot{q}\), we have
$$\begin{array}{rcl} 2{K}_{v}& =& \left ({m}_{1}({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}) + \cdots + {m}_{n}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1})\right )\dot{{q}}_{1}^{2} \\ & & +\left ({m}_{1}({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{2}) \cdot ({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{2}) + \cdots + {m}_{n}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2})\right )\dot{{q}}_{2}^{2} \\ & & \vdots\vdots\vdots \\ & & +\left ({m}_{1}({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{n}) \cdot ({\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{n}) + \cdots + {m}_{n}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n})\right )\dot{{q}}_{n}^{2} \\ & & +\left ({m}_{2}({\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}) + \cdots + {m}_{2}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2})\right )\dot{{q}}_{1}\dot{{q}}_{2} \\ & & +\left ({m}_{3}({\mathbf{\mathit{x}}'}_{3} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{3} \cdot {\mathbf{\mathit{L}}'}_{3}) + \cdots + {m}_{2}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{3})\right )\dot{{q}}_{1}\dot{{q}}_{3} \\ & & \vdots \\ & & +{m}_{n}({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n-1}) \cdot ({\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n})\dot{{q}}_{n-1}\dot{{q}}_{n}.\quad \quad \quad \quad \end{array}$$
(12.36)
Thanks to this decomposition, it is easy to see the symmetry of the terms; they could be organized in a matrix form to get a better compression of this equation:
$$\begin{array}{rcl}{ K}_{v} = \frac{1} {2}\left (\begin{array}{*{10}c} \dot{{q}}_{1} & \cdots &\dot{{q}}_{n} \end{array} \right ){\mathcal{M}}_{v}\left (\begin{array}{*{10}c} \dot{{q}}_{1}\\ \vdots \\ \dot{{q}}_{n}\end{array} \right ),& &\end{array}$$
(12.37)
where \({\mathcal{M}}_{v}\) is equal to
$$\begin{array}{rlrlrl} {\mathcal{M}}_{v} = \frac{1} {2}\left (\begin{array}{*{10}c} {\sum }_{j=1}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{1}) &\cdots & {\sum }_{j=n}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{1}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{n}) \\ {\sum }_{j=2}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{2}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{1}) &\cdots & {\sum }_{j=n}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{2}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{n})\\ \vdots & \ddots & \vdots \\ {\sum }_{j=n}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{n}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{1})&\cdots &{\sum }_{j=n}^{n}{m}_{ j}({\mathbf{\mathit{x}}'}_{j} \cdot \cdot {\mathbf{\mathit{L}}'}_{n}) \cdot ({\mathbf{\mathit{x}}'}_{j} \cdot {\mathbf{\mathit{L}}'}_{n}) \end{array} \right ), & &\end{array}$$
(12.38)
and each element \({\mathcal{M}}_{{v}_{ij}}\) of the matrix \({\mathcal{M}}_{v}\) is computed using
$$\begin{array}{rcl}{ \mathcal{M}}_{{v}_{ij}} =\sum \limits _{k\,=\,\textrm{ Max}(i,j)}^{n}{m}_{ k}({\mathbf{\mathit{x}}'}_{k} \cdot {\mathbf{\mathit{L}}'}_{i}) \cdot ({\mathbf{\mathit{x}}'}_{k} \cdot {\mathbf{\mathit{L}}'}_{j}).& &\end{array}$$
(12.39)
Note that the symmetric matrix \({\mathcal{M}}_{v}\) can be separated in the product of three matrices, two triangular and one diagonal:
$$\begin{array}{rcl}{ \mathcal{M}}_{v} = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1} \\ 0 \quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n} \end{array} \right )\left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {m}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ 0 \quad &{m}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{m}_{n} \end{array} \right )& & \\ \end{array}$$
$$\begin{array}{rcl} \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n} \end{array} \right ).& &\end{array}$$
(12.40)
Now we will define the matrix m and the matrix V as
$$\begin{array}{rcl} m := \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {m}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ 0 \quad &{m}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{m}_{n} \end{array} \right ),& &\end{array}$$
(12.41)
$$\begin{array}{rcl} V := \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n} \end{array} \right ).& &\end{array}$$
(12.42)
Then we can write Eq. 12.40 as follows:
$$\begin{array}{rcl}{ \mathcal{M}}_{v} = {V }^{T}mV.& &\end{array}$$
(12.43)
The elements of the matrix V are vectors and the elements of m are scalars, which means that the contribution of kinetic energy produced due to mass displacements with respect to the reference frame can be easily computed as
$$\begin{array}{rcl}{ K}_{v} = \frac{1} {2}\dot{{q}}^{T}{V }^{T}mV \dot{q}.& &\end{array}$$
(12.44)
Now we follow a similar procedure for the component of the kinetic energy K
i
:
$$\begin{array}{rcl}{ K}_{I} = \frac{1} {2}\sum \limits _{i=1}^{n}{I}_{ i}{\left (\sum \limits _{j=1}^{i}\dot{{q}}_{ j}\right )}^{2}.& &\end{array}$$
(12.45)
Evaluating the sums for i and j from 1 to n, we get
$$\begin{array}{rcl}{ K}_{I} = \frac{1} {2}\left [{I}_{1}{(\dot{{q}}_{1})}^{2} + {I}_{ 2}{(\dot{{q}}_{1} +\dot{ {q}}_{2})}^{2} + \cdots + {I}_{ n}{(\dot{{q}}_{1} + \cdots +\dot{ {q}}_{n})}^{2}\right ].& &\end{array}$$
(12.46)
Expanding the expression, extracting \(\dot{q}\), and writing in matrix form give us
$$\begin{array}{rcl}{ K}_{I} = \frac{1} {2}\left (\begin{array}{*{10}c} \dot{{q}}_{1} & \cdots &\dot{{q}}_{n} \end{array} \right ){\mathcal{M}}_{I}\left (\begin{array}{*{10}c} \dot{{q}}_{1}\\ \vdots \\ \dot{{q}}_{n}\end{array} \right ),& &\end{array}$$
(12.47)
where
$$\begin{array}{rcl}{ \mathcal{M}}_{I} = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} \sum \limits _{i=1}^{n}{I}_{i}\quad & \sum \limits _{i=2}^{n}{I}_{i}\quad &\cdots \quad &\sum \limits _{i=n}^{n}{I}_{i} \\ \sum \limits _{i=2}^{n}{I}_{i}\quad & \sum \limits _{i=2}^{n}{I}_{i}\quad &\cdots \quad &\sum \limits _{i=n}^{n}{I}_{i}\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ \sum \limits _{i=n}^{n}{I}_{i}\quad &\sum \limits _{i=n}^{n}{I}_{i}\quad &\cdots \quad &\sum \limits _{i=n}^{n}{I}_{i} \end{array} \right ).& &\end{array}$$
(12.48)
The matrix \({\mathcal{M}}_{I}\) can be written as the product of two matrices δ and I if we define them as
$$\begin{array}{rcl}{ M}_{I} = \delta I = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} 1\quad &1\quad &\cdots \quad &1\\ 0\quad &1\quad &\cdots \quad &1\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0\quad &0\quad &\cdots \quad &1 \end{array} \right )\left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {I}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {I}_{2}\quad &{I}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {I}_{n}\quad &{I}_{n}\quad &\cdots \quad &{I}_{n} \end{array} \right ).& &\end{array}$$
(12.49)
In such a way, the component of kinetic energy due to the movement of links around their mass center is given by
$$\begin{array}{rcl}{ K}_{I} = \frac{1} {2}\dot{{q}}^{T}\delta I\dot{q}.& &\end{array}$$
(12.50)
In conclusion, we have an expression to compute the total kinetic energy of the serial robot using the axes of the robot and the mass centers in conformal geometric algebra:
$$\begin{array}{rcl} K = \frac{1} {2}\dot{{q}}^{T}({V }^{T}mV + \delta I)\dot{q}.& &\end{array}$$
(12.51)
Note that this expression allows us to compute the kinetic energy without the derivatives.
3.2 Potential Energy
In contrast to kinetic energy, the potential energy does not depend on the velocity, but it depends on the position of each link of the serial robot. Thanks to the equations of direct kinematics (12.1), we can compute the position x
′
i
of each link. In order to know the potential energy U
i
, we compute the dot product of these points and the force applied to each point:
$$\begin{array}{rcl}{ U}_{i} ={ \mathbf{\mathit{x}}'}_{i} \cdot {F}_{i}.& &\end{array}$$
(12.52)
Here the potential energy is due to conservative forces such as the gravity forces, then F
i
= m
i
ge
2. Also the total potential energy of the system is equal to the sum of all U
i
:
$$\begin{array}{rcl} U = \sum \limits _{i=1}^{n}{\mathbf{\mathit{x}}'}_{ i} \cdot {F}_{i}.& &\end{array}$$
(12.53)
3.3 Lagrange’s Equations
The dynamic equations of a robot can be computed based on Newton’s equations, but the formulation becomes complicated when the number of degrees of freedom increases. For this reason, we will use Lagrange’s equations of movement.
The Lagrangian £ is defined as the difference between the kinetic energy and potential energy of the system:
$$\begin{array}{rcl} \mathit{\pounds } = K - U.& &\end{array}$$
(12.54)
Lagrange’s equations of movement are given by
$$\begin{array}{rcl} \frac{\mbox{ d}} {\mbox{ d}t}\left [\frac{\partial \mathit{\pounds }} {\partial \dot{q}} \right ] -\frac{\partial \mathit{\pounds }} {\partial q} = \tau.& &\end{array}$$
(12.55)
We first compute the partial derivative of £ with respect to \(\dot{q}\):
$$\begin{array}{rcl} \frac{\partial \mathit{\pounds }} {\partial \dot{q}} = \frac{\partial K} {\partial \dot{q}} -\frac{\partial U} {\partial \dot{q}} = \frac{\partial K} {\partial \dot{q}}.& &\end{array}$$
(12.56)
Note that the partial derivative of U with respect to \(\dot{q}\) is always zero since U does not depend on the velocities of the joints \(\dot{q}\). Replacing K in Eqs. 12.56 and 12.51 gives
$$\begin{array}{rcl} \frac{\partial \mathit{\pounds }} {\partial \dot{q}} = \frac{\partial } {\partial \dot{q}}\left (\frac{1} {2}\dot{{q}}^{T}({V }^{T}mV + \delta I)\dot{q}\right) = ({V }^{T}mV + \delta I)\dot{q}.& &\end{array}$$
(12.57)
In order to simplify the notation, the matrix \(\mathcal{M}\) is defined as \(\mathcal{M} = {\mathcal{M}}_{v} + {\mathcal{M}}_{I} = {V }^{T}mV + \delta I\) and Eq. 12.57 is now written as
$$\begin{array}{rcl} \frac{\partial \mathit{\pounds }} {\partial \dot{q}} = \mathcal{M}\dot{q}.& &\end{array}$$
(12.58)
On the other hand, the partial derivative of £ with respect to q is given by
$$\begin{array}{rcl} \frac{\partial \mathit{\pounds }} {\partial q} = \frac{\partial K} {\partial q} -\frac{\partial U} {\partial q} = \frac{1} {2}\dot{{q}}^{T}\left (\frac{\partial \mathcal{M}} {\partial q} \right )\dot{q} -\frac{\partial U} {\partial q}.& &\end{array}$$
(12.59)
Replacing Eqs. 12.58 and 12.59 in the Lagrange equation (12.55), we get
$$\begin{array}{rcl} \frac{\mbox{ d}} {\mbox{ d}t}\left [\mathcal{M}\dot{q}\right ] -\left [\frac{1} {2}\dot{{q}}^{T}\left (\frac{\partial \mathcal{M}} {\partial q} \right )\dot{q} -\frac{\partial U} {\partial q} \right ] = \tau.& &\end{array}$$
(12.60)
The time derivative of Eq. 12.60 gives
$$\begin{array}{rcl} \mathcal{M}\ddot{q} +\dot{ \mathcal{M}}\dot{q} -\frac{1} {2}\dot{{q}}^{T}\left (\frac{\partial \mathcal{M}} {\partial q} \right )\dot{q} + \frac{\partial U} {\partial q} = \tau.& &\end{array}$$
(12.61)
In order to simplify the expression, we rename parts of the equation as follows:
$$\begin{array}{rcl} \text{ C} =\dot{ \mathcal{M}}-\frac{1} {2}\dot{{q}}^{T}\left (\frac{\partial \mathcal{M}} {\partial q} \right ),& &\end{array}$$
(12.62)
$$G = \frac{\partial U} {\partial q},$$
(12.63)
where C is the Coriolis and centrifugal matrix and G is the vector of gravitational components. Therefore, we can write the dynamic equation for a serial robot with n degrees of freedom:
$$\begin{array}{rcl} \mathcal{M}\ddot{q} + C\dot{q} + G = \tau.& &\end{array}$$
(12.64)
Now we analyze the G matrix, looking for an equation that allows us to get it without partial derivatives. Using Eq. 12.53, we write
$$\begin{array}{rcl} G = \frac{\partial U} {\partial q} = \frac{\partial } {\partial q}\left (\sum \limits _{i=1}^{n}{F}_{ i} \cdot {\mathbf{\mathit{x}}'}_{i}\right ).& &\end{array}$$
(12.65)
Since the forces F
i
= m
i
ge
2 are produced by gravity, they do not depend on the joints positions q:
$$\begin{array}{rcl} G =\sum \limits _{i=1}^{n}{F}_{ i} \cdot \left (\frac{\partial } {\partial q}{\mathbf{\mathit{x}}'}_{i}\right ).& &\end{array}$$
(12.66)
Recalling the equation of differential kinematics (12.15), we know that
$$\begin{array}{rcl} \frac{\partial } {\partial q}{\mathbf{\mathit{x}}'}_{i} = \left (\begin{array}{*{10}c} {\mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{1} \\ {\mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{2}\\ \vdots \\ {\mathbf{\mathit{x}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{i} \end{array} \right ).& &\end{array}$$
(12.67)
Evaluating the sum of Eq. 12.66 from i = 1 to n and introducing in each term the evaluation of Eq. 12.67 give us
$$\begin{array}{rcl} G = \left (\begin{array}{*{10}c} {\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1} \\ 0\\ \vdots\\ 0 \end{array} \right ){F}_{1} + \left (\begin{array}{*{10}c} {\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1} \\ {\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\\ \vdots \\ 0 \end{array} \right ){F}_{2} + \cdots + \left (\begin{array}{*{10}c} {\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1} \\ {\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}\\ \vdots \\ {\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n} \end{array} \right ){F}_{n}.& &\end{array}$$
(12.68)
Equation 12.68 is written as a matrix:
$$\begin{array}{rcl} G = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{x}}'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{1} \\ 0 \quad &{\mathbf{\mathit{x}}'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{2}\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \cdot {\mathbf{\mathit{L}}'}_{n} \end{array} \right )\left (\begin{array}{*{10}c} {F}_{1} \\ {F}_{2}\\ \vdots \\ {F}_{n} \end{array} \right ).& &\end{array}$$
(12.69)
As one can see, this matrix is basically the transpose of the matrix V given in Eq. 12.42. Calling F the vector with components F
i
, we can finally write the equation:
$$\begin{array}{rcl} G = {V }^{T}F.& &\end{array}$$
(12.70)
Furthermore, F is given by the product of two matrices
$$\begin{array}{rcl} F = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {m}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ 0 \quad &{m}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{m}_{n} \end{array} \right )\left (\begin{array}{*{10}c} g{e}_{2} \\ g{e}_{2}\\ \vdots \\ g{e}_{2} \end{array} \right) = ma,& &\end{array}$$
(12.71)
where a is a vector of accelerations. Equation 12.71 allows us to separate the constant matrices and the variables of the serial robot:
$$\begin{array}{rcl} G = {V }^{T}ma.& &\end{array}$$
(12.72)
Finally, we have a short and useful equation to compute the vector G using the information of the joints’ axes. Now, we analyze the Coriolis matrix C. In fact, there are many ways to compute this matrix, and there are many matrices C that satisfy the dynamic equation (12.64). Although we already have an equation to compute the matrix C (12.62), we will look for a simpler equation to avoid the necessity of derivatives. Based on the properties of the matrices \(\mathcal{M}\) and C, we know that
$$\begin{array}{rcl} \dot{\mathcal{M}} = C + {C}^{T}.& &\end{array}$$
(12.73)
On the other hand, we know
$$\begin{array}{rcl} \mathcal{M} = {V }^{T}mV + \delta I.& &\end{array}$$
(12.74)
Computing the time derivative of \(\mathcal{M}\) produces
$$\begin{array}{rcl} \dot{\mathcal{M}}& =& \frac{\mathrm{d}} {\mathrm{d}t}M = \frac{\mathrm{d}} {\mathrm{d}t}\left ({V }^{T}mV + \delta I\right ),\end{array}$$
(12.75)
$$\begin{array}{rcl} \dot{\mathcal{M}}& =& \frac{\mathrm{d}} {\mathrm{d}t}{V }^{T}mV,\end{array}$$
(12.76)
$$\begin{array}{rcl} \dot{\mathcal{M}}& =& {V }^{T}m\dot{V } +\dot{ {V }}^{T}mV,\end{array}$$
(12.77)
$$\begin{array}{rcl} \dot{\mathcal{M}}& =& {V }^{T}m\dot{V } + {({V }^{T}m\dot{V })}^{T}.\end{array}$$
(12.78)
Taking into account Eqs. 12.73 and 12.78, we have a short and clear equation to compute the matrix C without derivatives:
$$\begin{array}{rcl} C = {V }^{T}m\dot{V }.& &\end{array}$$
(12.79)
The last sentence is true since we can compute the matrix \(\dot{V }\) without derivatives just as a function of the joints’ values q, \(\dot{q}\), and the axes of the robot. In order to compute the element \(\dot{{V }}_{ij}\), the time derivative of x
′
i
⋅L
′
j
is needed, using Eq. 12.27
$$\begin{array}{rcl} \dot{{V }}_{ij}& =& \frac{\mathrm{d}} {\mathrm{d}t}({\mathbf{\mathit{x}}'}_{i}{\mathbf{\mathit{L}}'}_{j}) =\dot{{ \mathbf{\mathit{x}}}'}_{i} \cdot {\mathbf{\mathit{L}}'}_{j} +{ \mathbf{\mathit{x}}'}_{i} \cdot \dot{ {L}'}_{j},\end{array}$$
(12.80)
$$\begin{array}{rcl} \dot{{V }}_{ij}& =& \sum \limits _{k=1}^{i}({\mathbf{\mathit{x}}'}_{ i} \cdot {\mathbf{\mathit{L}}'}_{k}) \cdot {\mathbf{\mathit{L}}'}_{j}\dot{{q}}_{k} + \frac{1} {2}\sum \limits _{k=1}^{j-1}{\mathbf{\mathit{x}}'}_{ i} \cdot ({\mathbf{\mathit{L}}'}_{j}{\mathbf{\mathit{L}}'}_{k} -{\mathbf{\mathit{L}}'}_{k}{\mathbf{\mathit{L}}'}_{j})\dot{{q}}_{k}.\end{array}$$
(12.81)
Note that V
ij
= 0 whenever j > i because \(\dot{{V }}_{ij} = 0\). Perhaps these equations to get \(\dot{V }\) can be confused. We will rewrite these equations as matrices to give a clearer explanation of the method to compute \(\dot{V }\). It is possible to write the matrix V as the product of two matrices:
$$\begin{array}{rcl} V = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{x}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ 0 \quad &{\mathbf{\mathit{x}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &{\mathbf{\mathit{x}}'}_{n} \end{array} \right )\left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{L}}'}_{n} \end{array} \right) = XL,& &\end{array}$$
(12.82)
then
$$\begin{array}{rcl} \dot{V } =\dot{ X}L + X\dot{L},& &\end{array}$$
(12.83)
with
$$\begin{array}{rcl} \dot{X} = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} \dot{{\mathbf{\mathit{x}}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ 0 \quad &\dot{{\mathbf{\mathit{x}}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ 0 \quad & 0 \quad &\cdots \quad &\dot{{\mathbf{\mathit{x}}}'}_{n} \end{array} \right ).& &\end{array}$$
(12.84)
Computing \(\dot{{\mathbf{\mathit{x}}}'}_{i}\) is simple using the equation of differential kinematics (12.15):
$$\begin{array}{rcl} \left (\begin{array}{*{10}c} \dot{{\mathbf{\mathit{x}}}'}_{1}\\ \vdots \\ \dot{{\mathbf{\mathit{x}}}'}_{n} \end{array} \right) = XL\dot{q} = V \dot{q}.& &\end{array}$$
(12.85)
On the other hand,
$$\begin{array}{rcl} \dot{L}' = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} \dot{{L}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ \dot{{L}'}_{1}\quad &\dot{{L}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ \dot{{L}'}_{1}\quad &\dot{{L}'}_{2}\quad &\cdots \quad &\dot{{L}'}_{n} \end{array} \right ).& &\end{array}$$
(12.86)
To compute \(\dot{{L}'}_{i}\), which represents the velocity of the axis i produced by the rotation around the previous axes, we can do the following:
$$\begin{array}{rcl} \left (\begin{array}{*{10}c} \dot{{L}'}_{1} \\ \dot{{L}'}_{1}\\ \vdots \\ \dot{{L}'}_{n} \end{array} \right )& =& \frac{1} {2}\left [\left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{L}}'}_{1}{\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {\mathbf{\mathit{L}}'}_{2}{\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{L}}'}_{2}{\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {\mathbf{\mathit{L}}'}_{n}{\mathbf{\mathit{L}}'}_{1}\quad &{\mathbf{\mathit{L}}'}_{n}{\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad &{\mathbf{\mathit{L}}'}_{n}{\mathbf{\mathit{L}}'}_{n} \end{array} \right )\right. \\ & & \left.-\left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {\mathbf{\mathit{L}}'}_{1}{\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad &\cdots \quad & 0 \\ {\mathbf{\mathit{L}}'}_{1}{\mathbf{\mathit{L}}'}_{2}\quad &{\mathbf{\mathit{L}}'}_{2}{\mathbf{\mathit{L}}'}_{2}\quad &\cdots \quad & 0\\ \vdots \quad & \vdots \quad & \ddots \quad & \vdots \\ {\mathbf{\mathit{L}}'}_{1}{\mathbf{\mathit{L}}'}_{n}\quad &{\mathbf{\mathit{L}}'}_{2}{\mathbf{\mathit{L}}'}_{n}\quad &\cdots \quad &{\mathbf{\mathit{L}}'}_{n}{\mathbf{\mathit{L}}'}_{n} \end{array} \right )\right ]\dot{q}.\end{array}$$
(12.87)
In conclusion, using Eqs. 12.57 and 12.79, we have rewritten the dynamic equation of a serial robot with n degrees of freedom:
$$\begin{array}{rcl} ({V }^{T}mV + \delta I)\ddot{q} + {V }^{T}m\dot{V }\dot{q} + {V }^{T}F& =& \tau,\end{array}$$
(12.88)
$$\begin{array}{rcl} \delta I\ddot{q} + {V }^{T}(mV \ddot{q} + m\dot{V }\dot{q} + F)& =& \tau.\end{array}$$
(12.89)
This decomposition allows us to see the components of inertia momentum, and centrifugal and gravity forces. Replacing F with ma and extracting m gives
$$\begin{array}{rcl} \delta I\ddot{q} + {V }^{T}m(V \ddot{q} +\dot{ V }\dot{q} + a) = \tau.& &\end{array}$$
(12.90)
Finally Eq. 12.90 is the dynamic equation of an n-degrees-of-freedom serial robot where the elements of the matrices are multivectors of the geometric algebra G
4, 1, 0. δ and a are constant and known matrices while m and I depend on the robot parameters but are also constants. Only the matrix V (Eq. 12.82) and, therefore, \(\dot{V }\) (Eq. 12.83) changes over time.
As an example, we will compare the classical and the proposed approach. We will compute the dynamics of a 2-DOF robot arm; see Fig. 12.3. First, we compute the matrices V and \(\dot{V }\). These matrices will be used to compute \(\mathcal{M}\), C, and G:
$$\begin{array}{rcl} V = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c} {C'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad \\ {C'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad \end{array} \right ),& &\end{array}$$
(12.91)
$$\begin{array}{rcl} \dot{{V }}_{1,1}& =& ({C}_{1} \cdot {\mathbf{\mathit{L}}}_{1}) \cdot {\mathbf{\mathit{L}}}_{1}\dot{{q}}_{1} + ({C}_{1} \cdot {\mathbf{\mathit{L}}}_{2}) \cdot {\mathbf{\mathit{L}}}_{1}\dot{{q}}_{2}, \\ \dot{{V }}_{2,1}& =& ({C}_{2} \cdot {\mathbf{\mathit{L}}}_{1}) \cdot {\mathbf{\mathit{L}}}_{1}\dot{{q}}_{1} + ({C}_{2} \cdot {\mathbf{\mathit{L}}}_{2}) \cdot {\mathbf{\mathit{L}}}_{1}\dot{{q}}_{2}, \\ \dot{{V }}_{2,2}& =& ({C}_{2} \cdot {\mathbf{\mathit{L}}}_{1}) \cdot {\mathbf{\mathit{L}}}_{2}\dot{{q}}_{1} + ({C}_{2} \cdot {\mathbf{\mathit{L}}}_{2}) \cdot {\mathbf{\mathit{L}}}_{2}\dot{{q}}_{2} + \frac{1} {2}({C}_{2} \cdot ({\mathbf{\mathit{L}}}_{2}{\mathbf{\mathit{L}}}_{1} -{\mathbf{\mathit{L}}}_{1}{\mathbf{\mathit{L}}}_{2}))\dot{{q}}_{1}\end{array}$$
The matrix \(\mathcal{M}\) is computed as
$$\begin{array}{rcl}{ \mathcal{M}}_{1,1}& =& {m}_{1}{l}_{c1}{l}_{c1} + {m}_{2}{l}_{1}{l}_{1} + {m}_{2}{l}_{c2}{l}_{c2} + 2{m}_{2}{l}_{1}{l}_{c2}\cos ({q}_{2}) + {I}_{1} + {I}_{2}, \\ {\mathcal{M}}_{1,2}& =& {m}_{2}{l}_{c2}{l}_{c2} + {m}_{2}{l}_{1}{l}_{c2}\cos ({q}_{2}) + {I}_{2}, \\ {\mathcal{M}}_{2,2}& =& {m}_{2}{l}_{c2}{l}_{c2} + {I}_{2}\end{array}$$
And using conformal geometric algebra gives
$$\begin{array}{rcl}{ \mathcal{M}}_{1,1}& =& {m}_{1}({V }_{1,1} \cdot {V }_{1,1}) + {m}_{2}({V }_{2,1} \cdot {V }_{2,1}), \\ {\mathcal{M}}_{1,2}& =& {m}_{2}({V }_{2,1} \cdot {V }_{2,2}), \\ {\mathcal{M}}_{2,2}& =& {m}_{2}({V }_{2,2} \cdot {V }_{2,2})\end{array}$$
The element M
2, 1 = M
1, 2 and M
I
is computed as
$$\begin{array}{rcl}{ M}_{I} = \left (\begin{array}{c@{\quad }c} 1\quad &1\\ 0\quad &1\end{array} \right )\left (\begin{array}{c@{\quad }c} {I}_{1}\quad & 0 \\ {I}_{2}\quad &{I}_{2} \end{array} \right) = \left (\begin{array}{c@{\quad }c} {I}_{1} + {I}_{2}\quad &{I}_{2} \\ {I}_{2} \quad &{I}_{2} \end{array} \right ).& & \\ \end{array}$$
Similarly, the matrix G is computed using the equations
$$\begin{array}{rcl}{ G}_{1}& =& ({m}_{1}{l}_{c1} + {m}_{2}{l}_{1})g\sin ({q}_{1}) + {m}_{2}g{l}_{c2}\sin ({q}_{1} + {q}_{2}), \\ {G}_{2}& =& {m}_{2}g{l}_{c2}\sin ({q}_{1} + {q}_{2})\end{array}$$
In conformal geometry, they are given by
$$\begin{array}{rcl}{ G}_{1}& =& {m}_{1}({V }_{1,1} \cdot g{e}_{2}) + {m}_{2}({V }_{2,1} \cdot g{e}_{2}), \\ {G}_{2}& =& {m}_{2}({V }_{2,2} \cdot g{e}_{2})\end{array}$$
Finally, the Coriolis matrix is given by
$$\begin{array}{rcl}{ C}_{1,1}& =& -{m}_{2}{l}_{1}{l}_{c2}\sin ({q}_{2})\dot{{q}}_{2}, \\ {C}_{2,1}& =& {m}_{2}{l}_{1}{l}_{c2}\sin ({q}_{2})(\dot{{q}}_{1}), \\ {C}_{1,2}& =& -{m}_{2}{l}_{1}{l}_{c2}\sin ({q}_{2})(\dot{{q}}_{1} +\dot{ {q}}_{2}), \\ \end{array}$$
and in geometric algebra as
$$\begin{array}{rcl}{ C}_{1,1}& =& {m}_{1}({V }_{1,1} \cdot \dot{ {V }}_{1,1}) + {m}_{2}({V }_{1,2} \cdot \dot{ {V }}_{2,1}), \\ {C}_{2,1}& =& {m}_{2}({V }_{2,2} \cdot \dot{ {V }}_{2,1}), \\ {C}_{1,2}& =& {m}_{2}({V }_{1,2} \cdot \dot{ {V }}_{2,2}), \\ \end{array}$$
where C
2, 2 = 0. This is a simple example, but when the number of degrees of freedom increases, in contrast to the classical approach, the new one preserves its clarity.
In this example, we show the form of the matrices for a 6-DOF serial robot like a robot arm Adept Six 600 (see Fig. 12.4). Matrix V
$$\begin{array}{rcl} V = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c@{\quad }c@{\quad }c} {C'}_{1} \cdot {\mathbf{\mathit{L}}'}_{1}\quad & 0 \quad & 0 \quad & 0 \quad & 0 \quad & 0 \\ {C'}_{2} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{2} \cdot {\mathbf{\mathit{L}}'}_{2}\quad & 0 \quad & 0 \quad & 0 \quad & 0 \\ {C'}_{3} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{3} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &{C'}_{3} \cdot {\mathbf{\mathit{L}}'}_{3}\quad & 0 \quad & 0 \quad & 0 \\ {C'}_{4} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{4} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &{C'}_{4} \cdot {\mathbf{\mathit{L}}'}_{3}\quad &{C'}_{4} \cdot {\mathbf{\mathit{L}}'}_{4}\quad & 0 \quad & 0 \\ {C'}_{5} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{5} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &{C'}_{5} \cdot {\mathbf{\mathit{L}}'}_{3}\quad &{C'}_{5} \cdot {\mathbf{\mathit{L}}'}_{4}\quad &{C'}_{5} \cdot {\mathbf{\mathit{L}}'}_{5}\quad & 0 \\ {C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{1}\quad &{C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{2}\quad &{C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{3}\quad &{C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{4}\quad &{C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{5}\quad &{C'}_{6} \cdot {\mathbf{\mathit{L}}'}_{6} \end{array} \right )& &\end{array}$$
(12.92)
Matrix m
$$\begin{array}{rcl} m = \left (\begin{array}{c@{\quad }c@{\quad }c@{\quad }c@{\quad }c@{\quad }c} {m}_{1}\quad & 0 \quad & 0 \quad & 0 \quad & 0 \quad & 0 \\ 0 \quad &{m}_{2}\quad & 0 \quad & 0 \quad & 0 \quad & 0 \\ 0 \quad & 0 \quad &{m}_{3}\quad & 0 \quad & 0 \quad & 0 \\ 0 \quad & 0 \quad & 0 \quad &{m}_{4}\quad & 0 \quad & 0 \\ 0 \quad & 0 \quad & 0 \quad & 0 \quad &{m}_{5}\quad & 0 \\ 0 \quad & 0 \quad & 0 \quad & 0 \quad & 0 \quad &{m}_{6} \end{array} \right )& &\end{array}$$
(12.93)
Matrix a
$$\begin{array}{rcl} a = \left (\begin{array}{*{10}c} g{e}_{2} \\ g{e}_{2} \\ g{e}_{2} \\ g{e}_{2} \\ g{e}_{2} \\ g{e}_{2} \end{array} \right )& &\end{array}$$
(12.94)
