Tricks of the Windows Game Programming Gurus:Playing God: Basic Physics Modeling

All ITKnowledge Source Code Search Tips Advanced Search |

| Tricks of the Windows Game Programming Gurus by Andre LaMothe Sams, Macmillan Computer Publishing ISBN: 0672313618 Pub Date: 09/27/99

Search this book:

Previous | Table of Contents | Next

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.

Figure 13.40 A kinematic chain worked out on 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

    θ1 = 45, θ2 = 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*Rθ2*Tl1*Rθ1

    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 z-axis, but as long as you pick a rotation convention then it all works out. I created `DEMO13_9.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.

Figure 13.41 The kinematic chain demo.

Math I could have kept the entire problem in matrix form, but this is more illustrative.

#### 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, θ1, θ2. But what if you didn’t know θ1, and θ2, 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 θ1 with the x-axis. Given p1(x1,y1), what is θ1?

Figure 13.42 A single link inverse kinematic problem.

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

    p1 = p0*Tl1*Rθ1

                      |1  0  0| | cos θ1 sin θ1  0|

    p1(x1,y1) =[0 0 1]*|0  1  0|*|-sin θ1 cos θ1  0|

                      |l1 0  1| |  0      0      1|

                 p0       Tl1           Rθ1

                        | cos θ1 sin θ1  0|

              =[l1 0 1]*|-sin θ1 cos θ1  0|

                        |  0      0     1|

                p0*Tl1          Rθ1

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

Therefore,

    x1 = l1*cos θ1

    y1 = l1*sin θ1

    θ1 = cos –1 x1/l1

or,

    θ1 = sin –1 y1/l1

Okay, this system is overdetermined; in other words, once you select x or y then the other is determined via θ1. This is interesting, but if you think about it then it makes sense—the arm link l1 causes us to loose 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 θ1

    y1 = l1*sin θ1

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

Previous | Table of Contents | Next

Copyright © Macmillan Computer Publishing, Inc.

Products | Contact Us | About Us | Privacy | Ad Info | Home
Use of this site is subject to certain Terms & Conditions, Copyright © 1996-2000 EarthWeb Inc. All rights reserved. Reproduction in whole or in part in any form or medium without express written permission of EarthWeb is prohibited. Read EarthWeb's privacy statement.