AP05_2.vp 1 Introduction There are two main approaches when animating articu- lated structures, e.g., human bodies. The first approach, called Forward Kinematics, derives the movement of the struc- ture from the movement of all its parts. The second technique, Inverse Kinematics, works in the opposite way. The movement of each part is determined by the movement of the complete structure. Solving Forward Kinematics is a straightforward transfor- mation between status vector q – angles between adjacent segments, and position vector x – position of the end point of the structure also called the end-effector or EE. It can be eas- ily written as x f q� ( ), (1) while the inverse transformation is required for solving the Inverse Kinematics problem. It can be represented by q f x� �1( ) . (2) The articulated structure is usually redundant, i.e. n > m. Thus the solutions to (2) are non-unique (except degenerated cases where no solution exists at all) and even for n � m several solutions can exist. So the Inverse Kinematics algorithms have to select a particular solution of (2) in the face of multiple solutions. Heuristic techniques have been proposed, e.g., freezing DOFs to eliminate redundancy. However, the redun- dant DOFs are not necessarily disadvantageous, as they can be used to optimize additional constraints. Hence it is useful to impose some optimization criterion g(q), usually repre- sented by a function with a unique global optimum. There are two generic approaches to solving the Inverse Kinematics problem with optimization criteria. Global methods find an optimal path of q with respect to the entire trajectory, usually in computationally expensive off-line calculations. In contrast, local methods, which are applicable in real-time, com- pute only an optimal change of status vector q, dq according the small displacement dx and then integrate dq to generate a complete joint space path. Resolved Motion Rate Control [12] is one such local method. It uses the Jacobian matrix J f ( ) ( ) q q q � � � from Forward Kine- matics to relate the change of status vector to a change of the end-effector. d dx J� �( )q q (3) This equation can be solved for desired dx and unknown dq by taking the inverse of J(q) if it is square (m � n) and non-singular. For redundant structures (n > m) the solving of (3) is described in the following sections. There are also analytical methods, optimization-based methods, and others [1, 7]. 1.1 Jacobian inversion method This method comes simply from (3) by inverting the Jacobian J(q). Although the standard inversion cannot be applied due to the Jacobian’s non-squareness in the general case, Moore-Penrose pseudo-inversion A A A A� �� � �T T( ) 1 has to be utilized. Equation (3) results in d dq q� �J x+( ) . (4) Pseudo-inversion can also be robustly found by Singular Value Decomposition [5]. An advantage of using pseudo-in- verse is the minimum norm solution for dq [12]. Liegeois [8] suggested a more general form of optimization with pseudo- -inverse by minimizing objective function g(q): � �d dq q q q q q � � � � �J x I J J+ + g ( ) ( ) ( ) ( ) � � � , (5) where I is identity matrix and � is positive gain constant. Al- though this formulation requires only that the gradient of g(q) be calculated, the gain � is difficult to obtain. As a general problem, pseudo-inverse methods are nu- merically unstable when the system reaches kinematic singularity [2] – the Jacobian is singular. Furthermore, there is still no control of inner parts of the structure. 1.2 Extended Jacobian method The Extended Jacobian method was originally developed by Baillieul [2, 3].There are r � n � m rows added to the Jacob- ian with goal to zero the gradient of g(q) in the null space of the Jacobian. Let �i� i � 1, …, r be the orthonormal set of vectors that span the null space of J(q), and g(q) is the desired optimization criterion that should be minimized. Equation (3) with the Extended Jacobian follows. © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ 21 Czech Technical University in Prague Acta Polytechnica Vol. 45 No. 2/2005 Solving Inverse Kinematics – A New Approach to the Extended Jacobian Technique M. Šoch, R. Lórencz This paper presents a brief summary of current numerical algorithms for solving the Inverse Kinematics problem. Then a new approach based on the Extended Jacobian technique is compared with the current Jacobian Inversion method. The presented method is intended for use in the field of computer graphics for animation of articulated structures. Keywords: inverse kinematics, Jacobian inversion, extended Jacobian technique. dx J q q q 0 0 � � � � � � � � � � � ( ) ( ( ) � � �i i g � � 1, ,� r dq. (6) Hence the solution requires only standard inversion in- stead of pseudo-inversion. The optimization criterion g(q) represents a set of con- straints even though its meaning is again more useful for robotics – it takes into account some objective-function (e.g., manipulability), which applies on the whole structure instead of physically-based constraints for each joint. This method was further extended in [6] from the numeri- cal point of view. 1.3 Jacobian Transposition method This method comes from the Jacobian Inversion method (section 1.1).The reason for his is the substitution of com- putationally difficult inversion by simple transposition. This simplification is based on the virtual work principle [9], and it results in: d dq J q x� �T ( ) . (7) This method suffers in almost the same way as the Jacob- ian Inversion method (section 1.1). There is still no control of the internal parts of the structure, while the stability of the solution is much better. 1.4 Constrained Inverse Kinematics problems The most common constraint on Inverse Kinematics sys- tems is the joint limits. Joint constraints are usually repre- sented by inequality constraints. Several different methods can be used to make sure a solution satisfies them. 1.4.1 The Lagrange approach The Lagrange approach with Lagrange multipliers will find all minima for the objective function, subject to equality constraints, as stated in [7]. Those minima that do not satisfy the joint limit inequalities can be discarded immediately. Any new minima that arise from the inequalities must lie on the boundary of the region defined by those limits. That is when one or more of the joint variables take extreme values [7]. Therefore by setting one qi to its lower bound or upper bound each time, these minima can be found by solving 2n smaller problems, each involving one less variable qi than the original. 1.4.2 Introducing new variables The new variables can be introduced to transform each inequality into an equality constraint [7], assuming the i-th joint angle qi has upper limit ui and lower limit li. The 2n new variables are added yil and yiu for the i-th joint to trans- form each inequality into an equality constraint. Now the Lagrange approach (section 1.4.1) can be used to solve the original problem plus 2n new variables and 2n new equality constraints. 1.4.3 Penalty function methods Another method adds penalty functions to the objective function. The algorithm looks for the minimum value of the objective function so the penalty causes the value of the objective function to increase as the joints approach their limits. The desired result is that the objective function itself ef- fectively prohibits joint limit violations [1]. Unfortunately, penalty function methods are not numerically stable. 2 A New approach to the Extended Jacobian technique We have focused on the Jacobian inversion method due to its mathematical simplicity and purity, as presented in [11]. We have also appreciated the Extended Jacobian technique suggested by Baillieul in [2, 3]. Extension of the Jacobian, to be an every-time matrix with rank at least n, is the only way to avoid using pseudo-inverse and to exploit standard Gaussi- an inverse. It is also possible to employ constraints on the structure and even on each joint. However, the constraints have to deal with particular joints, which is more interesting in the targeted field of com- puter animation. It is inappropriate to incorporate some objective-function which is being optimized as was suggested in [2], which is more useful in robotics for which this method was originally developed. The other motivation for herein presented method is the absence of similarly-based approaches in computer ani- mation. This is probably caused by the high computational complexity for real-time applications. Thus performance op- timization of presented approach is a future goal. 2.1 Notation and numbering This section will provide an explanation of the indexing of the segments and joints, for better orientation in the fur- ther text. The articulated structure consists of n segments l0, …, ln�1 and n joints 0, …, n�1. The end-effector is seen as the n�1-th joint, designated by symbol n. The angles between the adja- cent segments are represented by status vector q with q0, …, qn�1 items. We assume the structure in Fig. 1 with 4 segments l0, l1, l2, l3, 4 joints 0, 1, 2, 3 and the end-effector labeled with number 4. Status vector q contains 4 elements q0, q1, q2, q3. 2.2 Treating an articulated structure It is necessary to extend the Jacobian, as was stated above, in order to avoid problems coming from usage of pseudo-inverse. It seems natural to control every joint of an articulated structure by some physical characteristic – for instance by its 22 © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ Acta Polytechnica Vol. 45 No. 2/2005 Czech Technical University in Prague q0 q1 q2 q3 EE l2 l3 l1 l0 x y Fig. 1: Articulated structure with notation and numbering joint-stiffness – rather than by setting its angle value. Thus we decided to treat articulated structures by pairs instead of manipulating them in their entirety. We assumed that a complete articulated structure consists of basic elements – two segments connected by a joint, as is shown in Fig. 2. Using many of these basic elements, it is easy to create an articulated structure with n > 2 segments. At each basic element its be- havior can be controlled. The basic element is described by two quantities – either [x, y] in Cartesian coordinates or [L, �] in angle coordinates. The basic elements constituting a complex structure are not connected at the end-points, but the next basic element is connected into medial joint of its predecessor, so they share one segment of the final complex structure. Each basic element is labeled by a pair of joint numbers within the struc- ture – i : i � 2 where i is the joint where the first segment starts and i � 2 is the joint where the second (and last) seg- ment finishes. Each basic element lies in its local coordinate system, i.e., the coordinates [xi, i�1, yi, i�1] or [Li, i�1, �i, i�1] are relative. The Cartesian coordinates of the basic element i : i �2 are com- puted by x l q y l qi i j j ij i i i i j j ij i i , ,cos , sin ,� � � � � � � �� �1 1 1 1 (8) where q qj i k k i j � � � . Its origin is at the beginning of the corre- sponding basic element and is rotated by an angle equal to the sum of the previous angles from the status vector. For example, in Fig. 3, where three basic elements 0 : 2, 1 : 3 and 2 : 4 form the complete structure, we assume a basic element 2 : 4 described by [L23, �23], its coordinate system ori- gin lies at the end of the second segment l1 and is rotated by angle qi i� � 0 1 . 2.3 Extending Jacobian According to the preceding section we can obtain the Jacobian with rank at most n � 1 – the number of pairs in a complex structure. This is not enough to avoid problems with the Jacobian’s singularity. Hence these rows are added into the standard Jacobian used for solving (3) – the rows in the standard Jacobian represent the main link 0 � n (base to end-effector). Then the Jacobian looks as presented in equation (9). Using such a Jacobian extension we can control the end- -effector and also the behavior of the inner joints. © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ 23 Czech Technical University in Prague Acta Polytechnica Vol. 45 No. 2/2005 q0 q l l L 1 1 0 � x y Fig. 2: A basic element as a main part for creating complex articu- lated structures q0 q1 q2 q3 EE l2 l3 l1 l0 L01 �01 L12 L23 � � 12 23 Fig. 3: A complex joint structure consisting of basic elements J2 0 0 1 1 0 1 1 0 1 1 2 0 0 0 n n l q l q l q l q l , ( ) sin sin sin sin q � � � � � � � sin sin sin cos q l q l q l q l n n n 2 1 2 2 1 1 1 2 0 0 0 0 0 0 � � � � � � � � � � � � � 1 1 0 1 1 0 1 1 2 2 1 2 2 1 0 0 0 0 cos cos cos cos cos q l q l q l q l q � � � � � � � � 0 0 0 1 1 2 00 1 01 1 � l q l q l q n n n i i i n i i i n � � � � � � � � �� � cos sin sin � � � � � � � � � � � �� � l q l q l q l n n i i i n i i i n n 1 1 0 00 1 01 1 sin cos cos 1 1 0 cos qn � � � � (9) The i-th and ( i � n � 1)-th rows of the Jacobian correspond to the i-th basic element of the articulated structure. For instance, the 0-th and (n � 1)-th rows correspond to the basic element designated as L01 in Fig. 3. The last two Jacob- ian rows describe the main link – from the base to the end-effector. 2.3.1 Extending dx Consequently as the Jacobian is extended, the displace- ment vector dx also has to be extended to fit the Jacobian’s row dimension – dim dx � 2n . The dx items represent the change of the corresponding basic element or main link 0 : n (the last two rows). To set up the change of the main link 0 : n is straightforward – it is the only input of the method. All the remaining items representing a change of the particular basic elements have to be derived from the change of the main link taking into account the desired physical constraints imposed on the joints. As the displacement of the main link is easy to define and clear to understand, the definition of the displacements of a particular basic element is difficult to describe according to the required all-structure behavior. Future work and research remains to be done on this. 2.4 Solving Inverse Kinematics Equation (3) can be comprehended as a Linear System A x b� � (10) and thus can be solved by any appropriate method [4]. Vector x is the vector of unknown variables. In the case of (3), vector x corresponds to vector dq, which is being solved. Next, vector b represents the displacement of end-effector dx and matrix A corresponds to Jacobian J(q). Since the Jacobian is normally a matrix with dimensions (2n, n), it is necessary to employ an approximation method for solving (3). We decided to use the Ordinary Least Square (OLS) method based on normal matrices [10], as it minimizes the norm of residual vector r : r A x b� � � . However, any approximation method can be used [5]. OLS produces a square matrix, which can be already solved by the LU decomposition [4] that we decided to use. 3 Experiments and evaluation of results We performed a comparison test of herein presented Ex- tended Jacobian method with a method based on using pseudo-inverse (section 1.1) as presented in [11]. We decided to make a comparison with the Jacobian inversion method as it is one of the standard methods, together with Jacobian transposition (section 1.3) and CCD – Cyclic Coordinate Descent, for solving the Inverse Kinematics problem in the field of computer animation. Our goal was to acquire the same behavior of both meth- ods, but with good recovery from the singular case. The testing structure consists of 6 segments with lengths l0 � 90, l1 � 60, l2 � 90, l3 � 150, l4 � 150 and l5 � 90 points. The starting configuration was determined by qi � 0. The starting point lies at [�230, 0]. The desired trajectory was formed by an ellipse with a � 400 and b � 50 with the mid- -point at the origin. Therefore the end-effector touched the ellipse at the point [400, 0]. First we tried to minimize the movement of the inner parts and make a move to the end-effector only. We accomplished this by setting all dxi � 0 except those which correspond to a move of the end-effector. This only partly achieved the desired requirements. The structure recovered well from the singular case – see Fig. 4, but its behavior is different from the behavior of the method based on pseudo-inverse. Due to the these problems with behavior, we tried to mini- mize the influence of the added rows by multiplying each added row by a positive non-zero constant w>0. We expected a minimizing effect of the added rows. We assumed the fol- lowing: when w � 0 then the Extended Jacobian will become the Non-Extended Jacobian J JExt NExt� . However, this did not achieve the required effect. With w � 0 the behavior re- mained and the ability to recover from the singular cases disappeared. The behavior of the tested methods is shown in the Figs. 5 and 6 – Fig. 5 shows the standard Jacobian Inversion method while Fig. 6 displays the herein presented Jacobian Extension method. The pictures show the movement of the structure during the first several steps. 24 © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ Acta Polytechnica Vol. 45 No. 2/2005 Czech Technical University in Prague 0 50 100 150 200 250 Extended Jacobian Pseudo-inverse Step D is ta n c e fr o m d e s ir re d p o s it io n in p o in ts 0 1 2 3 4 5 6 7 Fig. 4: Graph presenting the distance of the end-effector from the desired position The Jacobian Inversion method made a “big jump” when moving from the starting configuration. This was caused by a kinematic singularity and consequent numerical instability of the system. This did not happen with the Extended Jacobian method due to the fact its Jacobian cannot be singular in any case, i.e., its rank is always guaranteed to be n. 4 Conclusions We have presented a brief overview of methods for solv- ing the Inverse Kinematics, together with a new option for solving an identical problem actually in plane mainly for the purposes of computer graphics – for animating articu- lated structures. The articulated structure is treated by pairs to allow us to control the inner joints based on physical characteristics. The method is based on an extension of the Jacobian. 2 ( n � 1) rows are added into the Jacobian corresponding n � 1 pairs – basic elements. The system is over-determined (the Jacobian is full-rank), so singular cases coming from usage of pseudo-inverse are avoided. Hence the method is stable. Several tests were performed to compare presented method with the method based on pseudo-inverse only. The same behavior was required for both methods. Next, the ability to recover from singular cases was also required. In re- covering from singular cases, the Extended Jacobian method passed successfully, but the same behavior for both methods was not achieved. 4.1 Future work The usability of herein presented method in 3D space and exact control of the inner joints have not been evaluated yet. For the first issue – exact structure control – the inverse transformation f �1 of (2) has to be found, either analytically or numerically. Then, a Sensitivity analysis can be performed to realize the dependencies in the system and hence discover the key to exact control. The next remaining problem – usage in 3D space – seems not to be so difficult now. This is mainly due to the fact that the rotating joints usually operate in a single plane. However, this problem becomes difficult if the joints are capable of unlimited rotation instead of rotation in a specified plane. The usability of the proposed technique is clear, for en- abling animation of articulated structures with user-defined (expressed as functions) constraints imposed on the joints. After the tasks mentioned above have been dealt with, the focus will turn to the performance and speed of the proposed solution, as speed is dominant requirement for real time com- puter animations. Also for this reason, the basic methods for solving the Inverse Kinematics problem, such as Jacobian in- version, Jacobian transposition, and CCD are only used in computer animation due to their relatively low computational complexity. To achieve the goal of speed we are planning to employ non-standard arithmetic, probably residual arithme- tic, together with “special hard-wear” which is available on today’s �-processors, for instance MMX, SSE, 3DNow, etc. © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ 25 Czech Technical University in Prague Acta Polytechnica Vol. 45 No. 2/2005 start position 2. step 3. step 4. step 5. step 10. step Fig. 5: Behavior of the Jacobian Inversion method during the test start position 2. step 10. step Fig. 6: Behavior of herein presented Jacobian Extension method during the test References [1] Badler, N. I., Phillips, C. B., Webber, B. L.: Simulating Humans: Computer Graphics Animation and Control. Ox- ford University Press, 1993. [2] Baillieul, J.: “Kinematic Programming Alternatives for Redundant Manipulators.” In IEEE International Con- ference on Robotics and Automation, 1985, p. 722–728. [3] Baillieul, J.: “Avoiding Obstacles and Resolving Kine- matic Redundancy.” In IEEE International Conference on Robotics and Automation, 1986, p. 1698–1704. [4] Friedberg, S. H., Insel, A. J., Spence, L. E.: Linear Alge- bra 3rd Ed. Upper Saddle River: Prentice-Hall, 1997. ISBN 0-13-233859-9. [5] Golub, G. H., Van Loan, C. F.: Matrix Computations 3rd Ed. Baltimore: The Johns Hopkins University Press, 1996. ISBN 0-8018-5414-8. [6] Klein, C. A., Chu-Jenq, C., Ahmed, S.: “A New Formula- tion of the Extended Jacobian Method and its use in Mapping Algorithmic Singularities for Kinematically Redundant Manipulators.” IEEE Transactions on Ro- botics and Automation, Vol. 11 (February 1995), No. 1, p. 50–55. [7] Korein, J. U., Badler, N. I.: “Techniques for Generat- ing the Goal-directed Motion of Articulated Structures.” IEEE Transactions on Computer Graphics and Applica- tions, November 1982, p. 71–81. [8] Liegeosis, A.: “Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms.” IEEE Transactions on Systems, Man, and Cybernetics, Vol. 7 (1977), No. 12, p. 868–871. [9] Paul, R. P.: Robot Manipulators: Mathematics, Programming and Control. Cambridge, Mass.: MIT Press, 1981. [10] Van Huffel, S., Vandewalle, J.: The Total Least-Squares Problem. Philadelphia: SIAM, 1991. ISBN 0-89871-275-0. [11] Šoch, M., Lórencz, R.: “Solving Inverse Kinematics – Jacobian Inversion Method in a Plane.” In 11th Interna- tional Workshop on Systems, Signals, Image Processing and Ambient Multimedia. Poznaň: PTETiS, 2004, p. 95–97. ISBN 83-906074-8-4. [12] Whitney, D. E.: “Resolved Motion Rate Control of Ma- nipulators and Human Prosthesis.” IEEE Transactions on Man-Machine Systems, MMS-10, June 1969, No. 2, p. 47–53. Ing. Martin Šoch e-mail: sochm@fel.cvut.cz Doc. Ing. Róbert Lórencz, CSc. e-mail: lorencz@fel.cvut.cz Department of Computer Science and Engineering Czech Technical University in Prague Faculty of Electrical Engineering Karlovo nám. 13 121 35 Praha 2, Czech Republic 26 © Czech Technical University Publishing House http://ctn.cvut.cz/ap/ Acta Polytechnica Vol. 45 No. 2/2005 Czech Technical University in Prague