Inverse Kinematics for Industrial Robots using Conformal Geometric Algebra

This paper shows how the recently developed formulation of conformal geometric algebra can be used for analytic inverse kinematics of two six-link industrial manipulators with revolute joints. The paper demonstrates that the solution of the inverse kinematics in this framework relies on the intersection of geometric objects like lines, circles, planes and spheres, which provides the developer with valuable geometric intuition about the problem. It is believed that this will be very useful for new robot geometries and other mechanisms like cranes and topside drilling equipment. The paper extends previous results on inverse kinematics using conformal geometric algebra by providing consistent solutions for the joint angles for the different configurations depending on shoulder left or right, elbow up or down, and wrist flipped or not. Moreover, it is shown how to relate the solution to the Denavit-Hartenberg parameters of the robot. The solutions have been successfully implemented and tested extensively over the whole workspace of the manipulators.


Introduction
Analytical inverse kinematics is a well-developed problem in robotics.Solutions are available as text-book material for revolute robots with a spherical wrist, or with three consecutive parallel axes [Siciliano et al. (2009); Spong et al. (2006)].The solutions are given in terms of trigonometric expressions, which are straightforward to find, although they can be somewhat involved.The complexity of the equations is partly related to the book-keeping of the different solutions related to shoulder left or right, elbow up or down, and wrist flipped or not.
The recently developed formulation of conformal geometric algebra as presented in [Dorst et al. (2009); Hildenbrand (2013); Perwass (2009)] provides additional insight into the problem.This formulation has very efficient tools to define geometric objects in the form of lines, circles, planes and spheres, and includes the geometric product, which is used to calculate intersections of such geometric objects and the distance between different objects.The formulation extends the 3-dimensional Euclidean space with 2 extra dimensions resulting in a homogeneous space including the point at infinity.In this formalism, the inverse kinematics has been previously solved for a robot with 5 revolute joints in terms of spheres, planes and lines, and the intersection of these geometric objects [Hildenbrand (2013); Hildenbrand et al. (2005); Hildenbrand et al. (2006); Zamora and Bayro-Corrochano (2004)].These inverse kinematic solutions have primarily been developed for graphical rendering, as the focus has been on the link configurations, whereas the joint angles are only given in terms of the cosines of the angles, which means that there is no systematic way of determining the right quadrant of the joint angles.Still, this work clearly demonstrates that the conformal geometric algebra is a very powerful tool for inverse kinematics, which makes it interesting to explore this formulation more in detail to investigate how it can be employed to solve and implement a range of practical kinematic problems in robotics.To do this we revisit the well-established inverse kinematic problem for robots to demonstrate how conformal geometric algebra can be used in robotics.
In this work we extend the existing solutions for analytic inverse kinematics based on conformal geometric algebra to obtain a systematic way of calculating the signs and quadrants of the joint angles.This includes the calculation of consistent solutions corresponding to shoulder left and right, elbow up or down and wrist flipped or not.Moreover, it is shown how the rotational direction of the joint angles are related to the Denavit-Hartenberg parameters.It is also shown how to describe links that have both a and d translations in the Denavit-Hartenberg convention.The proposed method is implemented for the Agilus R900 Sixx robot, which is a 6 DOF robot with a spherical wrist, and the UR5, which is a 6 DOF robot with parallel axes for joints 2, 3 and 4. Also singularities are discussed, and it is explained how the singularities appear in the solution based on conformal geometric algebra.
The paper is organized as follows.First a brief presentation of manipulator kinematics is given.Then the basics of conformal geometric algebra is presented, which includes a discussion on how to determine the sign of rotation in this formulation.Then the implementation of the analytic inverse kinematics is presented for the Agilus R900 Sixx and the UR5 robot.

Manipulator kinematics
The Denavit-Hartenberg convention is commonly used for describing robot kinematics.The convention describes the link transformation in terms the homogeneous link transformation matrix This can be used to calculate the forward kinematics of a robot with six links as The Denavit-Hartenberg parameters for the Aguilus robot are presented in Table 1 and Figure 2b, while the Denavit-Hartenberg parameters for the UR5 robot are shown Table 2 and Figure 6b.

Conformal Geometric Algebra
In this paper conformal geometric algebra is used for the inverse kinematics of robots.The main difference to the usual geometric formulation used in robotics is the introduction of the geometric product, and the extension of the 3 dimensional Euclidean space with 2 additional dimensions.This provides us with some very efficient tools, in particular, the formulation makes it very simple to define geometric objects in the form of lines, planes, circles and spheres.In addition, it is easy to calculate the occurrence of intersections between the geometric objects, and the distance between objects.
The Euclidean space R 3 is described with the orthogonal unit vectors e 1 , e 2 , e 3 .The vectors a and b in Euclidean space are given by a = a 1 e 1 + a 2 e 2 + a 3 e 3 and b = b 1 e 1 + b 2 e 2 + b 3 e 3 .The geometric product is defined as which is a scalar, and is the outer product, which is a bivector, as it is the sum of terms including the bivectors e 2 e 3 , e 3 e 1 and e 1 e 2 .It is noted that a and that e i e j = e i • e j + e i ∧ e j = 1, i = j e i ∧ e j , i = j where e i e j = e i ∧e j = −e j ∧e i = −e j e i whenever i = j.It follows that a ∧ a = 0.

Multivectors
A multivector a in Euclidean space is a linear combination of the basis elements {1, e 1 , e 2 , e 3 , e 2 e 3 , e 3 e 1 , e 1 e 2 , e 1 e 2 e 3 } A multivector A in conformal space is a linear combination of the basis elements {1, e 0 , e 1 , e 2 , e 3 , e ∞ , e 0 e 1 , . . ., e 0 e 1 e 2 e 3 e ∞ } The geometric product of two multivectors A and B is given by The outer product has the property that A ∧ A = 0 for any multivector A.

Duals and the pseudoscalar
The pseudoscalar in the Euclidean space R 3 is I E = e 1 e 2 e 3 .The Euclidean dual of a multivector a is The square of the Euclidean pseudoscalar is I 2 E = −1, and it follows that the dual of the Euclidean dual is (a The conformal pseudoscalar is As in the Euclidean case, the square of the pseudoscalar is I 2 c = −1, and it follows that the dual of the dual is (A * ) * = −A.

Conformal representation of Euclidean objects
In this paper the representation of geometric objects and their duals is based on the formulation in [Dorst et al. (2009)].It is noted that an alternative formulation is presented in [Hildenbrand (2013)], where the direct form of [Dorst et al. (2009)] is presented as the dual form.The Euclidean point p is represented in conformal space by the multivector Starting from the representation of a point in conformal space the direct representation in conformal space of several Euclidean geometric objects can be generated with the outer product.
Let P A , P B and P C be the conformal representation of the points on a circle in Euclidean space.The direct representation of the circle in conformal space is then where P A and P B are the conformal representation of two points on the line.A sphere in Euclidean space has the direct conformal representation where P A , P B , P C and P D are conformal representations of points on the sphere that are not all in the same plane.A plane in Euclidean space has the direct conformal representation where P A , P B and P C are conformal representations of points on the plane that are not collinear.In addition, the points P A and P B constitute a point pair A sphere S has the dual form where P center point and ρ is the radius of the sphere in Euclidean space.A plane Π has the dual form where n is the normal vector of the plane in Euclidean space and d is the distance from the origin.

Intersections
The intersection or meet M of two geometric objects A and B represented in the direct form in conformal space is given in terms of the dual M * = A * ∧ B * , or, equivalently, in the direct form as

Distances
The distance between geometric objects is related to the inner product in some cases.The Euclidean distance d from a point P to a plane Π is given by the inner product in conformal space as d = −P •Π * where d is positive if the point is in the direction of the normal vector.The Euclidean distance d between two points represented by P A and P B is given by d 2 = −2P A •P B .Consider two points with conformal representations P 1 and P 2 .Suppose that the two points are connected with a link with a 90 • offset of length d.Then the offset can be located with the horizon technique presented in [Hildenbrand (2013)].First the dual sphere S * = P 2 − 1 2 d 2 e ∞ with center point P 2 and radius d is defined.Next, define the sphere K * = P 1 − (P 1 • S * )e ∞ with center in P 1 .Then the intersection of the spheres S and K will be the horizon defined by the circle

Horizon calculation
This circle is the set of all points with a 90 • offset of length d.The intersection of this circle with a plane Π that contains both P 1 and P 2 will give a point pair Q = C * • Π where the two points of the point pair are on the tangent line from the point P 1 with an offset d from P 2 .An example of this can be seen in Figure 1.

Calculation of angles
In this section it is shown how to calculate the angle of rotation between two vectors a and b, and how to define the sign of the angle according to a defined direction of rotation.The corresponding unit vectors are given by â = a/ a and b = b/ b .The geometric product of â and b is The inner product of the two Euclidean unit vectors â and b is the usual scalar product, which means that where θ is the angle between the vectors.The outer product is where is a unit bivector that defines the plane of rotation from â to b.The plus applies if the rotation from â to b is counter-clockwise in the plane defined by N , while the minus applies if the rotation in clockwise.
Equations 13 and 14 give the following expressions for the sine and cosine of the angle: where is the inverse of N , which is equal to the reverse bivector.
It follows that the angle θ can be computed from This approach ensures that the angle is calculated with the right sign.
In the inverse kinematics problem the two vectors a and b will typically be directional vectors of a line, or the normal vector of a plane.The directional vector of a line L is computed from while the normal vector of a plane Π is computed from The rotation plane perpendicular to a line L is found from while the rotation plane parallel to a plane Π can be calculated from Note that the sign of the rotation plane N for a robot joint must be selected so that the sign of the rotation is correct.This will be the case if the rotation axis z of the Denavit-Hartenberg convention is the Euclidean dual of the rotation plane, that is, 4 Inverse Kinematics of the Agilus sixx R900 robot The input parameters to the inverse kinematics are the position vector p e , the approach vector a e , the slide vector s e and the normal vector n e of the end-effector.
Then the conformal representations of p e and the wrist position p e + d 6 a e are given by where d 6 is the distance between the end-effector and the wrist, which for the Agilus is 80 mm, as shown in Table 1.
The vertical plane Π c , which is the cross section of the robot through the wrist point, is then defined by We define three configurations: Front/Back, which defines if it is the front or back of the robot that faces the end-effector; Elbow up/Elbow down, which defines if the elbow joint is up or down; and Flip/No Flip, which defines if the wrist joint is flipped or not.
These configurations are selected with the following parameters: 4.1 Finding P 1 The position of joint 1 is represented by P 1 .The Denavit-Hartenberg parameters for link 1 has non-zero a and d parameters, which means that there is an offset from the rotational axis of joint 1, which is seen in Figure 2b.This point is on the point pair Q 1 that is found by intersecting a sphere with two planes as follows: This point pair consists of the two possible solutions for P 1 .One solution corresponds to robot facing towards the end-effector, while the other corresponds to the robot facing away from the end-effector.The solution for P 1 is selected according to Figure 6 shows the geometric objects in Equation 29 and the selected P 1 .
4.2 Finding P 2 P 2 will be on the circle C 2 , which is the intersection of the two spheres where S 1 has center point P 1 , and S w is centered in P w .Then the intersection of C 2 with the vertical plane Π c will give a point pair Q 2 , according to This is shown in Figure 4.The points in Q 2 are the two possible solutions for P 2 , and the solution is selected depending on the parameter elbow up or elbow down, and is given by Both configurations are shown in Figure 4a    The red sphere is S 0 , the blue plane is Π 1x , the green circle is generated from S 0 ∧ Π c , and the red point pair is Q 1 , where one is picked to be P 1 .
(a) The robot and the geometric objects used to find P 2 .The robot is configured with elbow up (b) The robot and the geometric objects used to find P 2 .
The robot is configured with elbow down Figure 4: The red spheres are S 1 and S w , the yellow plane is Π 1x , the green circle is generated from C 2 , and the blue point pair is Q 2 , where one is picked to be P 2 .
Figure 5: The robot and the geometric objects used to find P wx .The red spheres are S 2 and S w , the yellow plane is Π 1x , the green circle is generated from C wx , and the blue point pair is Q wx , where one is picked to be P wx .

Calculating the remaining kinematics
The Agilus has an offset a 3 from the joint position P 2 to the offset point P wx , as shown in Table 1.The point P wx is found using the horizon technique from Section 3.6, which gives Here the solutions for P wx are the points of the point pair Q wx , and the solution is selected according to the arm geometry from the calculations where Equation 35 and P wx are shown in in Figure 5.

Finding the joint angles
The link configurations have now been determined from the end-effector configuration, and as remarked by [Dorst et al. (2009)], this is sufficient for graphical rendering.The next step is to determine the joint angles.In previous works this has typically been done by calculating the cosine of the joint angle.This will not give consistent signs for the joint angles corresponding to the different solution for the arm.This problem is solved here, and it is shown how to determine the quadrant of the angle, and also to keep track of the different solutions.
To do this it is necessary to define the rotation plane of each joint, and the vectors defining the rotation of the joint.The point P 1x = C(d 1 e 3 ) and the following lines are defined: The rotation plane of θ 1 is Nθ1 = e 1 ∧e 2 , which is the horizontal base plane, while the rotation plane for θ 2 and θ 3 is found from the Π c using Equation 21.Next, the rotation plane for θ 4 it is found from L wxw using Equation 20, while for θ 5 the rotation plane is parallel to the plane L wxw ∧ P e , and its rotation depends on if it is flipped, i.e. k f n .Finally, −a + e is the rotation plane for θ 6 .The joint angles can then be found from Equation 17 using the parameters given in Table 3.

Singularities for the Agilus
There are two singularities in the given model, which correspond to the physical singularities of the robot.
In the wrist singularity, P e will be on the line L wxw .Then the the rotation plane N −1 θ5 becomes undefined since L wxw ∧ P e = 0.
In the shoulder singularity the point P w will be on the vertical line defined by e 3 , and the plane Π c becomes undefined since e 0 ∧ e 3 ∧ P w ∧ e ∞ = 0.

Inverse Kinematics for the UR5
The input to the inverse kinematics of the UR5 robot is p e , n e , s e and a e as for the Agilus robot.The conformal representation of the end effector position and the position of joint 5 is found from The configuration parameters are defined as First the vertical plane Π c through joints 1, 2, 3 and 4 is found.This is done by finding the point P c with an offset d 4 from P 5 .The calculation is done with the horizon technique to find the circle C 5k according to Then the point pair Q c with the two solutions for P c is found by intersection C 5k with the horizontal plane through P 5 : The solution for P c is selected depending on the parameter for shoulder right or shoulder left using When the solution for P c has been selected the vertical plane Π c is found from Figure 7 shows the geometric objects in Equation 43 and the point pair Q c .
5.1 Finding P 3 and P 4     The plane Π c is defined as the plane that is parallel to Π c and that contains the points P 4 and P 5 .This plane is found in the dual form with a distance P 5 • Π c from Π c according to The next step is to calculate the line through P 4 and P 5 from where Π c⊥ is a plane containing P 4 and P 5 and which normal is perpendicular to the normal of Π c .It is noted that n56⊥ = a + e = s e ∧ n e .The solutions for P 4 are then given by the point pair Q 4 , which is the intersection of the line L 45 and the sphere S 5 with center point in P 5 and radius d 5 .This is calculated from Next, the solutions for P 3 are given by the point pair Q 3 , which is the intersection of the line L 34 and the sphere S 4 with center point in P 4 and radius d 4 .This is calculated from 5.2 Finding P 1 and P 2 P 1 is computed from The solutions for the point P 2 are then given by the point pair Q 2 , which is found as the intersection of the two spheres S 1 and S 3 and the vertical plane Π c , which is calculated from The solution is selected depending on the the parameter for elbow up or down according to

Finding the joint angles
Expression for calculating the configuration have now been established.The next step is to find expressions for the calculation of the joint angles using Equation 17.
The following lines are defined The rotation planes for θ 1 and θ 6 of the UR5 are the same as for the Agilus: e 1 ∧ e 2 and −a + e respectively.The angles θ 2 , θ 3 and θ 4 have the same rotation plane, which is parallel to Π c , while the angle θ 5 rotates around the line L 45 .Then Equation 21and Equation 20 can be used, and the rotation planes are found as shown in Table 4.
Table 4 shows the parameters used in Equation 17 to calculate the joint angle for the UR5.

Singularities for the UR5
There are two singularity in this mathematical model, which are the same as the singularities of the robot.
The shoulder singularity occurs when P c = αe 3 , ∀α, which means that P c is on the rotational axis of joint 1.Then Π c in Equation 46 becomes undefined as e 0 ∧ e 3 ∧ P c ∧ e ∞ = 0.

Results
Analytic inverse kinematic solutions for the KUKA Agilus robot and the UR5 robot were implemented in the CluCalc software for calculation and display of geometric algebra.The files can be downloaded from https://github.com/ipk-ntnu/inversekinematics using cga.The solutions were extensively tested in simulations by interactively moving the robots over the whole workspace for different solutions of the type elbow up and down, shoulder left and right, and wrist flipped or not.The solutions were in particular tested close to the manipulator singularities.
The accuracy of the inverse kinematic solution was validated by calculating the homogeneous transformation matrix according to Equation 3 and comparing the result with the input parameters n e , s e , a e and p e .The results were correct with accuracy close to machine precision over the whole workspace.
The programming of the solutions is focused on the intersection of geometric objects like lines, circles, planes and spheres that are readily displayed during programming, and this gave valuable intuitive support in the development of the calculations.Moreover, extensive testing over the workspace was facilitated by the 3D graphics.

Conclusion
Conformal geometric algebra has been used to develop analytical inverse kinematic solutions for the KUKA Agilus robot and the UR5 robot.The inverse kinematic solutions gave consistent signs for the angles for the different solutions of the robots.Compared to earlier work in conformal geometric algebra the proposed method handles link offsets and gives correct joint angles over the whole workspace for the different solutions related to shoulder left and right, elbow up and down and wrist flipped or not.The software solution can be ported to standard software like C or C++ for implementation in robot controllers.The method is fairly intuitive and easy to program once the machinery of conformal geometric algebra is mastered, and it provides a powerful tool for developing solutions for new robot geometries and other mechanisms like cranes and automatic topside drilling equipment.
the intersection of a plane Π and a sphere S is the dual circle C * = Π * ∧ S * , and the intersection of a plane Π and a circle C is a dual point pair Q * = Π * ∧ C * .

Figure 1 :
Figure 1: The point pair Q a are two points which are d away from P 2 and 90 • between P 1 and P 2 The joint frames of the Agilus robot.The joint position is q = [0, − π 2 , π 2 , 0, 0, 0] T .

Figure 3 :
Figure3: The red sphere is S 0 , the blue plane is Π 1x , the green circle is generated from S 0 ∧ Π c , and the red point pair is Q 1 , where one is picked to be P 1 .

Figure 8 :
Figure 8: The green planes is Π c⊥ and the red plane is Π c and the blue line is L 45
(a) Overview of the UR5 robot, Qc and Πc (b) Closer view of the UR5 robot and Qc

Figure 7 :
Figure 7: The red spheres are S c and K 0 , the green circle is C 5k , Q c is the blue point pair, and Π c is the yellow plane.

Figure 9 :
Figure 9: Figures showing the process of finding P 3 and P 4 Joint angle parameters for the UR5 robot.It can be verified that the dual of Nθi is the rotational axis z i−1 of the Denavit-Hartenberg convention.Note that the table shows the non-normalized bivectors N θi .(a) The UR5 robot with the elbow up configuration (b) The UR5 robot with the elbow down configuration

Figure 10 :
Figure 10: The two red spheres are S 1 and S 3 , the blue circle is C 2 and the green point pair is Q 2

Table 2 :
DH-table for the UR5 robot

Table 3 :
Joint angle parameters for the Agilus robot.It can be verified that the dual of Nθi is the rotational axis z i−1 of the Denavit-Hartenberg convention.Note that the table shows the non-normalized bivectors N θi .