Homework 7 is due on Tuesday Nov. 24 at 11:59 pm
Your second program will demonstrate the idea of inverse kinematics. In this program, we give you an articulated body (a robot arm) and the robot gets a random target input three-dimensional point for the tip of the robot arm to touch. Your program will then determine the joint parameters in order for the robot arm to reach this position. Finally, you will animate the arm transitioning to its new position.
We will now define our robot arm setup. First we define the
"unit subarm" to be a joint that rotates about the length of the segment
(roll) followed by a segment that rotates about an axis perpendicular to
a plane containing the segment (yaw/pitch). A complete robot arm will
be multiple subarms linked together, i.e., alternating segments that rolls and
yaws. The image on the left shows a unit subarm and the image on the
right is a arm made up of 2 subarms.
Mark Meyer's slides on animation are useful, starting with the slide on Kinematics. Read the handout on the mathematics behind the kinematics. Although the exact setup of the robot arm is different, but the idea is the same -- we compute the jacobian and try to find change in the joint parameters (angles) given a change in tip position. The recitation slide on robot arm will be very helpful, but here is the outline:
1. Position of tip as a nonlinear function of the angles, t:
X = f(t1, t2, ..., tk)
2. We want to move from X to X', i.e., we take a small step along X' - X
deltaX = alpha * unit(X' - X),
where alpha is a small coefficient.
3. Compute the Jacobian, a matrix of partial derivatives, using finite
differencing:
J = dX/dT = [[dX/dt1, dX/dt2, ..., dX/dtk]
[dY/dt1, dY/dt2, ..., dY/dtk]
[dZ/dt1, dZ/dt2, ..., dZ/dtk]]
4. Since J = dX/dT, to first order we have deltaT = J^{-1}deltaX.
One problem is J is not square, so we need to compute (and use)
instead the pseudoinverse, J^+.
5. Now update the angles, using deltaT. At this point the tip of the
arm is just a tad closer to the target X'. Repeat these steps until
convergence.
Instead of writing out the explicit expressions for the tip position x, y, z in terms of the angles t1 through tk (hopefully our setup has discouraged you to do so), we will use matrix transforms to compute the tip position. Assume that our robot arm stems from [0,0,0,1] (homogenized), we reach the tip of the arm by stepping along the arm segments. The sequence of stepping amounts to multiplying a chain of rotation and translation matrices. Rotation will put us in the frame of the cylinder (related to angles t1 ~ tk) and translation will step along the arm segment (related to the length L1 ~ Lk).
Each unit subarm will be associated with Mi = (T2R2T1R1), where R1
is the roll rotation, and T1 steps along the roll segment; R2 is the
pitch/yaw rotation, and T2 steps along the second segment.
Then the overall transformation for an arm from root to the tip is
M = Mk*...*M2*M1
and tip = M * [0,0,0,1]^T
OpenGL can help us with this procedure. We start from the identity,
perform a sequence of glRotated and glTranslated, and we fetch the
overall transformation by calling glGetDoublev.
With that, we do a simple multiplication (in fact we don't even have
to do any multiplication -- it is one column of the matrix) to get the
tip position. Here's the idea reiterated:
Implement the nonlinear function f
Position getTipPosition(angles, armlengths):
glPushMatrix()
glLoadIdentity()
for (angle, length) in zip(angles, armlengths):
glRotatef(...) # watch out -- about which axis?
glTranslatef(...)
M = glGetDoublev(GL_MODELVIEW_MATRIX) # need 2nd argument for c++,no return value
glPopMatrix()
return M * [0,0,0,1].T # you might want to look at the corresponding 3-vector
This is trivial once you have the getTipPosition(angles, lengths) function.
[dX/dtj, dY/dtj, dZ/dtj] =
getTipPosition([t1,...,tj + eps,...,tk) - getTipPosition([t1,...,tk], L)
-------------------------------------------------------------------------
eps
Since getTipPosition returns a 3-vector, each of these operations will
give you three partial derivatives. We repeat k times to get the full
Jacobian. Now use your favorite matrix library to do the pseudoinverse.
Your program should be called inverse and have the following syntax:
inverse L1 L2 L3 L4 ... L{2k}
or
inverse k
where k is the number of unit subarms (meaning there are 2k segments), and L1 .. L{2k} are the lengths of the 2k segments. Default value for each segment is 1. It is ok to not allow variable arm lengths, but you must support arbitrary number of links. Document your choice, and your program input format.
You should calculate a lot of (x,y,z) points (like 100) and store them in a buffer. A small sphere should be visible where the robot is currently travelling to. When the robot touches it, change the target point to the next one in the list and display a sphere at that space. When all 100 points have been touched, just loop through to the beginning of the points and keep going until the user quits.
The method for determining random points should be pretty easy, but it might not be as trivial a task to guarantee that they are reachable. To pick points in range, randomize the angle parameter and find the corresponding tip position. Effectively we know the solution in angle- space, and have it actually solve for it.
Manual mode: You can also add the ability for the user to click a point in your scene (such as one on the ground plane, etc.) for the goal point (Note: this should be instead of the random points). You will want to add some objects in the scene for the user to click and instruct the robot to touch.
Spherical shell interpolation: Instead of interpolating along a straight line from where the robot is to the target point, instead treat the tip position and the target point as a points on a sphere and step along the arc connecting them. On each solve, we take deltaX to be the tangent to the arc. If you do it right, it helps with the popping and strangeness of the math that sometimes comes up. It also looks really smooth.
Fancier render: The design is totally up to you -- something
beyond cylinders that looks reasonably nicer. You can also apply robot-arm
like texture onto cylinders. Here's a sample of a slightly fancier
design/render.
If you set the randomized points correctly, you can just run the robot arm for a while and make sure it doesn't reach any strange configurations where it gets stuck.
Include a README file with your code, saying what you did, how to compile it, and anything else you think we need to know.