1 Introduction

In computer animation, an articulated figure is often modelled as a set of rigid segments connected by joints. Abstractly, a joint is a constraint on the geometric relationship between two adjacent segments. This relationship is expressed with a number of parameters called joint angles. With judicious selection of joints so that, for example, segments are connected to form a tree structure, a collection of joint angles of all the joints corresponds exactly to a configuration of the figure. This correspondence provides an immediate computer representation of articulated figure configurations such that given a set of joint angles, it is straightforward to compute the corresponding configuration; however, the problem of finding a set of joint angles that corresponds to a given configuration - the inverse kinematics problem - is nontrivial.

The inverse kinematics problem is extremely important in computer animation because it is often the spatial appearance, rather than the joint angles, that an animator finds easier to express.

Many interesting objects in the computer animation domain, such as human figures and other characters, have many redundant degrees of freedom when viewed as a tree-structured kinematics mechanism. So, it was necessary to look for effective means for solving this problem under circumstances applicable to computer animation.

Korein and Badler began to study and implement methods for kinematic chain positioning, especially in the context of joint limits and redundant degrees of freedom[4, 3]. In [1] Badler, Manoochehri and Walters used 3D position constraints to specify spatial configurations of articulated figures.

Girard and Maciejewski adopted a method from robotics[2]. In their work they calculated the pseudo-inverse of the Jacobian matrix that relates the increment of the joint angles to the displacement of the end effector in space. The main formula is

r/

Recently, most computer animation systems have adopted inverse kinematics techniques from robotics. In these approaches an inverse kinematics problem is cast into a system of nonlinear equations or an optimization problem which can be solved using an iterative numerical algorithm. Most inverse kinematics algorithms were originally designed to meet the requirements of robotics, their straightforward application to computer animation frequently lead to problems. It is instructive to highlight some of the difficulties: In robotics inverse kinematics tasks only involve constraining the position and orientation of the terminal segment or the end effector. In computer animation, many other types of constraints have to be considered, for example, constraining selected points on non-terminal segments, aiming the end effector, keeping the figure balanced, and avoiding collisions. It is not always easy to incorporate these constraints into a conventional inverse kinematics formulation. To make matters worse, multiple and possibly conflicting constraints can be active, and the system is usually under-determined or over-determined. Next, few robots have more than six joints. On the other hand, a virtual human model may have over a hundred degrees of freedom. Traditional inverse kinematics algorithms can break down or become unacceptably slow for the highly redundant systems that occur in computer animation.

Nonlinear programming is a numerical technique to solve for minima of nonlinear functions. The solution search maintains numerical efficiency and robustness; the intermediate values from the starting state to the final one could be in general fairly irregular. There are two classes of nonlinear programming problems. One is unconstrained nonlinear programming, where the variables are free to take any values; the other one is constrained nonlinear programming, where the variables can only take values in a certain range. The constraints on the variables fit exactly to joint limits of articulated figures. Although the latter problem can be theoretically reduced to the former one, both unconstrained and constrained nonlinear programming problems have been studied extensively because simple reduction may result in numerical instability.

We will offer a feasible approach to the inverse kinematics problem based on nonlinear programming. Because of the complex nature of nonlinear functions, many efficient nonlinear programming algorithms terminate when they find local minima.

The algorithm we choose has this limitation, too. In practice, however, this is not an unacceptably serious problem. Local minima are less likely when the target configuration is not too distance from the start one. If local minima are encountered during interactive manipulation, users can easily perturb the figure configuration slightly to get around the problem. Often, additional spatial constraints will be help guide a solution away from local minima.

Next, we will introduce the pseudo-inverse control with homogeneous term, which was first introduced in [5]. And then we will improve it to find the joint angles for given multiple end effectors.

The organization of the chapter is as follows. In the section to follow the end effector formula is given. Then, in §3 we explain nonlinear programming approach based on such formula. We introduce the pseudo-inverse control in §4. In §5 and §6 we extend to dual and multiple end effectors and in §7 we regularize our algorithm to avoid the singularity.

2 End Effector Formula

First, we will define state variables.

Let R_x(), R_y(), R_z() be the rotation matrix of angle with x-, y-, z-axis, respectively. Then the matrix forms are as follows:

Define rotation R : ³ GL(3, ) as

,

,

Consider an articulated model composed of N rigid segments and N joints. Let , be the set of all possible end effectors and state vectors

We can specify a wide variety of goals for the end effector. Multiple and disjunctive goals are also permitted.

1. position goals: The user wants to position the end effector and is unconcerned about the final orientation.

2. orientation goals: The user is only interested in orienting the end effector.

3. aiming at goals: It is sometimes necessary to aim a line on the end effector. The goal is reached if and only if the ray emanating from the position of end effector in a direction passes through a point in space.

4. plane goals: The user wants to lie on a plane specified by a point and a normal vector.

We will choose position goals in this chapter. So the end effector has three degrees of freedom.

Now we can compute the end effector formula as follows:

For 1 < j, n < N, define P_j⁽ⁿ⁾ as position of j-th joint when state vector is

End effector is obtained by sequential application of above equation:

And for = x, y, z and j = 1, 2, , N, we can get

Thus gradient of f( is

3 Nonlinear Programming Approach

For given target end effector X = (X₁, X₂, X₃) ³, our problem is to find a state variable = (₁, ₂, , _3N ) ^3N such that

The gradient F = (, , ) of the cost functional F is as follows:

We compute the trajectory of end effectors Y ₁, Y ₂, , Y _M as follows:

Let Y be an initial end effector and X a target end effector. It shows natural behavior that we divide the segment Y X equally - denote the internal points by Y ₁, Y ₂, , Y _M - and find Euler angles in sequence for each internal points: initial guess Y and then target end effector Y ₁, initial guess Y ₁ and then target end effector Y ₂, ,initial guess Y _M and target end effector X. It is also effective to solve our inverse kinematics problem.

For i = 1, 2, , M, since

For more smooth behavior we can select the internal points , , , satisfying the followings:

Figure 3: *: Trajectory of Target End Effectors, : Regularization Parameter

To solve the above constrained minimization problem we use L-BFGS-B method, which is a limited memory algorithm for solving large nonlinear optimization problems subject to simple bounds on the variables. For L-BFGS-B method, see [21] and [22].

Example 3.1. Consider a simple joint chain with an end effector in 2-dimensional space. Figure 2 shows a joint chain where the joint angles _i are relative angles between adjacent links. The end effector position is given by

where _i = _{j = 1}ⁱ_j,  i = 1, 2, 3.

Figure 3 shows the result.

4 Pseudo-inverse Control

Consider the problem to find the solution = (t) ⁿ at every instant time of the vector equation:

Equation (4.1) may have no solution, a single solution, or an infinite number of solutions. The latter case which requires m < n is considered in this correspondence. To solve the problem we can make use of the linearized model of (4.1) by considering small displacements about the current :

The various solutions proposed so far are equivalent in the sense that they use, more or less explicitly, a generalized inverse G of J such that

Proof. There exist nonsingular matrices P and Q such that Letting where U, V and W are arbitrary, it is easily verified that AGA = A.

Theorem 4.2. If the set (4.2) of linear equations is consistent, then a solution is given by

where G = G() is a generalized inverse matrix, of dimension n × m, of the matrix J. Also, for any generalized inverse G, d = GdX is a solution of the set (4.2).

Proof. Assume that J is of rank n < m. Then there exist nonsingular matrices P and Q such that We compute Then Q P is a generalized matrix by the proof of the above theorem. The proof of the case that J is of rank m < n is similar.

Let be a solution of dX = Ad and choose any generalized inverse G of J. Then since AGA = A and A = dX,

Thus, d = GdX is a solution of dX = Ad.

Theorem 4.3. The set of linear consistent equations

admits as a solution

where

1) G₁ and G₂ are generalized inverse matrices of J, that is,

and

2) I_n is the n × n identity matrix,

3) Z is an arbitrary vector in ⁿ.

Proof. Since

d = G₁dX + (G₂J - I_n)Z is a solution of dX = Jd. The last equality follows by the fact that d = G₁dX is a solution of dX = Jd.

Proof. Since I_n - G₂J is a projection matrix and since for any Z, the range-space of I_n - G₂J is the subset of the null-space of J. And if we choose any Z ⁿ in the null-space of J, (I_n - G₂J)Z = Z - G₂(JZ) = Z holds and it follows that Z is an element of range-space of I_n - G₂J. Therefore the range-space of I_n - G₂J is exactly the null-space of J.

The definition of the pseudo-inverse is as follows.

Denition 4.5. (Moore-Penrose) A pseudo-inverse A⁺ of an m × n matrix A is an n × m matrix satisfying

where X^T denotes the transpose of the matrix X.

These conditions amount to the requirement that AA⁺ and A⁺A are orthogonal projections onto the ranges of A and A^T , respectively.

It is well-known that the pseudo-inverse A⁺ of a matrix A is unique and it can be calculated using the singular value decomposition.

Theorem 4.6. If A is a real m×n matrix then there exist orthonormal matrices U ^m×m and V ^n×n such that

where ₁ > ₂ > > _k > 0 for some k < min{m, n}.

Theorem 4.7. The pseudo-inverse matrix A^{+ n×m} is given by

where

Proof. Let U = [u₁, u₂, , u_m] and V = [v₁, v₂, , v_n]. For any x ⁿ, we have

where = [₁, ₂, , _n]^T = V ^T x. Clearly, if x solves the least-square problem, then Therefore if we set _i = 0 for i = k + 1, , n,

In particular, if an m × n matrix A is of rank k = m < n, then the pseudo-inverse A⁺ is given by A⁺ = A^T (AA^T )^-1. Then

On the contrary, if an m × n matrix A is of rank k = n < m, then the pseudo-inverse A⁺ is given by A⁺ = (A^T A)^-1A^T . Then

5 Control of Dual End Effectors

Now, let’s control the highly redundant articulated figure that has dual end effectors.

Let J₁ be an m₁ × n matrix of rank m₁ < n and J₂ an m₂ × n matrix of rank m₂ < n. We can assume that m₁ + m₂ < n.

Proof. Since J₁⁺ is a generalized matrix of J₁, J₁ = J₁J₁⁺J₁ and it implies And since J₂(I - J₁⁺J₁) is of full rank,

Proof. Since J₂(I - J₁⁺J₁) is of full rank, Thus using the fact that I - J₁⁺J₁ is symmetric,

Proof. Since

(I - J₁⁺J₁)[I - {J₂(I - J₁⁺J₁)}⁺J₂(I - J₁⁺J₁)] is a projection and since

for any Z, the range-space of (I - J₁⁺J₁)[I - {J₂(I - J₁⁺J₁)}⁺J₂(I - J₁⁺J₁)] is the subset of the intersection of null-spaces of J₁ and J₂. And if we choose any Z ⁿ in the intersection of null-spaces of J₁ and J₂,

holds and it follows that Z is an element of range-space of (I - J₁⁺J₁)[I - {J₂(I - J₁⁺J₁)}⁺J₂(I - J₁⁺J₁)]. Therefore the range-space of (I - J₁⁺J₁)[I - {J₂(I - J₁⁺J₁)}⁺J₂(I - J₁⁺J₁)] is exactly the intersection of null-spaces of J₁ and J₂.

Theorem 5.5. The set of linear consistent equations

admits as a solution

for any Z.

Let

But we can show that

Let J = and G = ^T .

Proof. Since

we can obtain JGJ = J,  GJG = G and (JG)^T = JG. And since

GJ is symmetric:

Thus all Moore-Penrose conditions hold.

We can rewrite (5.1):

Since J⁺ is the pseudo-inverse of J = ,

Example 5.7. Consider a simple two-dimensional example. Figure 4 shows a two-dimensional joint chain where the joint angles _i are relative angles between adjacent links. The end effector positions are given by

where _i = _{j = 1}ⁱ_j,  i = 1, 2, , 6 and _6+i = _{j = 1}³_j + _{j = 1}ⁱ_6+j,  i = 1, 2, 3.

Differentiating yields the Jocobians:

where

If an initial state and the position of a target end effector are given, we can compute trajectories of end effectors and by (5.3) obtain euler angles for each end effector as Figure 5 and Figure 6.

6 Control of Multiple End Effectors

Let’s extend our approach to the case that the articulated figure has multiple end effectors. Suppose that the number of end effectors is M. And suppose that _{i = 1}^Mm_i < n, J_i is an m_i × n matrix of full rank for i = 1, 2, , M and

Lemma 6.1. For all i = 1, 2, , M there exists an n × m_i matrix Z_i such that

for all j = 1, 2, , M.

Proof. Let Z = be the pseudo-inverse matrix of J. Since J is of full rank, for all i, j = 1, 2, , M.

Theorem 6.2. The set of linear consistent equations

admits as a solution for any j {1, 2, , M}.

Proof. First,

And if lj,

Lemma 6.4. If

then J₁X = I,  J₂X = J₃X = O.

Proof. Let P = I - [J₂(I - J₃⁺J₃)]⁺J₂ - [J₃(I - J₂⁺J₂)]⁺J₃. By the above lemma P² = P and P^T = P. Then Thus J₁X = J₁P(J₁P)⁺ = I. And

Therefore J₂X = J₃X = O.

Proof. Since J₁N_2,3 is of full rank,

Lemma 6.7.

represents the projection of Z onto the intersection of null-spaces of J₁, J₂ and J₃.

Proof. Since

N_2,3{I - (J₁N_2,3)⁺J₁N_2,3} is a projection and since

for any Z, the range of N_2,3{I - (J₁N_2,3)⁺J₁N_2,3} is the subset of the intersection of null-spaces of J₁, J₂ and J₃. And if we choose any Z ⁿ in the intersection of null-spaces of J₁, J₂ and J₃,

holds and it follows that Z is an element of range-space of N_2,3{I - (J₁N_2,3)⁺J₁N_2,3}. Therefore the range-space of N_2,3{I - (J₁N_2,3)⁺J₁N_2,3} is exactly the intersection of null-spaces of J₁, J₂ and J₃.

Theorem 6.8. The set of linear consistent equations

admits as a solution where Z is any vector,

and

Next, we generalize for any M. We can obtain the exact formula of the general solution recursively in case that the number of end effectors is M.

Lemma 6.9. For i, j = 1, 2, , M such that ij, suppose {Z_i^j} satisfies

i.e., Let for j = 1, 2, , M. Then

Proof. First, since (Z₁^k  Z_{k - 1}^k Z_{k + 1}^k  Z_M^k) is a generalized inverse of (J₁  J_k-1 J_k+1  J_M )^T , we can obtain If j = k then

and if jk then

1,2,,i

Denition 6.10.

Proof. The proof is by induction. It is trivial for i = 1. Assume for i = k.

For i = k + 1,

and since J_k+2N_1,2,,k is of full rank m_k+2, I - (J_k+1N_1,2,,k)⁺J_k+1N_1,2,,k is of rank n - m_k+1 and m_k+2 < n - m_k+1, is of full rank m_k+2. And since J_k+2N_1,2,,k+1 is of full rank,

If j = 1, 2, , k, and if j = k + 1, Thus J_jN_1,2,,k+1 = 0 for j = 1, 2, , k + 1. For any Z in the intersection of null-spaces of J₁, J₂, , J_k+1,

Theorem 6.13. The set of linear consistent equations

admits as a solution for any vector Z.

Let F_i = N_1,2,,i,  i = 1, 2, , i.e., define {F_i} satisfies as follows.

Denition 6.14.

And define {} as follows:

Denition 6.15.

Proof. The proof is by induction. It is trivial for i = 1. Assume for i = k. For i = k + 1,

and

Proof. Let J = . Since J = I, JJ = J, J = and (J)^T = J. And since J = I - F_i and F_i is symmetric, (J)^T = (I - F_i)^T = I - F_i^T = I - F_i = J. Therefore is the pseudo-inverse of J.

Theorem 6.18. The set of linear consistent equations

admits as a solution for any vector Z.

In particular, if M = 3 then the minimal 2-norm solution is

Example 6.20.

---

Figure 7: Joint Chain with Three End Effectors

---

Consider a simple two-dimensional example with 3 end effectors. Figure 7 shows a two-dimensional joint chain where the joint angles _i are relative angles between adjacent links. The initial and target end effectors are as follows:

The end effector positions are given by

where

---

Figure 8: When all the three end effectors move

---

Differentiating yields the Jacobians:

where

and O_2×i is the 2 × i zero matrix.

Thus we can calculate

and

Then the solution is d₁ = dX₁ + dX₂ + dX₃. Figure 8 shows the result.

Example 6.21.

---

Figure 9: When one end effector is inserted in Figure 8

---

Consider an example with 4 end effectors. Suppose that one end effector is inserted in previous example: the initial and target end effectors are P₂ and P₂ + (0, -1), respectively. The 4th end effector position is given by

Differentiating yields the Jacobian:

where

Thus we can calculate

and

Then the solution is d₁ = dX₁ + dX₂ + dX₃ + dX₄. Figure 9 shows the result.

7 Regularized Formulation

Consider the configuration of the joint chain with end effector 5 in Figure 10. The joint chain is approaching the singular configuration which occurs when the joint chain is completely stretched out. As result the end effector 5 deviates from the target end effector.

A general approach to resolving the discontinuity at singular configurations and maintaining a well-conditioned formulation which results in physically meaningful joint velocities is to use the damped least squares formulation independently proposed in [11] and [12]. The damped least squares solution of dX = Jd is the solution which minimizes the sum

Lemma 7.1. Let

for orthonormal matrices U and V . Then Thus if > 0, then J^T J + ²I is nonsingular and

Proof. Since J = Udiag(₁, ₂, , _m)V ^T ,

Since > 0, J^T J + ²I = V diag(₁² + ², ₂² + ², , _m² + ², ², , ²)V ^T is nonsingular and

Lemma 7.2. If r = dX - J where is a minimizer of ||dX - Jd||² + ²||d||², then

Proof. Since = (J^T J + ²I)^-1J^T dX, r = {I - J(J^T J + ²I)^-1J^T }dX. Thus Since ||dX|| > 0,

And since dX - r = J,

Thus we can obtain

Figure 11: = 0.1; 1 = 12.3701,  4 = 0.1495 at 5

Figure 12: = 0.25; 1 = 12.3812,  4 = 0.1669 at 5

Figure 13: = 1.5; 1 = 12.1607,  4 = 0.5714 at 5

Figure 14: = 10.0; 1 = 11.7280,  4 = 0.5837 at 5

Figure 15: = 0.25; - < i < if i6 and - < i < 0 if i = 6

Lemma 7.1 shows that a large brings the condition number arbitrary close to unity. In contrast, by Lemma 7.2 should be small enough that the residual error is not greatly increased.

Now, suppose that (7.1) has simple bound constraints:

References

E-mail address: no3khs@snu.ac.kr

E-mail address: sheen@snu.ac.kr