Algorithmic approach to geometric solution of generalized Paden–Kahan subproblem and its extension

Kinematics as a science of geometry of motion describes motion by means of position, orientation, and their time derivatives. The focus of this article aims screw theory approach for the solution of inverse kinematics problem. The kinematic elements are mathematically assembled through screw theory by using only the base, tool, and workpiece coordinate systems—opposite to conventional Denavit–Hartenberg approach, where at least n + 1 coordinate frames are needed for a robot manipulator with n joints. The inverse kinematics solution in Denavit–Hartenberg convention is implicit. Instead, explicit solutions to inverse kinematics using the Paden–Kahan subproblems could be expressed. This article gives step-by-step application of geometric algorithm for the solution of all the cases of Paden–Kahan subproblem 2 and some extension of that subproblem based on subproblem 2. The algorithm described here covers all of the cases that can appear in the generalized subproblem 2 definition, which makes it applicable for multiple movement configurations. The extended subproblem is used to solve inverse kinematics of a manipulator that cannot be solved using only three basic Paden–Kahan subproblems, as they are originally formulated. Instead, here is provided solution for the case of three subsequent rotations, where last two axes are parallel and the first one does not lie in the same plane with neither of the other axes. Since the inverse kinematics problem may have no solution, unique solution, or many solutions, this article gives a thorough discussion about the necessary conditions for the existence and number of solutions.


Introduction
Historically, fundamental Chasles' theorem states that every proper motion is given by screw motion. Proper motion is three-dimensional (3-D) rotation about arbitrary axis followed by 3-D translation and describes the rigid body motion. A screw motion is rotation about arbitrary axis, followed by translation along the same axis. Formulation and proof using modern mathematical language is given by Palais and Palais 1 and in more depth and details by Selig. 2 If the set of geometrical characteristics is given, the position and orientation of every link of the robot could be expressed as function of the joint variables. A chain of coordinate systems is attached to the joints using traditional Denavit-Hartenberg (DH) convention, originally established in the study by Denavit and Hartenberg. 3 They are related by 4 Â 4 matrices used to describe rigid body motion. Details for DH convention and non-DH method discussion and introduction to screw theory, are given in the study by Jazar, 4 as well.
The most comprehensive and exact approach to build the screw theory and its application in robotics is given by Murray et al. 5 Rotation about direction specified with the unit vector o 2 R 3 and angle q is represented by 3 Â 3 matrix, given in exponential form eô q ¼ I 3 þô sin q þôð1 À cosqÞ ð 1Þ Equation (1) also is known as Rodrigues formula and its exponential form gives efficient computing method. Let v; o 2 R 3 are unit vectors in 3-D. A twist coordinates x are determined as and a twistx is defined as 4 Â 4 matrix x ¼ô v 0 0 ð4Þ A screw motion consists of rotation about an axis in direction determined by the unit vector o, through an angle q followed by translation by an amount d along the same axis. If it is not pure translation, then q 6 ¼ 0, so a pitch of the screw could be defined as h ¼ d=q. Let q be the arbitrary point of the axis of the screw. In a case of general screw motion which is not pure translation, the vector determines the twist coordinates, equation (3) and as well the twist, equation (4). This screw motion is represented by 4 Â 4 matrix, denoted as exponential ofxq The pure rotation as special case of screw motion with pitch h ¼ 0 is represented by equation (6) as well, taking the twist coordinates to be In the case of pure translation in a direction determined by unit vector v by amount q, the twist is defined as and its exponential form is given by Robot manipulator consists of sequence of rigid links, connected by joints with motors attached on them. Robot manipulator with n joints is said to have n degrees of freedom (DOF). The motion for every joint is determined with three constant parameters and one variable. All the n variables of the joints form the machine space (joint space) of the robot manipulator. The machine coordinates of the manipulator of n DOF are The end effector of the robot manipulator is needed to be controlled in the sense to control its pose-position and orientation. That determines the pose space of the robot manipulator.
Forward kinematics is a procedure for obtaining the pose of the robot manipulator if the machine coordinates are given. More formally, forward kinematics is a function from machine space to pose space of the robot.
Using screw theory, the forward kinematics is determined as product of the exponentials of the appropriate twists for each link, respectively gðqÞ ¼ ex 1 q 1 ex 2 q 2 :::ex n q n ð11Þ Only two coordinate frames are sufficient-the tool and base frame. If g bt ð0Þ is the transformation matrix from tool to base coordinate frame when all machine coordinates are zeros, the forward kinematics map is determined by g bt ðqÞ ¼ gðqÞg bt ð0Þ ¼ ex 1 q 1 ex 2 q 2 :::ex n q n g bt ð0Þ ð12Þ Some robotized machines are configured with two kinematics chains. Forward and inverse kinematics can be solved in similar manner. 6 Xiang and Altintas 6 give comparison between DH approach against screw theory approach. The latter is better since no local coordinate systems are needed and it provides explicit solution to the inverse kinematics problem. 7 Details and different inverse kinematic formulation methods for industrial robot manipulators using screw theory could be found in the study by Sariyildiz et al. 8 Comparison between both methods is given by Rocha et al. 9 If the pose coordinates of the end effector are given, inverse kinematics problem is used to find the machine coordinates. Inverse kinematics problem may have no solution, unique solution, or many solutions. Standard approach, using DH convention is focused on solving equation systems and optimization techniques in some cases.
We prefer screw theory, especially the geometric algorithm approach explained in this article, over the traditional DH method. There are several advantages In DH convention at least n þ 1 coordinate frames are needed, since a robot manipulator with n joints will have n þ 1 links, and we rigidly attach a coordinate frame to each link. 10 The choice for a coordinate frames in DH convention is constrained by the DH coordinate frame assumptions, so a procedure for assigning coordinate frames that satisfy the constraints is needed to be performed. 10 In screw theory, often, only two coordinate frames are sufficient, the base frame, and the tool frame, so it is more simple to explain entire kinematic chain. 5 Given the DH parameters for a manipulator, the corresponding twists can be easily determined. 5 The inverse kinematics solution in DH convention is implicit. Using screw theory, it is possible to find explicit solutions to inverse kinematics using the Paden-Kahan subproblems or its extensions. 7 The screw theory offers way to express explicit solution that can be reduced to an application of geometric algorithm. There are many researches and case studies based on screw theory published in the last decade. [11][12][13][14][15][16][17][18] For a large number of robot manipulators, it is sufficient to implement three classical Paden-Kahan subproblems, originally established in the study by Paden. 19

Paden-Kahan subproblems
The most commonly used geometric algorithms in inverse kinematics problem solutions are addressed by Murray et al. 5

Subproblem 1
Let x be a zero-pitch twist with unit magnitude and p; q 2 R 3 two points. Find q such that

Subproblem 2
Let x 1 and x 2 be two zero-pitch twists with unit magnitudes with intersecting axes and p; q 2 R 3 two points. Find q 1 and q 2 such that

Subproblem 3
Let x be a zero-pitch twist with unit magnitude, p; q 2 R 3 two points and d positive real number. Find q such that All subproblems stated this way are completely solved in the study by Murray et al. 5 There are also several examples of robot manipulators and the inverse kinematics problem solutions based on reducing the full inverse kinematics problem into appropriate subproblems.
There exist robot manipulators that cannot be solved using only these three such formulated subproblems. The study of Yew-sheng and Ai-ping 20 gives example of 5-DOF manipulator with two consequent nonintersecting rotational axes. They have changed the condition for existing intersection point of the axes in subproblem 2. The new subproblem is formulated and the complete solution for nonintersecting axes is fully explained in the study by Yew-sheng and Ai-ping. 20 Generalization of subproblem 2 From mathematical point of view, subproblems considered in the studies by Murray et al. 5 and Yew-sheng and Aiping 20 are just cases of one general subproblem. All of the cases can be covered considering the generalized formulation as it follows.

Subproblem 2 (generalized)
Let x 1 and x 2 be two zero-pitch twists with unit magnitudes and p; q 2 R 3 two points. Find q 1 and q 2 , so that the equation (14) is satisfied.

Solution
Geometrically, the problem is to find the angles q 1 and q 2 , so then if the point p is rotated about axis of the twist x 2 by the angle q 2 and then is rotated about axis of the twist x 1 by the angle q 1 , it will coincident with the point q.
Let o 1 and o 2 be the unit vectors in a direction of the axes of the twists x 1 and x 2 , respectively. In general, there are four cases (two cases by two subcases), on dependence on relative position of the axes in the space. Case 1b. The axes of rotation are skew lines. This is the case solved in the study by Yew-sheng and Ai-ping, 20 but discussion for existence of the solution and number of solutions is purely mentioned.
Below is given a new geometrical solution that covers the both subcases-cases 1a and 1b with detailed discussion about the existence of a solution and the number of solutions. This new algorithmic approach of geometrical solution consists of five steps detailed below. In fact, this solution is described as geometric algorithm and its steps refer to well-known geometric algorithms. Most of them have good explanation in the study by Dunn and Parberry. 21 Step 1. Determine the closest points r 1 and r 2 between the axes of twists x 1 and x 2 .
In the case where the two axes intersect in exactly one point, the closest point between them will be the intersecting point, and r 1 ¼ r 2 ¼ r. So, all the equations given in this section will be valid as well for the both subcases.
Step 2. Determine plane P 1 , the plane that is normal to the unit vector o 1 and passes through the point q (plane of the red circle- Figure 1). Determine plane P 2 , the plane that is normal to the unit vector o 2 and passes through the point p (plane of the blue circle- Figure 1). Next, determine the intersecting line l(t) of these two planes P 1 and P where l 0 is an arbitrary point of line l(t) and D is unit vector in direction of line l(t).
Step 3. Use the point r 1 to determine the projection s 1 of the point q on the axis of the twist x 1 Determine the intersection between the line l(t) and the red circle, circle k 1 centered in point s 1 and radius R 1 The set A as intersection between the line l(t) and the circle k 1 (both of them lie on the plane P 1 ) could contain zero, one, or two points depending on the relative position of the line l(t) and the circle k 1 .
Then determine the point s 1 0 , the closest point from the point s 1 to the line l(t). First, the necessary condition for existence of the solution is If this condition is satisfied, then it is not possible A to be the empty set. Depending on, whether the equality or the strict inequality is satisfied, the set A will contain one or two elements.
Step 4. Now, use the point r 2 to determine the projection s 2 of the given point p to the axis of twist x 2 Then determine the intersection between the line l(t) and the blue circle k 2 centered in point s 2 and radius R 2 The intersection will also be a set B that could also contain zero, one, or two points, depending on the relative position of the circle k 2 and the line l(t).
Next determine the closest point s 2 0 from the point s 2 to the line l(t), in order to get the second necessary condition for existence of the solution: The discussion is similar to the one given with the first necessary condition or if the condition given in equation (22) is satisfied, then it is not possible the set B be the empty set. If the equality is satisfied then the set B will have one element, and if the strict inequality is satisfied then the set B will have two intersecting points as elements.
Step 5. By determining the set M that represents the intersection of the sets A and B, it is obtained the third and final necessary condition for existence of the solution If equation (23) is not satisfied, then the subproblem 2 does not have a solution. If equation (23) is satisfied, then exists point c such that Let us introduce notations The problem is now reduced to two subproblems 1 using equation (24). First, q 2 should be determined by and then in the same manner q 1 is determined by When equation (23) is satisfied, the set M may have one or two elements. In the case of one element set, unique c exists and that is the element of intersection, that is, c 2 A \ B. Namely, this means that the point c represents and are satisfied, the final solution is unique pair of angles ðq 1 ; q 2 Þ. If equation (29) is not satisfied, that is, u 0 ¼ o, then any q 2 satisfies c ¼ ex 2 q 2 p, since p lies on the axis of the screw x 2 and must be c ¼ p. Additionally, if equation (30) is satisfied, there are infinitely many solutions ðq 1 ; q 2 Þ-q 1 is determined by equation (28) and q 2 is any angle. In opposite, any angles q 1 and q 2 are solution of the problem, since must be c ¼ p ¼ q ¼ r. If equation (29) is satisfied, but equation (30) is not, that is, v 0 ¼ o, then any q 1 satisfies c ¼ ex 1 q 1 q, since q lies on the axis of the screw x 1 and must be c ¼ q. There are infinitely many solutions ðq 1 ; q 2 Þ-q 2 is determined by equation (27) and q 1 is any angle.
In the case when the set M have two elements, and there are two points c, denoted by c 1 and c 2 . Assumption given in equation (29) is not satisfied or equation (30) is not satisfied leads to contrary with condition M to contain two points. So, in this case, conditions from equations (29)   Case 2a: Coincide axes: The problem is reduced to subproblem 1.
First necessary condition for solution existence is Geometrically, the condition given in equation (31) means that points p and q lie on a plane normal to the axis of the both screws.
Second necessary condition for solution existence is Geometrically, the condition from equation (32) means that points p and q are equally distanced from the axis of the screws.
If equations (31) and (32) are both satisfied, then the following necessary condition for uniqueness should be checked If equation (33) is satisfied, there is unique value of q, given by and there are infinitely many solutions of the problem. Namely, every pair of angles is a solution of the subproblem 2 in this case. If the conditions given in equations (31) and (32) are satisfied, but equation (33) is not, then any pair ðq 1 ; q 2 Þ is a solution of the problem, since p and q lie on the axis of the screws. If any of the conditions from equations (31) and (32) are not satisfied, then there is no solution.
Case 2b: Parallel axes. Let r 1 be arbitrary point on the axis of the screw x 1 (Figure 2). Denote The problem is now reduced to finding angle q 2 to rotate the point p around the axis of the screw x 2 to come at the distance d from the point s 1 , where That means to call subproblem 3 to find angle q 2 and then to call subproblem 1 to find angle q 1 .
Obviously, all the points p, q, and s 1 need to lie on the same plane normal to o, so the first necessary condition for the solution existence is In algorithmic manner, following two steps should be done if equation (38) is satisfied. Step 1. Call the subproblem 3 with p, s 1 , x 2 , and d. Subproblem 3 solution details can be found in the study by Murray et al. 5 Let r 2 be arbitrary point on the axis of the screw x 2 . Geometrically, there is a solution if the circle centered at s 2 (orthogonal projection of the point p on the axis of the screw x 2 ) with radius intersects the circle centered at s 1 with radius d.
Because of equation (38), the condition is satisfied. Denote Second necessary condition for solution existence is Suppose u 0 6 ¼ o and d > 0. If the condition given in equation (43) is satisfied, then using the law of cosine follows If inequality in equation (43) is strict, there are two angles q 12 and q 22 obtained from equation (44). If equality is hold in equation (43), then both of the circles touch each other, so there is unique angle q 2 .
Step 2. For every angle q 2 , obtained in step 1, call the subproblem 1, with q, x 1 , and c, where Denote The angle q 1 is then determined by If u 0 ¼ o, then the point p lies on the axis of the screw x 2 , so there is no solution in the case kp À s 1 k 6 ¼ d. If additionally, kp À s 1 k ¼ d, there are infinitely many solutions-q 2 could be any angle and q 1 is determined by equation (48).
The case v 0 ¼ o is not possible, since it leads to conclusion s 1 lies on the axis of the screw x 1 , what is contrary to the assumption the axes of both of the screws are parallel.
If d ¼ 0, then q lies on the axis of the screw x 1 and q ¼ s 1 . The problem is then reduced to subproblem 1 and if equation (38) is satisfied and ku 0 k ¼ kv 0 k 6 ¼ 0, then there is a unique solution for q 2 obtained by equation (44) and q 1 is any angle (infinitely many solutions); otherwise, there is no solution.

Extension of subproblem 2: New subproblem
In the previous section, a detailed elaboration about the generalization of Paden-Kahan subproblem 2 was given, and now this subproblem 2 can be used in general case without a consideration about the actual position of the axes of rotation. However, the given generalization of the subproblem 2 as well as the three Paden-Kahan subproblems are not enough to solve the inverse kinematics of a robot with general configuration. Chen et al. 22 noted that the solution for inverse kinematics of a serial robot with 6-DOF 'Qianjiang I' using the three well-known Paden-Kahan subproblems, as they are originally formulated, is impossible. So, they formulated a new subproblem just to solve the inverse kinematics of the above-mentioned robot.
The new subproblem comprises of a rotation of a point p (shown in Figure 3) about three axes of zero-pitch twists x 3 , x 2 , and x 1 successively, such that it coincides with a given point q. Thereto the axes of twists x 2 and x 3 , respectively, are parallel to each other and the first axis, the axis of twist x 1 is not parallel to the remaining two axes and does not lie in the same plane neither with the axis of twist x 2 nor axis of twist x 3 . In order to solve the new subproblem, the angles q 1 , q 2 , and q 3 have to be determined, such that So, the point p first rotates about the green axis of twist x 3 by angle q 3 , in Figure 3 represented by yellow arc, then it rotates about the blue axis of twists x 2 by angle q 2 represented by the dark blue arc and at the end it rotates about the red axis of twists x 1 by angle q 1 represented by the red arc. Chen et al. 22 gave complete solution, but as a part of the solution, they used the assumption that the axis of twist x 1 is perpendicular to the two remaining axes of twists x 2 and x 3 . Again, the solution of this new subproblem is not a general one.
In this article, a new geometric solution is given to this new subproblem in general case, where the axis of twist x 1 is in an arbitrary position and does not have to be perpendicular to the other axes of twists x 2 and x 3 . This solution only excludes the position when the axis of twist x 1 is parallel to the other two axes, but this case does not have to be taken into account because the problem definition excluded this case. In this new geometric solution, the generalized Paden-Kahan subproblem 2 is used, with the case, when the two axes are parallel to each other that was detailed in the previous section.
Let o 1 be the unit vector in a direction of the first axis of twist x 1 and o 2 be the unit vectors in a direction of the two parallel axes of twists x 2 and x 3 . Figure 3 visualizes the new geometrical solution that is constituted of four steps, explained below.
Step 1 Determine the plane P 1 , the plane of the red circle that is normal to the unit vector o 1 and passes through the point q. Determine the plane P 2 , the plane of a green circle that is normal to the unit vector o 2 and passes through the point p. Next, determine the line l(t) that is the intersection of these two planes P 1 and P 2 ( Figure 3) This intersecting line l(t) is determined, since the unit vectors o 1 and o 2 are not parallel to each other.
Step 2 Determine the projection s of the point q on the axis of the twist x 1 where s 1 is an arbitrary point on the axis of the twist x 1 .
Step 3 Determine the intersection q c between the line l(t) and a circle k 1 centered in the point s with radius on the plane P 1 . To find the intersection of the line l(t) and a circle, one needs to find the roots of a quadratic equation with t as an unknown. From the value of the discriminant D of the quadratic equation depends whether the line l(t) intersects the circle or not, and if it does, in how many points Step 4 If D < 0, then the line l(t) doesn't intersect the circle k 1 , and the new subproblem does not have a solution.
If D ¼ 0 then the line l(t) and the circle k 1 have one common point q c Once the point q c is determined, the angle q 1 can be found by calling the Paden-Kahan subproblem 1, so that the point q c coincides with the point q when it rotates about the axis of twist x 1 The two remaining unknown angles q 2 and q 3 can be determined by calling the Paden-Kahan subproblem 2, so that the point p first rotates about the axis of twist x 3 and then rotates about the axis with twist x 2 to coincides with point q c Here, the original formulation of Padern-Kahan subproblem 2 is not used, but our generalized version of this subproblem 2 detailed in the previous section, because the axes of twists x 2 and x 3 are parallel to each other, and case 2b needs to be considered.
If D > 0 then the line l(t) intersect the circle k 1 in two points q c1 and q c2 that can be determined where t 1=2 ¼ In this case in order to determine the unknown angles q 1 , q 2 , and q 3 , again it has to be done by calling of the Paden-Kahan subproblems 1 and 2 in the same manner like in the previously detailed case when D ¼ 0, but two times: once for the point q c1 and then the same for the point q c2 . At the end, the solution is a set of two or four triples of angles, if the given data satisfy necessary conditions for existence and uniqueness of the solution in generalized subproblem 2.

Kinematic model of 6-DOF industrial robot
We have used kinematic model based on the screw theory and our geometric algorithms as a part of experiment for accuracy improvement of 6-DOF industrial robot KUKA KR 360 R2830, manufactured by KUKA AG company.
Using the scheme shown in Figure 4 and the notations explained in "Introduction" section, construction of the twists for pure rotation are made according to equation (7)  Since the point q 6 lies on all three last axes, from equation (60) and the equation g bt ðqÞ ¼ T follows Finding q 1 , q 2 , and q 3 is now reduced to applying the extended subproblem 2-the new subproblem explained in the previous section, since axes of the twists x 2 and x 3 are parallel, and the axis of the twist x 1 is not parallel to the remaining two axes and does not lie in the same plane neither with the axis of twist x 2 nor axis of twist x 3 .
Taking into consideration, the obtained values for q 1 , q 2 , and q 3 and that the point lies on the axis of the twist x 6 , applying the standard Paden-Kahan subproblem 2 yields the solution for q 4 and q 5 , since ex 4 q 4 ex 5 q 5 q 7 ¼ e Àx 3 q 3 e Àx 2 q 2 e Àx 1 q 1 T Á g bt ð0Þ À1 Á q 7 ð64Þ Finally, any referent point on the axis of the twist x 5 , different from q 6 can be used for obtaining the angle q 6 , applying the standard Paden-Kahan subproblem 1. For instance, taking the point and using the equation ex 6 q 6 q 8 ¼ e Àx 5 q 5 e Àx 4 q 4 e Àx 3 q 3 e Àx 2 q 2 e Àx 1 q 1 T Á g bt ð0Þ À1 Á q 8 ð66Þ  Proposed inverse kinematics procedure is applied and eight solutions are obtained, shown in Table 1. All these solutions are checked by simulation and forward kinematics procedure and all of them lead to desired pose (68) with maximal orientation deviation of 3:5 Â 10 À15 and maximal position deviation of 1:5 Â 10 À12 mm.

Algorithm testing on 7-DOF AFP machine
The geometric algorithm presented in the previous section was implemented in the postprocessor part of MikroPlace [version 4.0]-software for off-line programming, design and simulation of automated fiber placement (AFP) and automatic tape layup (ATL) machines. Specifically, it was experimentally tested on AFP machine with 7 DOF-three linear axes and four rotational, deployed in the next parent/ child order: Linear X ! Linear Y ! Linear Z ! Rotation Z ! Rotation X ! Rotation Z and another free axis Rotation around X ( Figure 5).
The algorithm was tested on different mandrel shapes covering the two general types of mandrel geometry: Open shape surface mandrel (flat, shaped parabolic) (Figure 6(a)) and Closed surfaces-360 rotational mandrel surfaces (Figure 6(b)).
The layup was under different layup angles in order to test more axes combination. Figure 7 shows different machine positions on the software simulation.
In Figure 8, the actual laid material is shown.

Conclusion
Concentrating on the necessary and sufficient conditions, as well as the number of solutions for the inverse kinematics of the robot manipulator in order to transfer from pose to machine space coordinates, this article has presented a geometric algorithm that can be applied on multiple movement configurations. Three basic Paden-Kahan subproblems of screw theory were used in order to extend subproblem 2 designed not to depend on the mutual positioning of the screw axes. The article sets and solves (giving an algorithmic approach) a new subproblem where three independent screw axes are used in order to move the manipulator from one to another point in the space. Practically, this subproblem is used to solve a specific configuration of a 6-DOF robot that cannot be solved using the three well-known Paden-Kahan subproblems. The algorithms given here, solve the problem of inverse kinematics generally, so when they are implemented in some programming language there is no need to look after the mutual  positioning of the axes of rotation. Finally, combining the algorithms presented in this article gives a wider use of the screw theory based methods for several differently configured manipulators. Although, this article has been specifically focused on the step-by-step geometric algorithm for the solution of extended Paden-Kahan subproblems, these are the key algorithms to the actual inverse kinematics implementation for robotic movement and industrial gantry type machines with multiple movement axes.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.