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, q1, and q2. In addition, each arm has length l1 and l2, respectively. The forward kinematic problem can be stated as follows:
Given q1, q2, l1, and l2, find the position of p2.
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?
Given the position of p2, find values q1, q2 that satisfy all the constraints l1 and l2 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 p2 is nothing more than a translation of l2 and a rotation of q2. However, the point p2 itself is just located as a translation from p1 of l1 and a rotation of q1. 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 p1 and we want to move a distance l2 out on the x-axis and then rotate an angle q2 around the x,y plane (or the z-axis in 3D) to locate the point p2. This is easy—all we need to do is the following transformation from p1:
p2 = p1*Tl2*Rq2
But we don't have p1? That's okay—we assume that we do for the derivation. Anyway, Tl2 and Rq2 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, p2 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|
p2 = p0*Tl2*Rq2*Tl1*Rq1
where p0 = [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, Tl1 and Rq1 are of the same form as Tl2 and Rq2, but with different values. Note the order of multiplication—since we're working backward, we must first transform p0 by Tl2*Rq2 then by Tl1*Rq1, so order counts!
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 p2 given the input values:
l1 = 3, l2 = 5 q1 = 45, q2 = 90 p0 = (0,0)
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]
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 p2 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 p2 knowing p0, l1, l2, q1, q2. But what if you didn't know q1, and q2, but knew p2? 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 l1 making an angle q1 with the x-axis. Given p1(x1,y1), what is q1?
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)
x1 = l1*cos q1 y1 = l1*sin q1 q1 = cos –1 x1/l1
q1 = sin –1 y1/l1
I could have kept the entire problem in matrix form, but this is more illustrative.
Okay, this system is overdetermined; in other words, once you select x or y then the other is determined via q1. This is interesting, but if you think about it then it makes sense—the arm link l1 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