Singularities of Serial Robots: Identification and Distance Computation Using Geometric Algebra

Abstract

The singularities of serial robotic manipulators are those configurations in which the robot loses the ability to move in at least one direction. Hence, their identification is fundamental to enhance the performance of current control and motion planning strategies. While classical approaches entail the computation of the determinant of either a $6\times n$ or $n\times n$ matrix for an n-degrees-of-freedom serial robot, this work addresses a novel singularity identification method based on modelling the twists defined by the joint axes of the robot as vectors of the six-dimensional and three-dimensional geometric algebras. In particular, it consists of identifying which configurations cause the exterior product of these twists to vanish. In addition, since rotors represent rotations in geometric algebra, once these singularities have been identified, a distance function is defined in the configuration space $\mathcal{C}$, such that its restriction to the set of singular configurations $\mathcal{S}$ allows us to compute the distance of any configuration to a given singularity. This distance function is used to enhance how the singularities are handled in three different scenarios, namely, motion planning, motion control and bilateral teleoperation.

1. Introduction

A serial robot manipulator is an open kinematic chain made up of a sequence of rigid bodies, called links, connected by means of actuated kinematic pairs, called joints, that provide relative motion between consecutive links. At the end of the last link, there is a tool or device known as the end-effector. Only two types of joints are considered throughout this work: revolute joints, which only perform rotations, and prismatic joints, which only perform translations. If joint i is revolute (prismatic), the amount it rotates (translates) is encoded by an angle ${\theta }_i$ (a displacement $d_i$). These scalars are known as the joint variables of the robot.

From a kinematic point of view, the end-effector position and orientation (also known as the pose) can be expressed as a differentiable function $f:\mathcal{C}\to X$, where $\mathcal{C}$ denotes the space of joint variables, called the configuration space of the robot, and X is the space of all positions and orientations of the end-effector with respect to a reference frame, which is usually called the operational space. A serial robot is said to have n degrees of freedom (DoF) if its configuration can be minimally specified by n variables. For a serial robot, the number and nature of the joints determine the number of the DoF. For the task of positioning and orientating its end-effector in the three-dimensional space, the manipulators with more than six DoF are called redundant while the rest are non-redundant.

In order to describe its relative position and orientation, a frame $\{\mathit{o},\mathit{x},\mathit{y},\mathit{z}\}$ is attached to each joint (Figure 1). The relations between consecutive joint frames are conventionally described by homogeneous transformation matrices. In particular, ${}^{i-1}{T}_i$ relates frame $\left\{i\right\}$ to frame $\{i-1\}$ (the first joint frame is related to a fixed reference frame, known as the world frame). Therefore, the end-effector pose ${}^0{T}_n$ of a robot with n DoF with respect to the world frame can be represented as:

$${}^0{T}_n={}^0{T}_1 {}^1{T}_2 \dots {}^{n-1}{T}_n$$

(1)

with

$${}^0{T}_n=\left(\begin{array}{cc}R& \mathit{p}\\ 0& 1\end{array}\right),$$

(2)

where R is a rotation matrix that describes the end-effector orientation with respect to the world frame, while $\mathit{p}$ is a position vector describing the end-effector position with respect to the world frame. This description is equivalent to the one provided by f, known as the kinematic function of the serial robot. Thus, $f\left(\mathit{q}\right)=\mathit{x}$, where $\mathit{x}$ denotes the vector describing the end-effector pose and $\mathit{q}=(q_1,\dots ,q_n)$ denotes the vector whose components are the joint variables, also known as the configuration of the robot. Clearly, either $q_i={\theta }_i$ if joint i is revolute or $q_i=d_i$ if joint i is prismatic.

Deriving the kinematic relation defined by f with respect to time, we obtain another relation:

$$\dot{\mathit{x}}=J\left(\mathit{q}\right)\dot{\mathit{q}},$$

(3)

where $\dot{\mathit{x}}$ denotes the end-effector velocity vector, $\dot{\mathit{q}}$; the vector of the joint velocities; and J, the Jacobian matrix of f. If $J=[J_1 \dots J_n]$, then each column $J_i$ can also be computed as:

$$\begin{array}{cc}{J}_i=\left[\begin{array}{c}{\mathit{z}}_i\times ({\mathit{o}}_n-{\mathit{o}}_i)\\ {\mathit{z}}_i\end{array}\right]\hfill & \mathrm{if} \mathrm{joint} i \mathrm{is} \mathrm{revolute},\hfill \\ J_i=\left[\begin{array}{c}{\mathit{z}}_i\\ 0\end{array}\right]\hfill & \mathrm{if} \mathrm{joint} i \mathrm{is} \mathrm{prismatic}.\hfill \end{array}$$

(4)

Definition 1.

Given a serial robot with n DoF, a singularity or kinematic singularity is a configuration $\mathit{q}\in \mathcal{C}$ satisfying $\rho \left(J\right(\mathit{q}\left)\right)<min\{n,6\}$, where $\rho (·)$ denotes the rank of the matrix argument. The set of all singular configurations is a subset of $\mathcal{C}$ that is usually denoted by $\mathcal{S}$ and known as the singular set.

Using the relation (3), it is easy to see that if $\mathit{q}\in \mathcal{C}$ is a singularity of a given serial robot, then the following two statements hold:

• The robot loses at least one degree of freedom or, equivalently, its end-effector cannot be translated along or rotated around at least one Cartesian direction.
• Finite linear and angular velocities of the end-effector may require infinite joint velocities.

In addition, Gottlieb [1] and Hollerbach [2] have independently proven that any serial manipulator with $n>2$ DoF has singularities. The identification of such singularities is achieved by solving the following non-linear equation:

$$det\left(J\right(\mathit{q}\left)\right)=0$$

(5)

if the robot is non-redundant and

$$det\left(J\left(\mathit{q}\right)J^T\left(\mathit{q}\right)\right)=0$$

(6)

if it is redundant.

In general, if the serial robot possesses at least one revolute joint, several coefficients of the Jacobian matrix are non-linear expressions, and thus, neither Equation (5) nor Equation (6) are easy to formulate and solve. However, for manipulators with a spherical wrist, a simplification can be made. For these robots, the axes of their last three joints intersect at a common point, known as the wrist center point, or are parallel (the intersection point and, hence, the wrist center point, is the point at the infinity). Since the origin of the frame attached to the end-effector can be placed at the wrist center point, a zero block appears in $J\left(\mathit{q}\right)$ by definition (see Equation (4)). Hence:

$$J\left(\mathit{q}\right)=\left[\begin{array}{cc}{J}_{11}\left(\mathit{q}\right)& 0\\ J_{21}\left(\mathit{q}\right)& J_{22}\left(\mathit{q}\right)\end{array}\right],$$

(7)

where $J_{11}\left(\mathit{q}\right),J_{21}\left(\mathit{q}\right)$ are blocks of order $3\times (n-3)$, $J_{22}\left(\mathit{q}\right)$ is a block of order $3\times 3$ and 0 denotes a block of order $3\times 3$, whose entries are all zero. Now, Equation (5) is simplified to:

$$det\left(J\left(\mathit{q}\right)\right)=det\left(J_{11}\left(\mathit{q}\right)\right)det\left(J_{22}\left(\mathit{q}\right)\right),$$

(8)

from which the singularities can be obtained as solutions of either $det\left(J_{11}\left(\mathit{q}\right)\right)=0$ or $det\left(J_{22}\left(\mathit{q}\right)\right)=0$. These two equations allow us to decouple the singularities into position and orientation singularities as follows:

• Position singularities $PS=\{\mathit{q}\in \mathcal{C} : det\left(J_{11}\left(\mathit{q}\right)\right)=0\}$;
• Orientation singularities $OS=\{\mathit{q}\in \mathcal{C} : det\left(J_{22}\left(\mathit{q}\right)\right)=0\}$.

Similarly, we can make the same decoupling for redundant robots:

• Position singularities $PS=\{\mathit{q}\in \mathcal{C} : \mathrm{rank}\left(J_{11}\left(\mathit{q}\right)\right)<3\}$;
• Orientation singularities $OS=\{\mathit{q}\in \mathcal{C} : det\left(J_{22}\left(\mathit{q}\right)\right)=0\}$.

Remark 1.

The Jacobian matrix $J\left(\mathit{q}\right)$ is represented with respect to the world frame (usually located at the base of the robot). However, sometimes it is useful to represent $J\left(\mathit{q}\right)$ in a different frame $\mathcal{B}$. To do so, the following identity is used:

$${J\left(\mathit{q}\right)}^{\mathcal{B}}=BJ\left(\mathit{q}\right),$$

(9)

where:

$$B=\left[\begin{array}{cc}{R}_0^{\mathcal{B}}& 0\\ 0& R_0^{\mathcal{B}}\end{array}\right],$$

(10)

with $R_0^{\mathcal{B}}={\left(R_{\mathcal{B}}^0\right)}^T$ and where $R_{\mathcal{B}}^0$ denotes the rotation matrix that relates the orientation of $\mathcal{B}$ with respect to the orientation of the world frame.

Singularity identification is one of the fundamental research fields in robot kinematics since, as stated before, it affects the motion of the robot and its performance when executing different tasks. Therefore, such identification is fundamental to enhancing the performance of current control and motion planning strategies by designing approaches to handle them. For instance, current applications of the subject include the handling of singularities for a robust control architecture in human–robot collaboration [3], the planning of singularity-free trajectories [4,5] and the smooth trajectory generation for rotating extensible manipulators or painting robots [6,7]. However, such identification is still problematic. The majority of the current approaches are based on the computation of either $det\left(J\left(\mathit{q}\right)\right)=0$ or $det\left(J\left(\mathit{q}\right)J^T\left(\mathit{q}\right)\right)=0$ or on the manipulation of the singular values of $J\left(\mathit{q}\right)$ [8,9,10]; hence, they are not computationally efficient. In addition, there is no efficient way of computing how close an arbitrary configuration is to a given singularity (which is fundamental for defining a threshold from where the strategies to handle them start to work). In this context, geometric algebra turns out to be very useful. In addition, it is currently applied to several problems in robot kinematics and geometry [11,12,13,14,15].

There is not much literature regarding the identification of singularities using geometric algebra, and the majority of the contributions focus on parallel mechanisms. For serial robots, Corrochano & Sobczyk [16] extend the Lie bracket of two vectors defined in any Lie algebra to what they call the superbracket of the lines ${\ell }_1,\dots ,{\ell }_6$, $[{\ell }_1,\dots ,{\ell }_6]$, where the line ${\ell }_i$ denotes the axis of joint i. For serial robots with six DoF, the main idea is to split the superbracket into smaller superbrackets, called bracket monomials, that are equated to zero. The singularities are the solutions of these monomial bracket equations. Following the same idea, Kanaan et al. [17] define the superbracket in a Grassmann–Caley algebra. Since the Lie bracket is well defined in every Grassmann–Caley algebra, the superbracket is also well defined. However, splitting the superbracket into the bracket monomials in this context is not intuitive and, as a consequence, is not always realizable. In addition, there is no standard procedure for computing these brackets’ monomials.

For parallel mechanisms, the majority of works [18,19,20,21,22] focus on approaches developed for some particular parallel robots. The main idea consists of computing, for each leg of the mechanism, the exterior product of the twists defined by its joints and equating it to zero. For those legs with less than six actuated joints, combinations of two, three or more legs are considered. The main problem with these approaches is their lack of generalisability. Each approach is designed for the specific parallel robot the authors work with.

Huo et al. [23] present a mobility analysis applying conformal geometric algebra and a singularity analysis using an idea similar to the ones presented in the above-mentioned contributions. A mobility analysis of overconstrained parallel mechanisms is performed using Grassmann–Cayley algebra by Chai et al. [24], while Yang and Li [25] propose a novel identification method for the constraint singularities of parallel robots based on differential manifolds. Finally, Kim et al. [26] apply conformal geometric algebra to the identification of the singularities of a particular type of parallel manipulator, the SPS-parallel manipulator. Several lines and planes are defined using the different joint axes. Then, the relative positions of different combinations of these geometric entities are studied to geometrically find the singularities. However, this method cannot be extended to other classes of parallel or serial robots, nor can it be implemented as an algorithm due to its complex geometrical nature.

In this paper, a novel approach for singularity identification based on the six-dimensional and three-dimensional geometric algebras is introduced. It extends the works developed for parallel robots and reviewed above. In particular, one of the novelties of this method is that it can be applied to both redundant and non-redundant serial robots of any geometry. We first model the twists defined by the joint axes as vectors of the six-dimensional geometric algebra. Then, we manipulate the exterior product of these twists. In addition, this method can be simplified for serial robots with a spherical wrist using, instead of the six-dimensional geometric algebra ${\mathcal{G}}_6$, the three-dimensional geometric algebra ${\mathcal{G}}_3$. Once the singularities have been identified and since rotors describe the transformations between arbitrary multivectors in geometric algebra, a distance function D can be defined in the configuration space $\mathcal{C}$ that can be used to determine the distance of any arbitrary configuration $\mathit{q}\in \mathcal{C}$ to a given singularity ${\mathit{q}}_s\in \mathcal{S}$. This is the first time, to the best of the authors’ knowledge, that such a distance function has been defined. It is well-known that there are several indices that can be used to check whether a given configuration is close to a singularity or not. For instance, the Jacobian matrix $J\left(\mathit{q}\right)$ allows us to define the manipulability index $w_{\mathrm{m}}$ as:

$$w_{\mathrm{m}}=\sqrt{det\left({J\left(\mathit{q}\right)}^{T}J\left(\mathit{q}\right)\right)}={\sigma }_n\dots {\sigma }_1,$$

(11)

where $0\le {\sigma }_1\le \dots \le {\sigma }_n$ are the singular values of $J\left(\mathit{q}\right)$. Alternatively, we can also define the condition number of $J\left(\mathit{q}\right)$, $w_{\mathrm{c}}={\sigma }_n/{\sigma }_1$. Clearly, the former is close to zero when the configuration is close to a singularity, while the value of the latter increases as the robot approaches to a singular configuration. Although there are several approaches based on the use of such indices [27,28], none of them define a distance function, and as stated in [29], they do not provide a realistic measure of how close a singularity is, only whether it is close or not. On the other hand, Yao et al. [30] propose a different index of closeness to singularities for planar parallel robots based on the volume of the workspace. Although interesting, it is still not a distance function, and it cannot be easily applied to serial robots. Similarly, Nawratil [31] defines a distance function for parallel manipulators of the Stewart–Gough type. However, it measures how close a given pose of the end-effector is to a singular pose (i.e., the pose associated with a singular configuration). Hence, such a distance function is not defined in the configuration space $\mathcal{C}$ but in the operational space X. Finally, Bu [32] defines an angle between the velocity vector associated with one of the joints and the manifold generated by the others. Again, such an angle acts as a measure of closeness but not as a distance function, and thus, it does not provide a realistic measure of how close a singularity is.

The rest of the paper is organized as follows: Section 2 presents an overview of geometric algebra that will be useful for understanding the proposed contribution. In Section 3, the novel singularity identification approach and the simplification for serial robots with a spherical wrist are fully developed, while the novel distance function is constructed in Section 4. The application of these results to the Kuka LWR 4+, a redundant serial robot with a spherical wrist, is given in Section 5. Section 6 lists three different applications where both the singularity identification and the novel distance function can be applied in order to illustrate their utility. Finally, the conclusions are given in Section 7.

2. Mathematical Preliminaries: Geometric Algebra

One of the main problems of vector spaces is that linear transformations between them are represented through matrices, which entails a high computational cost when implemented. To overcome these and related problems, geometric algebra provides an excellent framework. It was first introduced by W. K. Clifford [33], whom based his own work on the previous works of H. Grassmann [34] and W. R. Hamilton [35]. Throughout this section, a brief overview of geometric algebra is presented. More detailed treatments of the subject can be found in [36,37,38,39].

Definition 2.

Given two vectors ${\mathit{x}}_1,{\mathit{x}}_2\in {\mathbb{R}}^n$, the outer or exterior product of ${\mathit{x}}_1$ and ${\mathit{x}}_2$, ${\mathit{x}}_1\wedge {\mathit{x}}_2$, is a new element that can be seen as the oriented area of the parallelogram obtained by sweeping the vector ${\mathit{x}}_1$ along ${\mathit{x}}_2$. The exterior product is bilinear, associative and anticommutative. In particular, $\mathit{x}\wedge \mathit{x}=0$ for every $\mathit{x}\in {\mathbb{R}}^n$.

The new element defined by the exterior product is called a bivector, and it is defined to have grade two. By extension, the outer product of a bivector with a vector is known as a trivector, is denoted by ${\mathit{x}}_1\wedge {\mathit{x}}_2\wedge {\mathit{x}}_3$ and is defined to have grade three. Trivectors can be seen as the oriented volume obtained by sweeping the bivector ${\mathit{x}}_1\wedge {\mathit{x}}_2$ along ${\mathit{x}}_3$.

This can be generalized to an arbitrary dimension. Thus,

$${\mathit{x}}_1\wedge {\mathit{x}}_2\wedge \dots \wedge {\mathit{x}}_k$$

(12)

denotes a k-blade, i.e., an element of grade k. Linear combinations of k-blades are known as k-vectors, while linear combinations of k-vectors (for different k) are known as multivectors.

In his work [33], Clifford extends the exterior product by adding a scalar product between vectors, the inner product. He defines the geometric product (also known as the Clifford product) as follows:

$${\mathit{x}}_1{\mathit{x}}_2={\mathit{x}}_1·{\mathit{x}}_2+{\mathit{x}}_1\wedge {\mathit{x}}_2 ({\mathit{x}}_1,{\mathit{x}}_2\in {\mathbb{R}}^n).$$

(13)

Thus, the geometric product between two vectors has two components: the scalar component given by the inner product and the bivector component given by the exterior product. Clearly, it also inherits the associativity and bilinearity of the exterior product.

When applied to an orthonormal basis $B=\{e_1,\dots ,e_n\}$ of ${\mathbb{R}}^n$, the geometric product acts as follows:

$$e_i{e}_j=\left\{\begin{array}{ccc}1\hfill & \mathrm{for}& i=j\hfill \\ e_i\wedge e_j\hfill & \mathrm{for}& i\ne j\hfill \end{array}\right.$$

(14)

Thus, for each $0\le k\le n$, the set of k-vectors is spanned by:

• $\begin{array}{|c|}\hline k=0\\ \hline\end{array} \left\{1\right\}$ (scalars).
• $\begin{array}{|c|}\hline k=1\\ \hline\end{array} \{e_1,\dots ,e_n\}$ (vectors).
• $\begin{array}{|c|}\hline k=2\\ \hline\end{array} {\{e_i\wedge e_j\}}_{1\le i<j\le n}$ (bivectors).
• $\begin{array}{|c|}\hline k=3\\ \hline\end{array} {\{e_i\wedge e_j\wedge e_k\}}_{1\le i<j<k\le n}$ (trivectors).
•      ⋮
• $\begin{array}{|c|}\hline k=\mathrm{r}\\ \hline\end{array} {\{e_{i_1}\wedge \dots \wedge e_{i_r}\}}_{1\le i_1<\dots <i_r\le n}$ (r-vectors).
•      ⋮
• $\begin{array}{|c|}\hline k=\mathrm{n}\\ \hline\end{array} \{e_1\wedge \dots \wedge e_n\}$ (pseudoscalar).

Then, for each $0\le k\le n$, there are exactly $C(n,k)$ generators for the set of k-vectors, and thus, the set of k-vectors defines a vector space with basis $B_k={\{e_{i_1}\wedge \dots \wedge e_{i_k}\}}_{1\le i_1<\dots <i_k\le n}$ and dimension $C(n,k)$.

Definition 3.

Let ${\mathbb{R}}^n$ denote the real vector space of dimension n. Then, the vector space spanned by the basis

$$\mathcal{B}={\{e_{i_1}\wedge \dots \wedge e_{i_r}\}}_{\begin{array}{c}1\le i_1<\dots <i_r\le n\\ 0\le r\le n\end{array}}$$

(15)

endowed with the geometric product defined in (13) is an algebra over $\mathbb{R}$ known as the geometric algebra (GA) of ${\mathbb{R}}^n$. Such an algebra is denoted by ${\mathcal{G}}_n$ and has the dimension $C(n,0)+C(n,1)+\dots +C(n,n)=2^n$.

Remark 2.

Since the grading structure of multivectors is a property associated with the exterior product, the elements of ${\mathcal{G}}_n$ can still be called k-blades, k-vectors and multivectors.

An important family of linear operators in ${\mathcal{G}}_n$ are the grade-k projection operators, denoted by ${〈·〉}_k$ for $0\le k\le n$. Applied to an arbitrary multivector A, ${〈A〉}_k$ projects onto the grade-k components in A, i.e., it returns the components of A that can be expressed as a linear combination of ${\{e_{i_1}\wedge \dots \wedge e_{i_k}\}}_{1\le i_1<\dots <i_k\le n}$. Obviously, if $A_k$ denotes a k-vector, then ${〈A_k〉}_k=A_k$.

Using these operators, general multivectors $A\in {\mathcal{G}}_n$ can be expressed as:

$$A={〈A〉}_0+{〈A〉}_1+\dots +{〈A〉}_n.$$

(16)

Hence, the set of all k-vectors for a given $1\le k\le n$ is a vector subspace of ${\mathcal{G}}_n$ denoted by ${〈{\mathcal{G}}_n〉}_k$ and spanned by $B_k={\{e_{i_1}\wedge \dots \wedge e_{i_k}\}}_{1\le i_1<\dots <i_k\le n}$.

The multivector representation (16) is very useful in defining another important operator in ${\mathcal{G}}_n$. This linear operator is known as the reversion operator and is denoted by the superscript ∼. The reversion is defined over the geometric product of m vectors as:

$${({\mathit{a}}_1\dots {\mathit{a}}_m)}^{\sim }={\mathit{a}}_m\dots {\mathit{a}}_1.$$

(17)

Applied to k-vectors, we have that:

$${\tilde{A}}_k={(-1)}^{\frac{k(k-1)}{2}}{A}_k$$

(18)

due to the anticommutativity of the exterior product. Finally, since reversion is a linear operator, the reverse of an arbitrary multivector is:

$$\tilde{A}={〈\tilde{A}〉}_0+\dots +{〈\tilde{A}〉}_n={〈A〉}_0+{〈A〉}_1-{〈A〉}_2+\dots +{(-1)}^{\frac{n(n-1)}{2}}{〈A〉}_n.$$

(19)

Notice that the reversion operator corresponds simply to matrix transposition when a matrix representation of the n-dimensional algebra is considered.

Finally, another operator of great interest is the dual operator. Every grade-n element of ${\mathcal{G}}_n$ is of the form $\alpha (e_1\wedge \dots \wedge e_n)$ for a scalar $\alpha \in \mathbb{R}$. For each $\alpha \in \mathbb{R}$, $\alpha (e_1\wedge \dots \wedge e_n)$ is known as the volume element $E_{\alpha }$ of ${\mathcal{G}}_n$, while the generator $e_1\wedge \dots \wedge e_n$ is known as the pseudoscalar of ${\mathcal{G}}_n$ and is usually denoted by I. Pseudoscalars allow us to define the dual operator, whose action over a k-vector $A_k$ is:

$$A_k^{*}=I{A}_k,$$

(20)

where $A_k^{*}$ is an $(n-k)$-vector.

Now, let us go back to the bivectors of ${\mathcal{G}}_n$ since they will be fundamental in the modelling of the rotations in ${\mathbb{R}}^n$. An important property of these bivectors is that they always square to a scalar. Therefore, given a bivector B, the unit bivector associated with B is $B^{\prime }=B/\left|B^2\right|$. Unit bivectors of ${\mathcal{G}}_n$ always square to -1. This allows us to compute the following series:

$$exp\left(\alpha B^{\prime }\right)=\sum _{m=0}^{\infty }{\displaystyle \frac{{\left(\alpha B^{\prime }\right)}^m}{m!}},$$

(21)

where $\alpha \in \mathbb{R}$. Expanding Equation (21), we have that:

$$\begin{array}{cc}\hfill exp\left(\alpha B^{\prime }\right)& =1+\alpha B^{\prime }-{\displaystyle \frac{{\alpha }^2}{2}}-{\displaystyle \frac{{\alpha }^{3}B}{3!}}+\dots \hfill \\ \hfill & =\left(1-{\displaystyle \frac{{\alpha }^2}{2}}+\dots \right)+B^{\prime }\left(\alpha -{\displaystyle \frac{{\alpha }^3}{3!}}+\dots \right)\hfill \\ \hfill & =cos\left(\alpha \right)+B^{\prime }sin\left(\alpha \right).\hfill \end{array}$$

(22)

Equation (22) indicates that $exp\left(\alpha B^{\prime }\right)$ could be related to rotations. Indeed, we have the following result.

Proposition 1.

Let B be a unit bivector and $0\le \theta \le 2\pi$; then, $R=exp(-\left(\theta B\right)/2)=cos(\theta /2)-Bsin(\theta /2)\in {〈{\mathcal{G}}_n〉}_0+{〈{\mathcal{G}}_n〉}_2$ defines a rotation by an angle θ with a rotation plane represented by B. It acts over an element $X\in {\mathcal{G}}_n$ through the sandwiching product:

$$X^{\prime }=RX\tilde{R}.$$

(23)

Such an element R is termed a rotor.

Rotors satisfy the following properties:

(1)

$R\tilde{R}=1$;

(2)

$R\mathit{x}\tilde{R}=(-R)\mathit{x}(-\tilde{R})$ for $\mathit{x}\in {\mathbb{R}}^n$;

(3)

$R\mathit{x}\mathit{y}\tilde{R}=R\mathit{x}\tilde{R}R\mathit{y}\tilde{R}$ for $\mathit{x},\mathit{y}\in {\mathbb{R}}^n$;

(4)

Given two frames $\{e_1,\dots ,e_n\}$ and $\{f_1,\dots ,f_n\}$, there always exists a rotor R that transforms one into the other, i.e., there exists a rotor R such that $e_i=R{f}_i\tilde{R}$ for $i=1,\dots ,n$. This rotor is computed as [36,40]:

$$R={\displaystyle \frac{1+e^1{f}_1+\dots +e^n{f}_n}{\parallel 1+e^1{f}_1+\dots +e^n{f}_n\parallel }},$$

(24)

where $\{e^1,\dots ,e^n\}$ denotes the reciprocal frame of $\{e_1,\dots ,e_n\}$.

The first property is the analogous version of the property defining the orthogonal matrices with determinant equal to 1, which are known to represent rotations. The second property proves that both R and $-R$ encode the same rotation, while the third property is known as the geometric covariance of rotors.

In general, rotors define a group $\mathfrak{R}$ with the geometric product as the group product:

$$\mathfrak{R}=\{R\in {〈{\mathcal{G}}_n〉}_0+{〈{\mathcal{G}}_n〉}_2 : R\tilde{R}=1\}.$$

(25)

Therefore, the product of two different rotors $R_1$ and $R_2$ also encodes a rotation. In particular, it is the rotation resulting from the composition of the rotations encoded by $R_1$ and $R_2$, respectively. In addition, the second property states that $\mathfrak{R}$ provides a double covering of the rotation group.

Finally, one of the most important geometric algebras is the spatial geometric algebra ${\mathcal{G}}_3$, whose basis is:

$$\{1,e_1,e_2,e_3,e_{12},e_{13},e_{23},I\},$$

(26)

where $\{e_1,e_2,e_3\}$ is an orthonormal basis of ${\mathbb{R}}^3$ and $e_{ij}=e_i\wedge e_j$.

3. Identification of Singularities Using Geometric Algebra

Since six degrees of freedom are required to describe the position and orientation of a rigid body in the three-dimensional space, the more natural way of formulating the singularity problem is through the six-dimensional geometric algebra ${\mathcal{G}}_6$, which extends naturally the three-dimensional algebra ${\mathcal{G}}_3$ introduced in Section 2. Screw theory [41,42] provides an intuitive and geometrical description of the differential kinematics of serial and parallel manipulators using six-dimensional vectors. Because of this, throughout this chapter, some concepts taken from this theory will be employed. This will provide the initial framework to completely understand the approach introduced in this section.

As stated in the introduction, we are going to work with three-dimensional rigid motions, i.e., three-dimensional orientation-preserving isometries. They form a Lie group, called the special Euclidean group, denoted by $SE\left(3\right)$. Its associated Lie algebra is:

$$\mathfrak{se}\left(3\right)=\left\{\widehat{\xi }\in {\mathcal{M}}_4 : \widehat{\xi }=\left(\begin{array}{cc}\Omega & \mathit{v}\\ 0& 0\end{array}\right)\right\},$$

(27)

where $\Omega$ is a skew-symmetric matrix of order 3, $\mathit{v}\in {\mathbb{R}}^3$, and ${\mathcal{M}}_4$ denotes the vector space of order 4 square matrices with real entries. Since every skew-symmetric matrix $\Omega$ can be represented as a vector $\mathit{\omega }$, we can express an element $\widehat{\xi }\in \mathfrak{se}\left(3\right)$ as a six-dimensional vector $\xi ={[\mathit{\omega } \mathit{v}]}^T$, termed a twist. Therefore, twists are the infinitesimal generators of rigid motions via the exponential map, i.e., $exp\left(\widehat{\xi }t\right)=f\left(t\right)$ with $f\in SE\left(3\right)$. The next theorem is a fundamental result in screw theory.

Theorem 1

(Chasles, 1830 [43]). Every rigid motion $f\in SE\left(3\right)$ can be realized as a rotation around an axis followed (preceded) by a translation along the same axis.

Definition 4.

A screw motion consists of a rotation around an axis followed (preceded) by a translation along the same axis, the screw axis ℓ. The ratio between the translational and the rotational parts of the motion is known as the pitch and is denoted by h. In particular, if a point is rotated around ℓ by an angle $\theta \ne 0$ and translated along ℓ by an amount d, then $h=d/\theta$. By convention, if $\theta =0$, $h=\infty$.

Remark 3.

For infinitesimal motions, if $\theta \ne 0$, then the pitch is defined as $h=\dot{d}/\dot{\theta }$.

Hence, every rigid motion is a screw motion. Particular cases of screw motions are pure rotations (pure translations) where the translation (rotation) is the identity or, equivalently, $h=0$ ($h=\infty$). In addition, every screw motion can be characterized by the triple $(\ell ,h,q)$, where q denotes the magnitude of the motion. If $h\ne \infty$, then $\theta =q$ and $d=h\theta$, while if $h=\infty$, then $\theta =0$ and $d=q$. We call this triple the screw associated with the screw motion, and we denote it using $.

Proposition 2.

Given a screw $\$=(\ell ,h,q)$ with screw axis ℓ, pitch h and magnitude q, there exists a twist ξ such that the rigid motion it generates is the screw motion associated with $.

Proposition 2 states a correspondence between twists and screws that is useful for our purposes. In particular, if $\mathit{p}$ is a point on ℓ and $\mathit{v}$ is its direction unit vector, then $\ell =\{\mathit{p}+\mathit{v}\lambda :\lambda \in \mathbb{R}\}$ and we have that:

$$\begin{array}{cc}\xi =\theta \left[\begin{array}{c}\mathit{v}\\ \mathit{p}\times \mathit{v}+h\mathit{v}\end{array}\right]\hfill & \mathrm{for} \mathrm{a} \mathrm{general} \mathrm{screw} \mathrm{motion},\hfill \\ & \\ \xi =\theta \left[\begin{array}{c}\mathit{v}\\ \mathit{p}\times \mathit{v}\end{array}\right]\hfill & \mathrm{for} \mathrm{a} \mathrm{pure} \mathrm{rotation},\hfill \\ & \\ \xi =d\left[\begin{array}{c}\mathbf{0}\\ \mathit{v}\end{array}\right]\hfill & \mathrm{for} \mathrm{a} \mathrm{pure} \mathrm{translation}.\hfill \end{array}$$

(28)

A twist $\xi$ associated with a magnitude 1 screw $ is said to be a unit twist. Hence, any twist $\xi$ can be seen as a unit twist multiplied by the magnitude of the associated screw axis:

$$\xi =\theta {\xi }_U=\theta \left[\begin{array}{c}\mathit{v}\\ \mathit{p}\times \mathit{v}+h\mathit{v}\end{array}\right],$$

(29)

where ${\xi }_U$ is a unit twist. Clearly, $\xi$ is associated with $\$=(\ell ,h,q)$, while ${\xi }_U$ is associated with $\$=(\ell ,h,1)$.

Proposition 3.

Let us consider a rigid body performing a screw motion represented by the screw $\$=(\ell ,h,q(t\left)\right)$, where the magnitude $q\left(t\right)$ is a time-dependent variable. Its velocity during the screw motion is given by the associated twist ξ, where, now, the pitch is defined as in Remark 3. In particular:

$$\begin{array}{cc}\xi =\dot{\theta }\left(t\right)\left[\begin{array}{c}\mathit{v}\\ \mathit{p}\times \mathit{v}+h\mathit{v}\end{array}\right]\hfill & \mathrm{if} \theta \ne 0,\hfill \\ & \\ \xi =\dot{d}\left(t\right)\left[\begin{array}{c}\mathbf{0}\\ \mathit{v}\end{array}\right]\hfill & \mathrm{if} \theta =0,\hfill \end{array}$$

(30)

where, here, $\dot{\theta }\left(t\right)$ ($\dot{d}\left(t\right)$) is known as the twist amplitude.

Now, let us consider a serial robot with n DoF, where $\mathit{\omega },\mathit{v}$ denote the angular and linear velocity vectors of its end-effector. If Equation (3) is expanded, the following is obtained:

$$\left[\begin{array}{c}\mathit{v}\\ \mathit{\omega }\end{array}\right]=J_1\left(\mathit{q}\right){\dot{q}}_1+\dots +J_n\left(\mathit{q}\right){\dot{q}}_n,$$

(31)

where $J_i$ denotes the i-th column of the Jacobian matrix J. Notice that the right side of Equation (31) can be seen as the addition of the twists associated with the joints of the robot, where ${\dot{q}}_i$ plays the role of the twist amplitude and where the linear and angular parts are interchanged. However, for the sake of formality, let us consider the unit twist ${\xi }_i$ associated with the i-th joint of the robot (Since, from now on, we are going to work exclusively with unit twists, the subindex U is omitted for simplicity). Then:

$${\xi }_i\left(\mathit{q}\right){\dot{q}}_i=\left\{\begin{array}{ccc}\left[\begin{array}{c}{\mathit{z}}_i\\ {\mathit{z}}_i\times ({\mathit{o}}_n-{\mathit{o}}_i)\end{array}\right]{\dot{q}}_i\hfill & & \mathrm{if} \mathrm{joint} i \mathrm{is} \mathrm{revolute}\hfill \\ \left[\begin{array}{c}\mathbf{0}\\ {\mathit{z}}_i\end{array}\right]{\dot{q}}_i\hfill & & \mathrm{if} \mathrm{joint} i \mathrm{is} \mathrm{prismatic}\hfill \end{array}\right.$$

(32)

where, as stated in the introduction, ${\mathit{z}}_i$ is the direction vector of the joint axis, ${\mathit{o}}_n$ (${\mathit{o}}_i$) is the origin of the frame attached to the end-effector (i-th joint) and ${\dot{q}}_i={\dot{\theta }}_i$ if joint i is revolute and ${\dot{q}}_i={\dot{d}}_i$ if joint i is prismatic.

Remark 4.

The unit twists ${\xi }_i\left(\mathit{q}\right)$ defined in Equation (32) are represented with respect to the world frame, not with respect to the local frame attached to the previous joint. If the unit twists are defined with respect to a local frame, we need to use the adjoint transformation to represent them with respect to the world frame. In particular, ${\xi }_i^{\prime }\left(\mathit{q}\right)={Ad}_f{\xi }_i\left(\mathit{q}\right)$, where ${Ad}_f:{\mathbb{R}}^6\to {\mathbb{R}}^6$ is the adjoint transformation associated with the rigid motion f, i.e., the rigid motion transforming the reference frame to the local frame in which the twist is initially represented.

The following is a key result:

Theorem 2

(Tsai, 1999 [44]). Given a serial robot with n DoF:

$$\left[\begin{array}{c}\mathit{\omega }\\ \mathit{v}\end{array}\right]={\xi }_1\left(\mathit{q}\right){\dot{q}}_1+\dots +{\xi }_n\left(\mathit{q}\right){\dot{q}}_n=[{\xi }_1\left(\mathit{q}\right) \dots {\xi }_n\left(\mathit{q}\right)]\dot{\mathit{q}},$$

(33)

where, again, $\mathit{\omega },\mathit{v}$ denote the angular and linear velocity vectors of the robot’s end-effector and $\dot{\mathit{q}}=({\dot{q}}_1,\dots ,{\dot{q}}_n)$.

The main advantage of the screw-based Jacobian matrix defined in Equation (33) is that it allows a geometrical identification of the singularities. Moreover, if an approach based on geometric algebra is used, an intuitive geometrical and computer-friendly algebraic identification of the singularities is possible. For that purpose, let us consider the geometric algebra ${\mathcal{G}}_6$, where, for every $i=1,\dots ,n$, the unit twist ${\xi }_i\left(\mathit{q}\right)$ can be modelled as a vector. Indeed, we make the identification ${\xi }_i\left(\mathit{q}\right)={[{\xi }_{i_1}\dots {\xi }_{i_6}]}^T$ with the vector $x={\xi }_{i_1}{e}_1+\dots +{\xi }_{i_6}{e}_6\in {\mathcal{G}}_6$, where $e_1,\dots ,e_6$ are the basis vectors of ${\mathcal{G}}_6$.

The following gives the main result of this section.

Theorem 3.

Let ${\xi }_i\left(\mathit{q}\right)$ denote the unit twist defined by the i-th joint expressed as a vector of ${\mathcal{G}}_6$. Then:

$${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=det\left([{\xi }_1\left(\mathit{q}\right) \dots {\xi }_6\left(\mathit{q}\right)]\right)e_1\wedge \dots \wedge e_6.$$

(34)

Theorem 3 can be seen as a particular case of a more general result:

Theorem 4.

Let ${\mathit{a}}_1,\dots ,{\mathit{a}}_n$ be a set of n vectors of ${\mathcal{G}}_n$. Then:

$${\mathit{a}}_1\wedge \dots \wedge {\mathit{a}}_n=det\left([{\mathit{a}}_1 \dots {\mathit{a}}_n]\right)e_1\wedge \dots \wedge e_n$$

(35)

Theorem 4 can be easily deduced from Equation (4.143) in [36] (p. 108):

$$F\left(I\right)=det\left(F\right)I,$$

(36)

where F is a linear transformation in ${\mathcal{G}}_n$, $F:{\mathcal{G}}_n\to {\mathcal{G}}_n$, and, as stated in Section 2, I denotes the pseudoscalar of ${\mathcal{G}}_n$.

In particular, Theorem 4 is true for any set of six vectors ${\mathit{a}}_1,\dots ,{\mathit{a}}_6$ of ${\mathcal{G}}_6$, which proves Theorem 3. Now, the following corollary of Theorem 3 allows us to characterize the singularities of any serial robot of 6 DoF.

Corollary 1.

Given a serial robot with 6 DoF and associated unit twists ${\xi }_1\left(\mathit{q}\right),\dots ,{\xi }_6\left(\mathit{q}\right)$, then $\mathit{q}\in \mathcal{S}$ if, and only if, ${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=0$.

Proof. 

Taking the dual of Equation (34), the following identity is obtained:

$${({\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right))}^{*}=det\left([{\xi }_1\left(\mathit{q}\right) \dots {\xi }_6\left(\mathit{q}\right)]\right)$$

(37)

and, therefore, the singularities of the serial robot are those configurations $\mathit{q}\in \mathcal{C}$ verifying that:

$${({\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right))}^{*}=0.$$

(38)

Now, since for a given non-zero multivector $M\in {\mathcal{G}}_n$, $M^{*}=0$ if, and only if, $M=0$, Equation (38) can be simplified to:

$${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=0.$$

(39)

Thus, $\mathit{q}\in \mathcal{S}$ if, and only if, ${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=0$. □

Remark 5.

Notice that what has also been proven in Corollary 1 is that the outer product of n vectors of ${\mathcal{G}}_n$ is zero if, and only if, the n vectors are linearly dependent.

In addition, Corollary 1 allows us to re-define the singular set as:

$$\mathcal{S}=\{\mathit{q}\in \mathcal{C} : {\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=0\}.$$

(40)

Remark 6.

What Theorem 3 states is that, for instance, if two unit twists ${\xi }_1$ and ${\xi }_2$ satisfy ${\xi }_1\wedge {\xi }_2=0$, then they represent the same twist, and hence, they generate the same screw motion. This means that if such a screw motion is a pure translation, then the translational axes are either parallel or coincident, while if the screw motion is a pure rotation, the rotational axes are coincident (Since the twists contain the term $({\mathit{z}}_i\times ({\mathit{o}}_6-{\mathit{o}}_i))$ for $i=1,2$, they cannot be parallel). Regarding the kinematic singularities of serial robots, this implies that two prismatic joints whose axes are either parallel or coincident give rise to a singularity and, equivalently, that two revolute joints whose axes are coincident give rise to a singularity. This is, in fact, in agreement with what is known about kinematic singularities since two parallel revolute joint axes do not give rise to a singularity. Obviously, the same geometrical interpretation can be made for three, four or more unit twists satisfying that their outer product is zero.

With respect to redundant serial robots, it is clear that, for $n>6$, ${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_n\left(\mathit{q}\right)=0$ for any $\mathit{q}\in \mathcal{C}$. Hence, Corollary 1 on its own does not allow us to characterize the singularities of redundant robots. However, this problem can be easily overcome by studying all the possible combinations of six unit twists in $\{{\xi }_1\left(\mathit{q}\right),\dots ,{\xi }_n\left(\mathit{q}\right)\}$. We denote the set of all combinations of six elements that can be drawn from $\{1,\dots ,n\}$ by S. Clearly, S has $C(n,6)=\left(\genfrac{}{}{0pt}{}{n}{6}\right)$ elements of the form $\{i_1,\dots ,i_6\}$, where $1\le i_1<\dots <i_6\le n$ and $1\le i\le C(n,6)$.

Theorem 5.

Given a serial robot with n DoF and associated unit twists ${\xi }_1\left(\mathit{q}\right),\dots ,{\xi }_n\left(\mathit{q}\right)$, then $\mathit{q}\in \mathcal{S}$ if, and only if, for each $1\le i\le C(n,6)$:

$${\xi }_{i_1}\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_{i_6}\left(\mathit{q}\right)=0,$$

(41)

where $\{i_1,\dots ,i_6\}$ is the i-th element of S.

Proof. 

It follows that, for $\mathit{q}\in \mathcal{C}$:

$$\begin{array}{cc}\hfill & {\xi }_{i_1}\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_{i_6}\left(\mathit{q}\right)=0 \mathrm{for} \mathrm{every} 1\le i\le C(n,6)\hfill \\ \hfill & \stackrel{\left(1\right)}{⟺}det\left([{\xi }_{i_1}\left(\mathit{q}\right) \dots {\xi }_{i_6}\left(\mathit{q}\right)]\right)=0 \mathrm{for} \mathrm{every} 1\le i\le C(n,6)\hfill \\ \hfill & \stackrel{\left(2\right)}{⟺}\rho \left([{\xi }_1\left(\mathit{q}\right) \dots {\xi }_n\left(\mathit{q}\right)]\right)<6\hfill \end{array}$$

(42)

where $\left(1\right)$ uses Equations (37) and $\left(2\right)$ uses the fact that all the minors of order 6 of the matrix $[{\xi }_1\left(\mathit{q}\right) \dots {\xi }_n\left(\mathit{q}\right)]$ have null determinants. Clearly, $\rho \left([{\xi }_1\left(\mathit{q}\right) \dots {\xi }_n\left(\mathit{q}\right)]\right)<6$ if, and only if, $\rho \left(J\right(\mathit{q}\left)\right)<6$, which, in turn, is equivalent to $\mathit{q}\in \mathcal{S}$ (by Definition 1). □

The computation of either Equation (39) for non-redundant robots or Equation (41) for redundant ones is computationally more efficient than the computation of either $det\left(J\right(\mathit{q}\left)\right)=0$ or $det\left(J\left(\mathit{q}\right)J^T\left(\mathit{q}\right)\right)=0$. The main reason for this lies in the computational complexity of the operations needed to obtain the expressions (39) or (41) with respect to the complexity of the operations needed for obtaining $det\left(J\right(\mathit{q}\left)\right)=0$ or $det\left(J\left(\mathit{q}\right)J^T\left(\mathit{q}\right)\right)=0$. It is clear that the outer product of n vectors of ${\mathcal{G}}_n$ behaves like the addition and product of real numbers, and hence, it has complexity $O\left(n\right)+O\left(n^2\right)$, while the determinant has complexity $O\left(n^3\right)$ or $O\left(n^4\right)$ depending on the algorithm used. In addition, for redundant robots, there are two main operations: the product between $J\left(\mathit{q}\right)$ and $J^T\left(\mathit{q}\right)$ and the determinant of the product matrix. This implies that, for this case, the complexity increases to $O\left(n^3\right)+O\left(n^4\right)$.

Special Case: Serial Robots with a Spherical Wrist

Similarly to what happens with the Jacobian matrix J, a simplification can be achieved for robots that have a spherical wrist. As stated in Section 1, the singularities of these robots can be decoupled into position and orientation singularities. Position singularities involve the first $n-3$ joints and are computed by studying the rank of the following matrix:

$$J_p=\left[\begin{array}{ccc}{\mathit{s}}_1& \dots & {\mathit{s}}_{n-3}\end{array}\right],$$

(43)

where ${\mathit{s}}_i={\mathit{z}}_i\times ({\mathit{o}}_n-{\mathit{o}}_i)$ if joint i is revolute and ${\mathit{s}}_i={\mathit{z}}_i$ if joint i is prismatic. On the other hand, orientation singularities involve the last three joints and are computed through the determinant of the following matrix:

$$J_o=\left[\begin{array}{ccc}{\mathit{z}}_{n-2}& {\mathit{z}}_{n-1}& {\mathit{z}}_n\end{array}\right].$$

(44)

Now, let us consider the three-dimensional geometric algebra ${\mathcal{G}}_3$. As proven in Theorem 4 for $n=3$, ${\mathit{a}}_1\wedge {\mathit{a}}_2\wedge {\mathit{a}}_3=det\left([{\mathit{a}}_1 {\mathit{a}}_2 {\mathit{a}}_3]\right)e_1\wedge e_2\wedge e_3$ for any three vectors ${\mathit{a}}_1,{\mathit{a}}_2,{\mathit{a}}_3\in {\mathbb{R}}^3$. Hence, analogously to what has been performed before, the following characterization for the position and orientation singularities can be deduced.

Theorem 6.

Given a serial robot with n DoF and a spherical wrist, if either ${\mathit{z}}_i\times ({\mathit{o}}_n-{\mathit{o}}_i)$ or ${\mathit{z}}_i$ are denoted by ${\mathit{s}}_i$ for $i=1,\dots ,n-3$, then:

• $\mathit{q}\in \mathcal{C}$ is a position singularity if, and only if, ${\mathit{s}}_{i_1}\left(\mathit{q}\right)\wedge {\mathit{s}}_{i_2}\left(\mathit{q}\right)\wedge {\mathit{s}}_{i_3}\left(\mathit{q}\right)=0$ for each $1\le i\le C(n-3,3)$, where $\{i_1,i_2,i_3\}$ is the i-th combination of three elements drawn from $\{1,\dots ,n-3\}$.
• $\mathit{q}\in \mathcal{C}$ is an orientation singularity if, and only if,

  $${\mathit{z}}_{n-2}\left(\mathit{q}\right)\wedge {\mathit{z}}_{n-1}\left(\mathit{q}\right)\wedge {\mathit{z}}_n\left(\mathit{q}\right)=0.$$

  (45)

Proof. 

The proof is completely analogous to the proof of Corollary 1 and Theorem 5. □

Remark 7.

Since the last three joint axes either intersect at a single point or are parallel, there is only one orientation singularity, namely, when these three joint axes are coplanar. This can also be easily deduced from Equation (45). A schematic representation of such singularity, also called wrist singularity, is depicted in Figure 2.

4. Distance to Singularities

Let ${\mathit{q}}_1,{\mathit{q}}_2\in \mathcal{C}$ be two arbitrary configurations of a serial robot with n DoF and let ${\xi }_1,\dots ,{\xi }_n$ be the unit twists associated with its joints. Then, there exist $R_1({\mathit{q}}_1,{\mathit{q}}_2),\dots ,R_n({\mathit{q}}_1,{\mathit{q}}_2)$, where, for each $1\le i\le n$, $R_i({\mathit{q}}_1,{\mathit{q}}_2)$ is a configuration-dependent rotor in the six-dimensional geometric algebra ${\mathcal{G}}_6$ such that (Figure 3):

$${\xi }_i\left({\mathit{q}}_2\right)=R_i({\mathit{q}}_1,{\mathit{q}}_2){\xi }_i\left({\mathit{q}}_1\right){\tilde{R}}_i({\mathit{q}}_1,{\mathit{q}}_2).$$

(46)

The reason why these rotors exist is simple: unit twists are modelled as vectors in ${\mathcal{G}}_6$ and there always exists a rotor relating any pair of vectors in any geometric algebra ${\mathcal{G}}_n$. In particular, there is always a rotor relating the same unit twist $\xi$ in two different configurations ${\mathit{q}}_1,{\mathit{q}}_2$.

Now, let ${\mathit{q}}_s\in \mathcal{S}$ denote a singularity of a serial robot. As explained in the previous section, if the serial robot has a spherical wrist, then ${\mathit{q}}_s$ only involves a maximum of two or three joints and, therefore, two or three unit twists. If, conversely, the robot has no spherical wrist, then it can involve a maximum of six joints. Let us suppose, without loss of generality, that a given singularity ${\mathit{q}}_s$ involves the joints $i_1,\dots ,i_r$ with associated unit twists ${\xi }_{i_1}\left({\mathit{q}}_s\right),\dots ,{\xi }_{i_r}\left({\mathit{q}}_s\right)$ for $2\le r\le 6$. Then, for any configuration $\mathit{q}\in \mathcal{C}$, there exist $R_{i_1}(\mathit{q},{\mathit{q}}_s),\dots ,R_{i_r}(\mathit{q},{\mathit{q}}_s)$ such that:

$${\xi }_{i_j}\left({\mathit{q}}_s\right)=R_{i_j}(\mathit{q},{\mathit{q}}_s){\xi }_{i_j}\left(\mathit{q}\right){\tilde{R}}_{i_j}(\mathit{q},{\mathit{q}}_s) \mathrm{for} \mathrm{each} 1\le j\le r.$$

(47)

The notation chosen for these rotors expresses a configuration dependence that is not a functional dependency, i.e., there is no analytical expression for these rotors with $\mathit{q}$ as a variable.

Now, it is clear that $R_{i_j}(\mathit{q},{\mathit{q}}_s)=1$ if, and only if, $\mathit{q}={\mathit{q}}_s$ for every $j=1,\dots ,r$. However, since for each j, $R_{i_j}(\mathit{q},{\mathit{q}}_s)$ does not define a function on $\mathit{q}$, a distance function cannot be defined. However, the measure of how close a given configuration $\mathit{q}$ is to a singularity can be set as:

$$\mathit{q}\approx {\mathit{q}}_s⟺R_{i_j}\left(\mathit{q}\right)\approx 1 \mathrm{for} \mathrm{every} j=1,\dots ,r.$$

(48)

Example 1.

Let ${\mathit{q}}_s\in \mathcal{S}$ be a singularity of a serial robot that only involves the second and third joints. Then, for any configuration $\mathit{q}\in \mathcal{C}$, there exist $R_2(\mathit{q},{\mathit{q}}_s)$ and $R_3(\mathit{q},{\mathit{q}}_s)$ such that:

$$\begin{array}{cc}\hfill {\xi }_2\left({\mathit{q}}_s\right)& =R_2(\mathit{q},{\mathit{q}}_s){\xi }_2\left(\mathit{q}\right){\tilde{R}}_2(\mathit{q},{\mathit{q}}_s),\hfill \\ \hfill {\xi }_3\left({\mathit{q}}_s\right)& =R_3(\mathit{q},{\mathit{q}}_s){\xi }_3\left(\mathit{q}\right){\tilde{R}}_3(\mathit{q},{\mathit{q}}_s).\hfill \end{array}$$

(49)

Therefore, $\mathit{q}$ is close to ${\mathit{q}}_s$ if, and only if:

$$\left.\begin{array}{cc}\hfill R_2(\mathit{q},{\mathit{q}}_s)& \approx 1\hfill \\ \hfill R_3(\mathit{q},{\mathit{q}}_s)& \approx 1\hfill \end{array}\right\}$$

(50)

and is singular if, and only if:

$$\left.\begin{array}{cc}\hfill R_2(\mathit{q},{\mathit{q}}_s)& =1\hfill \\ \hfill R_3(\mathit{q},{\mathit{q}}_s)& =1\hfill \end{array}\right\}$$

(51)

where, in general, $R_2(\mathit{q},{\mathit{q}}_s)\ne R_3(\mathit{q},{\mathit{q}}_s)$.

These rotors can be constructed in many different ways. The easiest way consists of considering, for each $1\le i\le n$, the frame $\left\{i\right\}$ attached to joint i and constructed from ${\xi }_i$. This three-dimensional frame varies with the configuration $\mathit{q}$. Hence, for two different configurations ${\mathit{q}}_1$ and ${\mathit{q}}_2$, there are two frames $\left\{i\right\}$ attached to joint i. As shown in Section 2, we can recover the three-dimensional rotor that transforms one of the frames into the other. Since each frame $\left\{i\right\}$ depends continuously on the configuration $\mathit{q}$, the rotor $R_i\left(\mathit{q}\right)$ is a continuous function defined as follows:

$$\begin{array}{cc}\hfill R_i:\mathcal{C}& \to \mathfrak{R}\hfill \\ \hfill \mathit{q}& \mapsto R_i\left(\mathit{q}\right)\hfill \end{array}$$

(52)

Thus, these configuration-dependent rotors exhibit a functional dependency on the configuration, which allows us to define a distance function. Such distance is based on the norm of a multivector $X\in {\mathcal{G}}_n$, defined by the relation:

$${\parallel X\parallel }^2={〈X\tilde{X}〉}_0.$$

(53)

To prove that $\parallel ·\parallel$ is a norm, the following two lemmas are necessary.

Lemma 1.

For any given multivector $X\in {\mathcal{G}}_n$, ${〈X\tilde{X}〉}_0\in {\mathbb{R}}^{+}=[0,+\infty )$.

Proof. 

According to Equation (19), it follows that:

$$\tilde{X}={〈\tilde{X}〉}_0+{〈\tilde{X}〉}_1+\dots +{〈\tilde{X}〉}_n.$$

(54)

Then:

$$X\tilde{X}=\sum _{i=0}^n\sum _{j=0}^n{〈X〉}_i{〈\tilde{X}〉}_j.$$

(55)

Now, note that, for each $i=1,\dots ,n$, ${〈X〉}_i$ is a i-vector, i.e., it only contains terms of grade i. The geometric product of two k-vectors (with different k) is stated as follows [36] (p. 103):

$$A_r{B}_s={〈A_r{B}_s〉}_{|r-s|}+{〈A_r{B}_s〉}_{|r-s|+2}+\dots +{〈A_r{B}_s〉}_{r+s},$$

(56)

Therefore, it is clear that ${〈{〈X〉}_i{〈\tilde{X}〉}_j〉}_0=0$ for $i\ne j$. Thus:

$${〈X\tilde{X}〉}_0=\sum _{i=0}^n{〈{〈X〉}_i{〈\tilde{X}〉}_i〉}_0.$$

(57)

Now, each ${〈X〉}_i$ can be expanded as follows:

$${〈X〉}_i=\sum _{j=1}^{C(n,i)}{\alpha }_j\left(i\right)e_{j_1}\dots e_{j_i},$$

(58)

where, for every $1\le j\le C(n,i)=\left(\genfrac{}{}{0pt}{}{n}{i}\right)$, ${\alpha }_j\left(i\right)\in \mathbb{R}$ and $e_{j_1}\dots e_{j_i}$ are the base elements of ${〈{\mathcal{G}}_n〉}_i$. Therefore:

$${〈\tilde{X}〉}_i=\sum _{j=1}^{C(n,i)}{\alpha }_j\left(i\right)e_{j_i}\dots e_{j_1}$$

(59)

and, thus:

$$\begin{array}{cc}\hfill {〈X〉}_i{〈\tilde{X}〉}_i& =\sum _{j=1}^{C(n,i)}{\alpha }_j\left(i\right)e_{j_1}\dots e_{j_i}\sum _{j=1}^{C(n,i)}{\alpha }_j\left(i\right)e_{j_i}\dots e_{j_1}\hfill \\ \hfill & =\sum _{j=1}^{C(n,i)}\sum _{k=1}^{C(n,i)}{\alpha }_j\left(i\right){\alpha }_k\left(i\right)e_{j_1}\dots e_{j_i}{e}_{k_i}\dots e_{k_1},\hfill \end{array}$$

(60)

where, clearly, ${〈e_{j_1}\dots e_{j_i}{e}_{k_i}\dots e_{k_1}〉}_0={\delta }_{jk}$ with ${\delta }_{jk}$ the Kronecker delta. Then:

$${〈{〈X〉}_i{〈\tilde{X}〉}_i〉}_0=\sum _{j=1}^{C(n,i)}{\alpha }_j^2,$$

(61)

which, for every $1\le i\le n$, is a positive scalar. This implies that the sum of Equation (57) is also a positive scalar. □

Lemma 2.

Given three strictly positive real numbers $a_1,a_2,a_3\in {\mathbb{R}}^{+}\setminus \left\{0\right\}$, the following properties hold:

• $\sqrt{a_1+a_2-a_3}\le \sqrt{a_1}+\sqrt{a_2}$;
• $\sqrt{a_1+a_2+a_3}\le \sqrt{a_1}+\sqrt{a_2}$ if, and only if, $a_3\le 2\sqrt{a_1{a}_2}$.

Proof. 

Both properties can be obtained by a straightforward computation. □

Proposition 4.

The function $\parallel ·\parallel :{\mathcal{G}}_n\to {\mathbb{R}}^{+}$ defined by the identity ${\parallel X\parallel }^2={〈X\tilde{X}〉}_0$ is a norm in ${\mathcal{G}}_n$, i.e.,

(i) 

$\parallel X\parallel \ge 0$ for all $X\in {\mathcal{G}}_n$. In particular, $\parallel X\parallel =0$ if, and only if, $X=0$.

(ii) 

$\parallel \lambda X\parallel =\left|\lambda \right|\parallel X\parallel$ for all $X\in {\mathcal{G}}_n$ and $\lambda \in \mathbb{R}$.

(iii) 

$\parallel X+Y\parallel \le \parallel X\parallel +\parallel Y\parallel$ for all $X,Y\in {\mathcal{G}}_n$ (usually known as the triangle inequality).

Proof. 

(i)

Given a multivector X, identity ${\parallel X\parallel }^2={〈X\tilde{X}〉}_0$ is equivalent to:

$$\parallel X\parallel =\pm \sqrt{{〈X\tilde{X}〉}_0}.$$

(62)

Thus, it is clear by Lemma 1 that the positive branch of Equation (62) is well defined and that $\parallel X\parallel \ge 0$. In particular, if $\parallel X\parallel =0$, then:

$$\sqrt{{〈X\tilde{X}〉}_0}=0⟹{〈X\tilde{X}〉}_0=0⟹\sum _{i=0}^n{〈{〈X〉}_i{〈\tilde{X}〉}_i〉}_0=0,$$

(63)

where all the terms of the last equation are positive by Lemma 1 and, thus, all of them are equal to zero. Now, note that each addend is the geometric product of an i-vector with its reverse. Therefore, if such product is zero, the corresponding i-vector must be zero. Since all the terms are zero, all the i-vectors that form X are zero, and thus, X is zero.

(ii)

If $\lambda \in \mathbb{R}$ and $X\in {\mathcal{G}}_n$, then:

$$\begin{array}{cc}\hfill \parallel \lambda X\parallel & =\sqrt{{〈\left(\lambda X\right)\left(\lambda \tilde{X}\right)〉}_0}=\sqrt{{〈{\lambda }^{2}X\tilde{X}〉}_0}\hfill \\ \hfill & \stackrel{\left(1\right)}{=}\sqrt{{\lambda }^2{〈X\tilde{X}〉}_0}=\left|\lambda \right|\sqrt{{〈X\tilde{X}〉}_0}=\left|\lambda \right|\parallel X\parallel ,\hfill \end{array}$$

(64)

where $\left(1\right)$ uses the linearity of the grade-0 projection operator (as stated in Section 2).

(iii)

Given two different multivectors X and Y, they can be expanded as linear combinations of the basis elements of ${\mathcal{G}}_n$ as follows:

$$\begin{array}{cc}\hfill X& =\sum _{i=0}^{2^n}{\alpha }_i{e}_{j_1}\dots e_{j_i},\hfill \\ \hfill Y& =\sum _{i=0}^{2^n}{\beta }_i{e}_{j_1}\dots e_{j_i}.\hfill \end{array}$$

(65)

Now, it follows that:

$$X+Y=\sum _{i=0}^{2^n}({\alpha }_i+{\beta }_i)e_{j_1}\dots e_{j_i}$$

(66)

and hence:

$$\begin{array}{cc}\hfill \parallel X+Y\parallel & =\sqrt{{〈\left(X+Y\right){\left(X+Y\right)}^{\sim }〉}_0}\stackrel{\left(1\right)}{=}\sqrt{{\displaystyle \sum _{i=0}^{2^n}}{({\alpha }_i+{\beta }_i)}^2}\hfill \\ \hfill & =\sqrt{\underset{A}{\begin{array}{c}\hfill \begin{array}{|c|}\hline {\displaystyle \sum _{i=0}^{2^n}{\alpha }_i^2}\\ \hline\end{array}\end{array}}+\underset{B}{\begin{array}{c}\hfill \begin{array}{|c|}\hline {\displaystyle \sum _{i=0}^{2^n}{\beta }_i^2}\\ \hline\end{array}\end{array}}+2\underset{C}{\begin{array}{c}\hfill \begin{array}{|c|}\hline {\displaystyle \sum _{i=0}^{2^n}{\alpha }_i{\beta }_i}\\ \hline\end{array}\end{array}}},\hfill \end{array}$$

(67)

where $\left(1\right)$ uses Lemma 1, while $A,B$ and C are just notations given to simplify the different manipulations. Since $A,B>0$ (If either $A,B$ are equal to zero, then either $X=0$ or $Y=0$, which will make the condition $\parallel X+Y\parallel \le \parallel X\parallel +\parallel Y\parallel$ trivial):

$$\sqrt{A+B+C}\stackrel{\left(1\right)}{\le }\sqrt{A}+\sqrt{B}=\sqrt{{\sum }_{i=0}^{2^n}{\alpha }_i^2}+\sqrt{{\sum }_{i=0}^{2^n}{\beta }_i^2}=\parallel X\parallel +\parallel Y\parallel ,$$

(68)

where $\left(1\right)$ uses the first or second property of Lemma 2 depending on whether $C<0$ or $C>0$. It only remains to check if $C>0$, $2C\le 2\sqrt{AB}$. Indeed, the previous inequality is equivalent to that $C^2\le AB$. Now:

$$\begin{array}{cc}\hfill AB& =\sum _{i=0}^{2^n}{\alpha }_i^2\sum _{i=0}^{2^n}{\beta }_i^2=\sum _{i=0}^{2^n}\sum _{j=0}^{2^n}{\alpha }_i^2{\beta }_j^2=\sum _{i=0}^{2^n}{\alpha }_i^2{\beta }_i^2+\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2,\hfill \\ \hfill C^2& ={\left(\sum _{i=0}^{2^n}{\alpha }_i{\beta }_i\right)}^2=\sum _{i=0}^{2^n}{\alpha }_i^2{\beta }_i^2+\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i{\beta }_i{\alpha }_j{\beta }_j\hfill \end{array}$$

(69)

and, thus, $C^2\le AB$ turns to:

$$\sum _{i=0}^{2^n}{\alpha }_i^2{\beta }_i^2+\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i{\beta }_i{\alpha }_j{\beta }_j\le \sum _{i=0}^{2^n}{\alpha }_i^2{\beta }_i^2+\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2,$$

(70)

which is equivalent to:

$$\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i{\beta }_i{\alpha }_j{\beta }_j\le \sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2$$

(71)

which, in turn, is equivalent to:

$$\begin{array}{cc}\hfill 0& \le \sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2-\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i{\beta }_i{\alpha }_j{\beta }_j\hfill \\ \hfill & ={\displaystyle \frac{1}{2}}\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2+{\displaystyle \frac{1}{2}}\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i^2{\beta }_j^2-\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{\alpha }_i{\beta }_i{\alpha }_j{\beta }_j\hfill \\ \hfill & ={\displaystyle \frac{1}{2}}\sum _{i=0}^{2^n}\sum _{\begin{array}{c}j=0\\ j\ne i\end{array}}^{2^n}{({\alpha }_i{\beta }_j-{\alpha }_j{\beta }_i)}^2.\hfill \end{array}$$

(72)

Since this last inequality is always true, the triangle inequality is also true.

□

Now, a distance function D can be defined for rotors.

Theorem 7.

The function $D:\mathfrak{R}\times \mathfrak{R}\to {\mathbb{R}}^{+}$ defined by the identity $D(R_1,R_2)=\parallel R_1-R_2\parallel$ is a distance in $\mathfrak{R}$, i.e.,

(i) 

$D(R_1,R_2)\ge 0$ for all $R_1,R_2\in \mathfrak{R}$. In particular, $D(R_1,R_2)=0$ if, and only if, $R_1=R_2$;

(ii) 

$D(R_1,R_2)=D(R_2,R_1)$ for all $R_1,R_2\in \mathfrak{R}$;

(iii) 

$D(R_1,R_3)\le D(R_1,R_2)+D(R_2,R_3)$ for all $R_1,R_2,R_3\in \mathfrak{R}$.

Proof. 

The proof is straightforward and uses the fact that $\parallel ·\parallel$ is a norm. Given two different rotors $R_1$ and $R_2$:

(i)

$D(R_1,R_2)=\parallel R_1-R_2\parallel \ge 0$. In particular:

$$D(R_1,R_2)=0⟺\parallel R_1-R_2\parallel =0\stackrel{\left(1\right)}{⟺}{R}_1-R_2=0⟺R_1=R_2,$$

(73)

where $\left(1\right)$ uses the first property of a norm.

(ii)

We have that:

$$\begin{array}{cc}\hfill D(R_1,R_2)& =\parallel R_1-R_2\parallel =\sqrt{{〈\left(R_1-R_2\right){\left(R_1-R_2\right)}^{\sim }〉}_0}\hfill \\ \hfill & =\sqrt{{〈\left(R_2-R_1\right){\left(R_2-R_1\right)}^{\sim }〉}_0}=\parallel R_2-R_1\parallel =D(R_2,R_1).\hfill \end{array}$$

(74)

(iii)

Given a third rotor $R_3$, we have that:

$$\begin{array}{cc}\hfill D(R_1,R_3)& =\parallel R_1-R_3\parallel =\parallel R_1-R_2+R_2-R_3\parallel \hfill \\ \hfill & \stackrel{\left(1\right)}{\le }\parallel R_1-R_2\parallel +\parallel R_2-R_3\parallel =D(R_1,R_2)+D(R_2,R_3),\hfill \end{array}$$

(75)

where $\left(1\right)$ uses the third property of a norm.

□

As stated before, the end-effector pose of a serial robot and the pose of each one of its joints are described by the configuration-dependent rotors $R\left(\mathit{q}\right)$ and $R_i\left(\mathit{q}\right)$, respectively. Thus, one can be tempted to extend the distance function D to $\mathcal{C}$ as follows:

$$\begin{array}{cc}\hfill & D:\mathcal{C}\times \mathcal{C}\to {\mathbb{R}}^{+}\hfill \\ \hfill & D({\mathit{q}}_1,{\mathit{q}}_2)=\parallel R\left({\mathit{q}}_1\right)-R\left({\mathit{q}}_2\right)\parallel \hfill \end{array}$$

(76)

This function verifies all the requirements of a distance function with the exception of:

$$D({\mathit{q}}_1,{\mathit{q}}_2)=0⟺{\mathit{q}}_1={\mathit{q}}_2.$$

(77)

The reason is simple: a given pose of the end-effector can be associated with up to 16 different configurations if the serial robot is non-redundant and an infinite number if it is redundant. In particular, this means that $R\left({\mathit{q}}_1\right)=R\left({\mathit{q}}_2\right)$ with ${\mathit{q}}_1\ne {\mathit{q}}_2$. However, this problem can be overcome as follows:

• For each joint i, denote by ${\mathcal{C}}_i$ the configuration space of the subchain formed by the first i joints. It is clear that, if the robot has n degrees of freedom, ${\mathcal{C}}_i\subset \mathcal{C}$ for every $1\le i\le n$. Then, the following set of functions can be defined:

  $$\begin{array}{cc}\hfill D_i:{\mathcal{C}}_i\times {\mathcal{C}}_i& \to {\mathbb{R}}^{+}\hfill \\ \hfill D_i({\mathit{q}}_1,{\mathit{q}}_2)& =\parallel R_i\left({\mathit{q}}_1\right)-R_i\left({\mathit{q}}_2\right)\parallel \hfill \end{array}$$

  (78)

  where, as stated before, $R_i$ is the rotor that describes the pose of joint i. Again, these functions are not distance functions for the same reason as D (Equation (76)) is not a distance function.
• The function:

  $$\begin{array}{cc}\hfill & D:\mathcal{C}\times \mathcal{C}\to [0,+\infty )\hfill \\ \hfill & D({\mathit{q}}_1,{\mathit{q}}_2)=D_1\left({\mathit{q}}_{1_1}{\mathit{q}}_{2_1}\right)+\dots +D_n({\mathit{q}}_{1_n},{\mathit{q}}_{2_n})\hfill \end{array}$$

  (79)

  where ${\mathit{q}}_{1_i}$ (${\mathit{q}}_{2_i}$) denotes the first i coordinates of the configuration vector ${\mathit{q}}_1$ (${\mathit{q}}_2$), defines a distance function in $\mathcal{C}$.

  Proof. 

  Since, for each $1\le i\le n$, $D_i$ satisfies the requirements $\left(ii\right)$ and $\left(iii\right)$ of a distance function, it is clear that D also satisfies them. In addition, $D_i({\mathit{q}}_{1_i},{\mathit{q}}_{2_i})\ge 0$ for each $1\le i\le n$ and ${\mathit{q}}_{1_i},{\mathit{q}}_{2_i}\in {\mathcal{C}}_i$. Therefore, $D({\mathit{q}}_1,{\mathit{q}}_2)\ge 0$ for arbitrary ${\mathit{q}}_1,{\mathit{q}}_2\in \mathcal{C}$. Finally, if $D({\mathit{q}}_1,{\mathit{q}}_2)=0$, then, since any term of Equation (79) is a positive scalar, it can be deduced that $D_i({\mathit{q}}_{{\mathbf{1}}_{\mathit{i}}},{\mathit{q}}_{2_i})=0$ for every $1\le i\le n$. Thus, ${\mathit{q}}_1$ and ${\mathit{q}}_2$ not only have the same end-effector pose, but the same pose for of each of its joints, which clearly implies that ${\mathit{q}}_1={\mathit{q}}_2$. □

This distance function can be restricted to $\mathcal{S}$ just by considering the joints involved in a given singularity ${\mathit{q}}_s$.

Definition 5.

Let ${\mathit{q}}_s\in \mathcal{S}$ be a singularity of a serial robot that involves joints $i_1,\dots ,i_r$. Then, the function $D:\mathcal{C}\times \mathcal{S}\to {\mathbb{R}}^{+}$ is defined by the expression:

$$D(\mathit{q},{\mathit{q}}_s)=D_{i_1}({\mathit{q}}_{i_1},{\mathit{q}}_{s_{i_1}})+\dots +D_{i_r}({\mathit{q}}_{i_r},{\mathit{q}}_{s_{i_r}}),$$

(80)

where, for each $i_1\le k\le i_r$, $D_k$ is the function defined in (78) and is a distance function in $\mathcal{C}$.

5. Application to the Serial Robot Kuka Lwr 4+

To show the advantages of the proposed method, an illustrative example is developed in this section, making use of the Kuka LWR 4+, an anthropomorphic robotic arm with seven degrees of freedom and a spherical wrist. It is schematically depicted in Figure 4. Since it has a spherical wrist, its singularities can be decoupled into position and orientation singularities. Hence, Theorem 6 can be applied in order to find out such singularities. The computations with the vectors of ${\mathcal{G}}_3$ have been carried out using the Clifford Multivector Toolbox of MATLAB [45].

With respect to the position singularities, the following system of $C(4,3)=4$ equations should be solved:

$$\left.\begin{array}{cc}\hfill ({\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1))\wedge ({\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2))\wedge ({\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3))& =0\hfill \\ \hfill ({\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1))\wedge ({\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2))\wedge ({\mathit{z}}_4\times ({\mathit{o}}_7-{\mathit{o}}_4))& =0\hfill \\ \hfill ({\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1))\wedge ({\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3))\wedge ({\mathit{z}}_4\times ({\mathit{o}}_7-{\mathit{o}}_4))& =0\hfill \\ \hfill ({\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2))\wedge ({\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3))\wedge ({\mathit{z}}_4\times ({\mathit{o}}_7-{\mathit{o}}_4))& =0\hfill \end{array} \right\}$$

(81)

where, as computed in [46], we have that:

$$\begin{array}{cc}\hfill & {\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1)\hfill \\ \hfill & =\left[\begin{array}{c}-400{\mathrm{c}}_2{\mathrm{s}}_1-390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)-390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1\\ 400{\mathrm{c}}_1{\mathrm{c}}_2-390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)+390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4\\ 0\end{array}\right],\hfill \\ \\ \hfill & {\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2)\hfill \\ \hfill & =\left[\begin{array}{c}-{\mathrm{c}}_1(400{\mathrm{s}}_2+390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)\\ -{\mathrm{s}}_1(400{\mathrm{s}}_2+390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)\\ A_1\end{array}\right],\hfill \\ \\ \hfill & {\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3)\hfill \\ \hfill & =\left[\begin{array}{c}{\mathrm{c}}_2{\mathrm{s}}_1(390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)-{\mathrm{s}}_2(390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)+390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1)\\ -{\mathrm{s}}_2(390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)-390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4)-{\mathrm{c}}_1{\mathrm{c}}_2(390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)\\ {\mathrm{c}}_1{\mathrm{c}}_2(390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)+390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1)+{\mathrm{c}}_2{\mathrm{s}}_1(390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)-390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4)\end{array}\right],\hfill \\ \\ \hfill & {\mathit{z}}_4\times ({\mathit{o}}_7-{\mathit{o}}_4)\hfill \\ \hfill & =\left[\begin{array}{c}(390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)({\mathrm{c}}_1{\mathrm{c}}_3-{\mathrm{s}}_1{\mathrm{s}}_2{\mathrm{s}}_3)-{\mathrm{c}}_2{\mathrm{s}}_3(390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)+390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1)\\ (390{\mathrm{c}}_4{\mathrm{s}}_2-390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4)({\mathrm{c}}_3{\mathrm{s}}_1+{\mathrm{c}}_1{\mathrm{s}}_2{\mathrm{s}}_3)-{\mathrm{c}}_2{\mathrm{s}}_3(390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)-390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4)\\ A_2\end{array}\right],\hfill \end{array}$$

(82)

where

$$\begin{array}{cc}\hfill A_1& ={\mathrm{c}}_1(400{\mathrm{c}}_1{\mathrm{c}}_2-390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)+390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4)+{\mathrm{s}}_1(400{\mathrm{c}}_2{\mathrm{s}}_1\hfill \\ \hfill & +390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)+390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1),\hfill \\ \hfill A_2& =({\mathrm{c}}_1{\mathrm{c}}_3-{\mathrm{s}}_1{\mathrm{s}}_2{\mathrm{s}}_3)(390{\mathrm{s}}_4({\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_1{\mathrm{c}}_3{\mathrm{s}}_2)-390{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4)-(390{\mathrm{s}}_4({\mathrm{c}}_1{\mathrm{s}}_3+{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2)\hfill \\ \hfill & +390{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1)({\mathrm{c}}_3{\mathrm{s}}_1+{\mathrm{c}}_1{\mathrm{s}}_2{\mathrm{s}}_3),\hfill \end{array}$$

and where ${\mathrm{c}}_i=cos\left({\theta }_i\right)$ and ${\mathrm{s}}_i=sin\left({\theta }_i\right)$. However, in order to simplify these expressions, the system of Equation (81) is expressed with respect to the frame attached to the fourth joint of the Kuka LWR 4+. To do so, a relation analogous of relation (9) is applied. Here, instead of pre-multiplying by the corresponding rotation matrix, the system of Equation (81) is multiplied by the three-dimensional rotor R that performs the rotation between the frame attached to the end-effector and the frame attached to the fourth joint. For instance, the first equation of the system (81) becomes:

$$R({\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1))\wedge ({\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2))\wedge ({\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3))\tilde{R}=0,$$

(83)

which, using the geometric covariance property for rotors introduced in Section 2, becomes:

$$R({\mathit{z}}_1\times ({\mathit{o}}_7-{\mathit{o}}_1))\tilde{R}\wedge R({\mathit{z}}_2\times ({\mathit{o}}_7-{\mathit{o}}_2))\tilde{R}\wedge R({\mathit{z}}_3\times ({\mathit{o}}_7-{\mathit{o}}_3))\tilde{R}=0.$$

(84)

Therefore, the system of Equation (81) becomes:

$$\left.\begin{array}{cc}\hfill a_1\wedge a_2\wedge a_3& =0\hfill \\ \hfill a_1\wedge a_2\wedge a_4& =0\hfill \\ \hfill a_1\wedge a_3\wedge a_4& =0\hfill \\ \hfill a_2\wedge a_3\wedge a_4& =0\hfill \end{array} \right\}$$

(85)

where

$$\begin{array}{cc}\hfill a_1& =(-10{\mathrm{c}}_2{\mathrm{s}}_3(40{\mathrm{c}}_4+39))e_1+\left(400{\mathrm{c}}_2{\mathrm{s}}_3{\mathrm{s}}_4\right)e_2+(400{\mathrm{c}}_2{\mathrm{c}}_3+390{\mathrm{s}}_2{\mathrm{s}}_4+390{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{c}}_4)e_3,\hfill \\ \hfill a_2& =\left(10{\mathrm{c}}_3(40{\mathrm{c}}_4+39)\right)e_1+(-400{\mathrm{c}}_3{\mathrm{s}}_4)e_2+\left(10{\mathrm{s}}_3(40{\mathrm{c}}_4+39)\right)e_3,\hfill \\ \hfill a_3& =\left(390{\mathrm{s}}_4\right)e_3,\hfill \\ \hfill a_4& =(-390)e_1.\hfill \end{array}$$

(86)

Now, the system of Equation (85) becomes:

$$\left.\begin{array}{cc}\hfill 0& =0\hfill \\ \hfill 40{\mathrm{c}}_2{\mathrm{s}}_4+39{\mathrm{s}}_2{\mathrm{c}}_3{\mathrm{s}}_4^2+39{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_4& =0 \hfill \\ \hfill {\mathrm{c}}_2{\mathrm{s}}_3{\mathrm{s}}_4^2& =0\hfill \\ \hfill {\mathrm{c}}_3{\mathrm{s}}_4^2& =0\hfill \end{array}\right\}$$

(87)

which clearly has two different solutions:

• ${\mathrm{s}}_4=0$ or, equivalently, $q_4=0$.
• ${\mathrm{c}}_2={\mathrm{c}}_3=0$ or, equivalently, $q_2=\pm \frac{\pi }{2}$ and $q_3=\pm \frac{\pi }{2}$.

These two solutions correspond to the position singularities of the Kuka LWR 4+.

With respect to the orientation singularities, there is only one equation to solve:

$${\mathit{z}}_5\wedge {\mathit{z}}_6\wedge {\mathit{z}}_7=0.$$

(88)

Again, the expression of each ${\mathit{z}}_i$ for $i=5,6,7$ can be simplified by expressing those vectors with respect to the frame attached to the fourth joint. Thus, Equation (88) becomes:

$$\begin{array}{cc}\hfill & e_2\wedge (-{\mathrm{s}}_5{e}_1-{\mathrm{c}}_5{e}_3)\wedge ({\mathrm{c}}_5{\mathrm{s}}_6{e}_1+{\mathrm{c}}_6{e}_2-{\mathrm{s}}_5{\mathrm{s}}_6{e}_3)\hfill \\ \hfill & =(-{\mathrm{s}}_5{e}_2\wedge e_1-{\mathrm{c}}_5{e}_2\wedge e_3)\wedge ({\mathrm{c}}_5{\mathrm{s}}_6{e}_1+{\mathrm{c}}_6{e}_2-{\mathrm{s}}_5{\mathrm{s}}_6{e}_3)\hfill \\ \hfill & \stackrel{\left(1\right)}{=}-{\mathrm{s}}_5^2{\mathrm{s}}_6{e}_1\wedge e_2\wedge e_3-{\mathrm{c}}_5^2{\mathrm{s}}_6{e}_1\wedge e_2\wedge e_3=-{\mathrm{s}}_6{e}_1\wedge e_2\wedge e_3=0,\hfill \end{array}$$

(89)

where $\left(1\right)$ uses the anticommutativity of the outer product. Clearly, the last expression of Equation (89) is zero if, and only if, ${\mathrm{s}}_6=0$ or, equivalently, if, and only if, $q_6=0$. Thus, the Kuka LWR 4+ only has one orientation singularity (the wrist singularity, as explained in Remark 7).

Finally, the distance function defined in Definition 5 can be applied to any of the already obtained singular configurations. Let us consider, for instance, the position singularity $q_4=0$. Then, the distance between an arbitrary configuration $\mathit{q}\in \mathcal{C}$ and this singularity is given by the expression:

$$D(\mathit{q},{\mathit{q}}_s)=\parallel R_4\left(\mathit{q}\right)-R_4\left({\mathit{q}}_s\right)\parallel ,$$

(90)

where ${\mathit{q}}_s$ denotes the singular configuration $q_4=0$ and $R_4$ is the rotor defining the pose of the fourth joint of the Kuka LWR 4+.

In particular, $R_4$ can be found as explained in Section 2. Indeed, if $\{e_1,e_2,e_3\}$ denotes the orthogonal basis defined by the world frame and $\{f_1,f_2,f_3\}$ (respectively, $\{f_1^{\prime },f_2^{\prime },f_3^{\prime }\}$), the orthogonal basis defined by the frame attached to the fourth joint under the effect of configuration $\mathit{q}$ (respectively, singular configuration ${\mathit{q}}_s$), then:

$$\begin{array}{cc}\hfill R_4\left(\mathit{q}\right)& ={\displaystyle \frac{1+e^1{f}_1+e^2{f}_2+e^3{f}_3}{\parallel 1+e^1{f}_1+e^2{f}_2+e^3{f}_3\parallel }},\hfill \\ \\ \hfill R_4\left({\mathit{q}}_s\right)& ={\displaystyle \frac{1+e^1{f}_1^{\prime }+e^2{f}_2^{\prime }+e^3{f}_3^{\prime }}{\parallel 1+e^1{f}_1^{\prime }+e^2{f}_2^{\prime }+e^3{f}_3^{\prime }\parallel }},\hfill \end{array}$$

(91)

where $\{e^1,e^2,e^3\}$ is the reciprocal frame of $\{e_1,e_2,e_3\}$. Since $\{e_1,e_2,e_3\}$ is also an orthonormal set of vectors, such a reciprocal frame is:

$$\begin{array}{cc}\hfill e^1& =e_1,\hfill \\ \hfill e^2& =e_2,\hfill \\ \hfill e^3& =e_3.\hfill \end{array}$$

(92)

Thus, Equation (91) turns to:

$$\begin{array}{cc}\hfill R_4\left(\mathit{q}\right)& ={\displaystyle \frac{1+e_1{f}_1+e_2{f}_2+e_3{f}_3}{\parallel 1+e_1{f}_1+e_2{f}_2+e_3{f}_3\parallel }},\hfill \\ \\ \hfill R_4\left({\mathit{q}}_s\right)& ={\displaystyle \frac{1+e_1{f}_1^{\prime }+e_2{f}_2^{\prime }+e_3{f}_3^{\prime }}{\parallel 1+e_1{f}_1^{\prime }+e_2{f}_2^{\prime }+e_3{f}_3^{\prime }\parallel }}.\hfill \end{array}$$

(93)

Evaluating Equation (93) for the KUKA LWR 4+, we obtain:

$$\begin{array}{cc}\hfill R_4\left(\mathit{q}\right)& ={\displaystyle \frac{a_1+a_2{e}_1\wedge e_2+a_3{e}_1\wedge e_3+a_4{e}_2\wedge e_3}{\sqrt{a_1^2+a_2^2+a_3^2+a_4^2}}},\hfill \\ \\ \hfill R_4\left({\mathit{q}}_s\right)& ={\displaystyle \frac{b_1+b_2{e}_1\wedge e_2+b_3{e}_1\wedge e_3+b_4{e}_2\wedge e_3}{\sqrt{b_1^2+b_2^2+b_3^2+b_4^2}}},\hfill \end{array}$$

(94)

where $\{e_1\wedge e_2,e_1\wedge e_3,e_2\wedge e_3\}$ are the basic bivectors of ${\mathcal{G}}_3$ and

$$\begin{array}{cc}\hfill a_1& ={\mathrm{c}}_2{\mathrm{s}}_3+{\mathrm{c}}_4{\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_4{\mathrm{c}}_3{\mathrm{c}}_1{\mathrm{s}}_2+{\mathrm{s}}_3{\mathrm{s}}_4{\mathrm{c}}_1+{\mathrm{s}}_4{\mathrm{s}}_1{\mathrm{s}}_2{\mathrm{c}}_3+{\mathrm{s}}_4{\mathrm{c}}_1{\mathrm{c}}_2+{\mathrm{c}}_2{\mathrm{c}}_4{\mathrm{s}}_1,\hfill \\ \hfill a_2& ={\mathrm{c}}_2{\mathrm{s}}_1{\mathrm{s}}_4-{\mathrm{c}}_4{\mathrm{c}}_1{\mathrm{s}}_3-{\mathrm{c}}_4{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2-{\mathrm{c}}_1{\mathrm{c}}_2{\mathrm{c}}_4+{\mathrm{s}}_4{\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{s}}_4{\mathrm{s}}_2{\mathrm{c}}_1{\mathrm{c}}_3,\hfill \\ \hfill a_3& ={\mathrm{s}}_2{\mathrm{s}}_4+{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{c}}_4+{\mathrm{c}}_3{\mathrm{s}}_1+{\mathrm{c}}_1{\mathrm{s}}_2{\mathrm{s}}_3,\hfill \\ \hfill a_4& ={\mathrm{c}}_4{\mathrm{s}}_2-{\mathrm{c}}_2{\mathrm{c}}_3{\mathrm{s}}_4-{\mathrm{c}}_1{\mathrm{c}}_3+{\mathrm{s}}_1{\mathrm{s}}_2{\mathrm{s}}_3,\hfill \\ \hfill b_1& ={\mathrm{c}}_2{\mathrm{s}}_3+{\mathrm{s}}_1{\mathrm{s}}_3-{\mathrm{c}}_3{\mathrm{c}}_1{\mathrm{s}}_2+{\mathrm{c}}_2{\mathrm{s}}_1,\hfill \\ \hfill b_2& =-{\mathrm{c}}_1{\mathrm{s}}_3-{\mathrm{c}}_3{\mathrm{s}}_1{\mathrm{s}}_2-{\mathrm{c}}_1{\mathrm{c}}_2,\hfill \\ \hfill b_3& ={\mathrm{c}}_2{\mathrm{c}}_3+{\mathrm{c}}_3{\mathrm{s}}_1+{\mathrm{c}}_1{\mathrm{s}}_2{\mathrm{s}}_3,\hfill \\ \hfill b_4& ={\mathrm{s}}_2-{\mathrm{c}}_1{\mathrm{c}}_3+{\mathrm{s}}_1{\mathrm{s}}_2{\mathrm{s}}_3.\hfill \end{array}$$

(95)

Therefore, by Proposition 4 and the decomposition used in the Proof of Lemma 1, the distance of an arbitrary configuration $\mathit{q}$ to the position singularity $q_4=0$ is given by:

$$D(\mathit{q},{\mathit{q}}_s)=\sqrt{{(a_1^{\prime }-b_1^{\prime })}^2+{(a_2^{\prime }-b_2^{\prime })}^2+{(a_3^{\prime }-b_3^{\prime })}^2+{(a_4^{\prime }-b_4^{\prime })}^2},$$

(96)

where

$$a_i^{\prime }={\displaystyle \frac{a_i}{\sqrt{a_1^2+a_2^2+a_3^2+a_4^2}}} \mathrm{and} b_i^{\prime }={\displaystyle \frac{b_i}{\sqrt{b_1^2+b_2^2+b_3^2+b_4^2}}}.$$

(97)

6. Handling of Singularities

Once the set of singular configurations $\mathcal{S}$ has been identify, several methods can be applied to handle the singularities. The detailed treatment of this topic is beyond the scope of this work. However, in order to show the possibilities of the distance function proposed in Section 4, we comment on three different situations, namely, motion planning, motion control and bilateral teleoperation. In each one of these situations, the distance function defined in Definition 5 plays an important role for handling the singularities.

6.1. Singularity Handling in Motion Planning

Motion planning consists of programming collision-free motions for a given robotic manipulator from a start position to a goal position among a collection of static obstacles. The subset of robot configurations that do not cause collision with such obstacles is termed the free-of-obstacles configuration space, and it is denoted by ${\mathcal{C}}_{\mathrm{free}}$. The main methods used for motion planning can be grouped into three categories:

• Potential field methods, where a differentiable real-valued function $U:\mathcal{C}\to \mathbb{R}$, called the potential function, is defined. Such a function has an attractive component that pulls the trajectory towards the goal configuration and a repulsive component that pushes the trajectory away from the start configuration and from the obstacles.
• Sampling-based multi-query methods, where a roadmap is constructed over ${\mathcal{C}}_{\mathrm{free}}$. The nodes represent free-of-obstacles configurations, while the edges represent feasible local paths between those configurations. Once the roadmap is constructed, a search algorithm finds out the best solution trajectory by selecting and joining the local paths through an optimization process.
• Sampling-based single-query methods, where tree-structure data are constructed by searching new configurations (nodes) in ${\mathcal{C}}_{\mathrm{free}}$ and connecting them through local paths (edges). Its main difference with respect to the multi-query methods is that, while the multi-query methods work in two steps (construction of the roadmap and searching of a solution trajectory), in the single-query methods, both steps are taken together. Each new configuration added to the set of nodes is connected by a local path and evaluated in order to check its feasibility.

For any method of these three categories, the distance function D defined in Definition 5 can be applied to construct solution trajectories that also avoid the singularities. Indeed:

• For a potential field method, it is sufficient to add a repulsive component that pushes the trajectory, not only away from obstacles, but also away from singularities. To do so, the most efficient method is to define, for each singularity ${\mathit{q}}_s$, a quadratic repulsive component as follows:

  $$U_{r,{\mathit{q}}_s}\left(\mathit{q}\right)=\left\{\begin{array}{cc}{\displaystyle \frac{\kappa }{2}}{\left({\displaystyle \frac{1}{D(\mathit{q},{\mathit{q}}_s)}}-{\displaystyle \frac{1}{D_0}}\right)}^2\hfill & \mathrm{if} D(\mathit{q},{\mathit{q}}_s)\le D_0\hfill \\ 0\hfill & \mathrm{if} D(\mathit{q},{\mathit{q}}_s)>D_0\hfill \end{array}\right.$$

  (98)

  where $D_0$ is set as a threshold for the distance D and $\kappa \in \mathbb{R}$.
• For a sampling-based method with multiple queries, it is sufficient to remove from the roadmap those nodes associated with singular configurations. During the construction of the roadmap, each configuration $\mathit{q}\in \mathcal{C}$ is evaluated to determine whether $\mathit{q}$ is free-of-obstacles or not. Similarly, the idea is to evaluate each $\mathit{q}\in \mathcal{C}$ in order to determine whether $\mathit{q}$ is close to a singularity or not. To speed up the process, both evaluations can be carried out together:

  (1)

  Select a value $D_0>0$ that will work as a threshold.

  (2)

  Given a discretization of the configuration space $\mathcal{C}$, each $\mathit{q}$ of this discretization is evaluated to check whether:

  *

  It is free-of-obstacles;

  *

  It is far from any singularity. This can be achieved simply by evaluating whether $D(\mathit{q},{\mathit{q}}_s)>D_0$ or $D(\mathit{q},{\mathit{q}}_s)\le D_0$;

  (3)

  If $\mathit{q}$ is free-of-obstacles and far from any singularity, then it can be added to the set of nodes of the roadmap.

• For a sampling-based method with a single query, the approach is completely analogous to the one used for methods with multiple-queries due to the similarities between both categories.

6.2. Singularity Handling in Motion Control

Motion control consists of making the end-effector of a robot follow a time-varying trajectory specified within the manipulator workspace. A typical Inverse Dynamics Control scheme (depicted as a block diagram in Figure 5) can be described as:

• An input, i.e., the desired or target configuration ${\mathit{q}}_d$ together with its velocity ${\dot{\mathit{q}}}_d$;
• A controller based on the dynamical model of the robot:

  $$\mathit{\tau }=M\left(\mathit{q}\right)\ddot{\mathit{q}}+C(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+g\left(\mathit{q}\right)$$

  (99)

  where $M\left(\mathit{q}\right)$ denotes the inertia matrix of the robot, $C(\mathit{q},\dot{\mathit{q}})$ denotes the matrix of Coriolis and centrifugal forces and $g\left(\mathit{q}\right)$ denotes the gravity vector.
• An output, i.e., the vector of torques $\mathit{\tau }$, that is sent to the robot to perform the desired motion;
• The robot executes the motion and updates the vectors $\mathit{q}$ and $\dot{\mathit{q}}$;
• The robot sends such updated vectors to the controller (also known as the feedback of the system).

To handle the singularities, a restriction can be defined inside the controller:

(1)

The target configuration ${\mathit{q}}_d$ is entered into the controller.

(2)

${\mathit{q}}_d$ is checked in order to determine whether it is close to a singularity or not:

–

Select a threshold value $D_0>0$.

–

Evaluate the condition $D({\mathit{q}}_d,{\mathit{q}}_s)>D_0$ for each singularity ${\mathit{q}}_s$.

–

If the evaluation returns yes, then $\mathit{\tau }$ can be computed from ${\mathit{q}}_d$ using the dynamical model (Equation (99)) and sent to the robot. Otherwise, ${\mathit{q}}_d$ is substituted by ${\mathit{q}}_d+D_0{\mathit{q}}_d$ and evaluated again.

A block diagram of this scheme is depicted in Figure 6.

6.3. Singularity Handling in Bilateral Teleoperation

Teleoperated robotic systems are characterized by a robot that executes the movements/actions commanded by a human operator. Any high-level or planning decision is made by a human user, while the robot is responsible for their mechanical implementation [47]. Teleoperation systems are often, at least conceptually, split into two parts: a local manipulator and a remote manipulator. The first one refers to the device moved by the human operator, while the second refers to the robot or robot system that performs the action.

According to the information flow direction, the teleoperation may be unilateral or bilateral. In unilateral teleoperation, the local manipulator sends position or force data to the remote manipulator and only receives, as feedback, visual information from the remote scene. However, in bilateral teleoperation, the position or force data are also sent from the remote manipulator in addition to the visual information.

In a bilateral teleoperation system, some strategies for handling of kinematic singularities can make use of the distance function D defined in Definition 5. For instance, the following scheme could be applied:

(1)

Select a value $D_0>0$ that will work as a threshold.

(2)

The local manipulator sends a pose (force) $\mathit{p}$ ($\mathit{f}$) to the remote manipulator.

(3)

The controller of the remote manipulator obtains the associated configuration $\mathit{q}$.

(4)

The distance $D(\mathit{q},{\mathit{q}}_s)$ is computed for each ${\mathit{q}}_s\in \mathcal{S}$.

(5)

If, for some ${\mathit{q}}_s$, $D(\mathit{q},{\mathit{q}}_s)<D_0$, then the remote manipulator controller computes a reaction force ${\mathit{f}}_s$ in the same direction of the motion but with inverse sense.

(6)

The remote manipulator sends this force ${\mathit{f}}_s$ to the local manipulator.

(7)

The human operator will not be able to move the local manipulator in such a direction implied by ${\mathit{f}}_s$ and, thus, the singularity ${\mathit{q}}_s$ will never be reached.

7. Conclusions

This paper proposes a novel singularity identification method for arbitrary serial robots based on the six-dimensional geometric algebra ${\mathcal{G}}_6$. For non-redundant serial robots, we take the six unit twists ${\xi }_1,\dots ,{\xi }_6$ associated with the joints, and we model them as vectors of ${\mathcal{G}}_6$. Hence, the problem reduces to find the configurations causing the exterior product ${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)$ to vanish since, as proven in Corollary 1, ${\xi }_1\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_6\left(\mathit{q}\right)=0$ if, and only if, $\mathit{q}\in \mathcal{S}$. Analogously, for a redundant robot with n DoF, we consider the $C(n,6)$ different combinations of six unit twists taken from $\{{\xi }_1,\dots ,{\xi }_n\}$, and we find the configurations causing all the exterior products of the form ${\xi }_{j_1}\left(\mathit{q}\right)\wedge \dots \wedge {\xi }_{j_6}\left(\mathit{q}\right)$ for $1\le j\le C(n,6)$ to vanish.

For serial robots with a spherical wrist, a simplification is possible. For these manipulators, the singularities are of two types: position singularities and orientation singularities. The former are identified as the configurations causing the exterior products ${\mathit{s}}_{i_1}\left(\mathit{q}\right)\wedge {\mathit{s}}_{i_2}\left(\mathit{q}\right)\wedge {\mathit{s}}_{i_3}\left(\mathit{q}\right)$ to vanish for $1\le i\le C(n-3,3)$, where ${\mathit{s}}_{i_j}$ is the linear velocity component of the unit twist ${\xi }_{i_j}$ and is modelled as a vector of ${\mathcal{G}}_3$, while the latter are identified as the configuration causing the exterior product ${\mathit{z}}_{n-2}\left(\mathit{q}\right)\wedge {\mathit{z}}_{n-1}\left(\mathit{q}\right)\wedge {\mathit{z}}_n\left(\mathit{q}\right)$ to vanish, where ${\mathit{z}}_i$ is the i-th joint axis and, again, is modelled as a vector of ${\mathcal{G}}_3$. Thus, the simplification consists of evaluating the exterior product of three vectors in ${\mathcal{G}}_3$ instead of six vectors in ${\mathcal{G}}_6$.

Once the singularities are identified, a distance function is defined such as its restriction to the singular set $\mathcal{S}$, defined in Definition 5, is also a distance function that allows us to check how far an arbitrary configuration $\mathit{q}$ is to a singularity. This distance function exploits the fact that between any two vectors $\mathit{x},\mathit{y}\in {\mathcal{G}}_n$, there always exists a rotor R such that $\mathit{y}=R\mathit{x}\tilde{R}$.

The advantages of the strategy introduced in this work are clear. First, it is a computer-friendly approach that avoids the computation of the determinant of an order $6\times n$ (for non-redundant robots) or $n\times n$ (for redundant robots) matrix and the Jacobian matrix J. In addition, the novel distance function defined in Definition 5 can be used to improve the performance of current control schemes or motion planning algorithms, which, as seen in the introduction, is still a hot research topic in robotics.