Simple Kinematics

The term kinematics means a lot of things. To a 3D artist it means one thing, to the 3D game programmer it means another, and to a physicist it means yet another. However, in this section of the book it means the mechanics of moving linked chains of rigid bodies. In computer animation there are two kinematic problems. The first is called forward kinematics and the second is inverse kinematics. The forward kinematic problem is shown in Figure 13.38; here you see a 2D serially linked chain of rigid bodies (straight arms). Each joint can freely rotate in the plane, thus there are 2 degrees of freedom in this example, q₁, and q₂. In addition, each arm has length l₁ and l₂, respectively. The forward kinematic problem can be stated as follows:

Given q₁, q₂, l₁, and l₂, find the position of p₂.

Why are we interested in this? Well, if you are going to write a 2D or 3D game and want to have real-time models that have links that move around, then you better know how to do this. For example, 3D animation is accomplished in two ways. The quick and dirty method is to have a set of meshes that represent the 3D animation of an object. The more flexible method is to have a single 3D mesh that has a number of joints and arms and then to "play" motion data through the 3D model. However, to do this you must understand the physics/mechanics of how to move a hand in relation to the wrist, in relation to the elbow, in relation to the shoulder, in relation to the hips, and so forth—see the problem?

The second kinematic problem is the converse of the first:

Given the position of p₂, find values q₁, q₂ that satisfy all the constraints l₁ and l₂ of the physical model. This is much harder than you can imagine. Figure 13.39 shows an example of why this is true.

Referring to the figure, you see that there are two possible solutions that satisfy all the constraints. I'm not going to tackle this problem in general since in most cases we never need to solve it, and the math is gnarly, but I will give an example later in the section of how to go about it.

Solving the Forward Kinematic Problem

What I want to do is show you how to solve the forward kinematic problem because it's almost trivial. Referring back to Figure 13.38, the problem is nothing more than relative motions. If you look at the problem from joint 2, then locating p₂ is nothing more than a translation of l₂ and a rotation of q₂. However, the point p₂ itself is just located as a translation from p₁ of l₁ and a rotation of q₁. Thus, the solution of the problem is nothing more than a series of translations and rotations from frame to frame or link to link. Let's solve the problem in pieces.

Forget about the first arm and just focus on the second, that is, let's work our way backward. The starting point is p₁ and we want to move a distance l₂ out on the x-axis and then rotate an angle q₂ around the x,y plane (or the z-axis in 3D) to locate the point p₂. This is easy—all we need to do is the following transformation from p₁:

p2 = p1*Tl2*Rq2

But we don't have p₁? That's okay—we assume that we do for the derivation. Anyway, T_l2 and Rq₂ are the standard 2D translation and rotation matrices you learned about in Chapter 8, "Vector Rasterization and 2D Transformations." Therefore, we have

      |1  0  0|        | cos q2 sin q2   0|
Tl2 =  |0  1  0|   Rq2 = |-sin q2 cos q2  0|
      |l1 0  1|         |  0      0     1|

Therefore, p₂ is the product:

          |1  0  0|   | cos q2 sin q2  0|
p2 = p1 *  |0  1  0| * |-sin q2 cos q2  0|
          |l2  0  1|   |  0     0      1|

Okay, if we can compute p₂ from p₁, then p₁ should be p₀ transformed in the same way, that is, translated by l₁ and rotated by q₁. Or mathematically:

p2 = p0*Tl2*Rq2*Tl1*Rq1

where p₀ = [0,0,1], that is, the origin in homogenous 2D coordinates (we could use any point if we wanted, but basically this represents the base of the kinematic chain). The reason for three components in a 2D system is so we can use homogenous transforms and accomplish translation with matrices, hence that last 1.0 is a place holder. All points are in the form (x,y,1). Furthermore, T_l1 and Rq₁ are of the same form as T_l2 and Rq₂, but with different values. Note the order of multiplication—since we're working backward, we must first transform p₀ by T_l2*Rq₂ then by T_l1*Rq₁, so order counts!

With all that in mind, we see that the point p₂ is really just the starting point p₀ multiplied by the matrices (T_l2*R_q2) * (T_l1*R_q1). This holds for as many links as needed, or in general:

pn = p0 * Tn*Rn * Tn-1*Rn-1 * Tn-2*Rn-2 *...* T1*R1

for n links.

This works because each matrix multiplication pair T*R transforms the coordinate system relative to the link, hence, the products of these transforms is like a sequence of changing coordinate systems that you can use to locate the end point. As an example, let's see if this mumbo jumbo works. Figure 13.40 depicts a carefully worked out version of the problem on graph paper.

I have labeled the points, angles, and so on, and using a compass and ruler computed the position of p₂ given the input values:

l1 = 3, l2 = 5
q1 = 45, q2 = 90
p0 = (0,0)

I roughly estimate from the figure that

p2 = (-1.4,5.6)

Now, let's see if the math gives us the same answer.

             |1 0 0| | 0 1 0|  |1 0 0| | .707 .707 0|
p2 = [0,0,1]*|0 1 0|*|-1 0 0|* |0 1 0|*|-.707 .707 0|
             |5 0 1| | 0 0 1|  |3 0 1| |  0    0   1|
       p0       Tl2      R_2       Tl1         R_1
             | 0  1  0|  * | .707  .707  0|
   = [0 0 1]*|-1  0  0|  * |-.707  .707  0|
             | 0  5  1|  * |2.121 2.121  1|
       p0       Tl2*R_2          Tl1*R_1

             |-.707   .707   0|
   = [0 0 1]*|.707    .707   0|
             |-1.414 5.656   1|
       p0        Tl2*Rq2*Tl1*Rq1

p2 =[-1.414, 5.656,1]

Discarding the 1.0 since [x,y,1] really means, x' = x/1, y'=y/1, or x'=x, y'=y, we have:

p2 =(-1.414, 5.656).

If you look at Figure 13.40, it looks pretty close! That's all there is to forward kinematics in 2D. Of course, doing it in 3D is a bit more complex due to the z-axis, but as long as you pick a rotation convention then it all works out. I created DEMO13_9.CPP|EXE (16-bit version, DEMO13_9_16B.CPP|EXE) shown in Figure 13.41, as an example of forward kinematics. It lets you change the angle of the two links and then computes the positions p1, p2 and displays them. The keys A, S, D, and F control the angles of link 1 and link 2, respectively. See if you can add a restraint to the program, so the end effector at p₂ can't drop below the y=0 axis, shown by the green line.

Solving the Inverse Kinematic Problem

Solving inverse kinematics is rather complex in general, but I want to give you a taste of it so you can at least know where to start. The previous section solved for p₂ knowing p₀, l₁, l₂, q₁, q₂. But what if you didn't know q₁, and q₂, but knew p₂? The solution of the kinematic problem can be found by setting up a system of restraint equations and then solving for the unknown angles. The problem is that you may have an underdetermined system, meaning that there is more than one solution. Thus, you must add other heuristic or constraints to find the solution you want.

As an example, let's try a simpler problem with only one link, so you can see the process. Figure 13.42 shows one link l₁ making an angle q₁ with the x-axis. Given p₁(x₁,y₁), what is q₁?

We can use the forward kinematic matrices to solve the problem like this:

p1 = p0*Tl1*Rq1
                  |1  0  0| | cos q1 sin q1  0|
p1(x1,y1) =[0 0 1]*|0  1  0|*|-sin q1 cos q1  0|
                  |l1 0  1| |  0      0      1|
             p0       Tl1          Rq1

                    | cos q1 sin q1  0|
          =[l1 0 1]*|-sin q1 cos q1  0|
                    |  0      0     1|
            p0*Tl1          Rq1

p1(x1,y1)   =  (l1*cos q1, l1*sin q1,  1)

Therefore,

x1 = l1*cos q1
y1 = l1*sin q1

q1 = cos –1 x1/l1

or,

q1 = sin –1 y1/l1

Okay, this system is overdetermined; in other words, once you select x or y then the other is determined via q₁. This is interesting, but if you think about it then it makes sense—the arm link l₁ causes us to lose a degree of freedom therefore, you can't locate any point you wish (x,y) anymore, in fact, the only points that are valid anymore are of the form:

x1 = l1*cos q1
y1 = l1*sin q1

If this had two links, then you would see that for any x,y there would be more than one solution set q₁, q₂ that satisfied the equations along with a relationship between q₁, and q₂.