Inverse kinematics: Defining a Jacobian of rotation

[aubrey.r.jones]

I'm working on an inverse kinematics problem (I make video games), and I'm reaching a bit beyond my education.

Right now, I've got an algorithm that solves the basic IK equation for a chain of rigid bodies connected by joints by approximately inverting ##J\Delta\theta = e##. Where ##\theta## are the joint parameters, ##J## is the Jacobian matrix of position for my joint chain, and ##e## is the offset vector between the current tip position and the target tip position. The Jacobians for each of the joints is a simple 3d vector, which is a pretty good approximation of the positional derivative. I approximate the solution with a least-squares SVD solver (and I actually solve a damped version of that equation, but I don't believe that's relevant). This all works beautifully.

This weekend I decided to extend my IK solver to also target a tip rotation. This has gone less well, and I need some help.

I have ##i## joints, each with free parameter ##\theta## which encodes an angle of rotation around fixed axis ##a##. ##R_{i}## is the rotation matrix created by the usual angle/axis method for joint ##i##. To find the rotation of the chain tip ##R_{s}##, I concatenate ##R_{1...i}##. This works fine, as I use it in my integration step.

Now, to find the Jacobian matrix of my joint chain. From the IK literature, I believe I'm looking for $$J(\theta) = (\frac{\partial R_{s}}{\partial \theta})_i$$
First I tried taking the partial derivative of my angle/axis formula. Here my lack of education shows itself, as I don't know how to say what's wrong with that answer. As far as I can tell, the Jacobian approximation assumes that scaling the joint's Jacobian column is approximately equivalent to varying ##\theta##. If the matrix has a bunch of ##\cos\theta## terms, I don't think I can just dump it in a vector to be scaled by ##\Delta\theta##.

Then, after exhaustive search, I found Euler's "generator of rotation". After playing with it in maxima for a little bit, and comparing it to the position Jacobians in my reference papers, I really think this is the right thing. It encodes the fixed plane of rotation, and for small changes in angle is an okay approximation for the change in rotation. And if I leave out the identity matrix, and deal only with the skew-symmetric part, it scales linearly with ##\theta##, and is manipulated in a plausible way by matrix operations. And it looks a little like a cross product, which also makes me like it.

So I spent today implementing the code necessary to make it work. But I made a mistake. I assumed I could make ##J(\theta)## a (4,##i##*4) matrix, and that my ##e## could be a 3d pseudo-transform! It can't! I mean, I still think that it's a reasonable approximation of the solution, but my numerical solvers can't do anything with it. They need a vector for their right-hand side. And so ##J(\theta)## needs to be a (6,##i##) matrix--six degrees of positional freedom, by ##i## joints.

This means that I need to turn my generators of rotation into a vector some how. I know of quaternions. But I don't see how to encode an infinitesimal rotation as a quaternion. And I don't think a quaternion is going to behave in any meaningful way when it's mutilated by the SVD solver.

Could somebody point me in the right direction? Or, tell me that I'm barking up the wrong tree?

 

[aubrey.r.jones]

Alright, I'm quite embarassed now by this question.

I've been solving in linear velocity the whole time, not position. I think it got in my head that it was position since the algorithm iterates to converge on a position. Similarly, it wasn't obvious to me because it's "velocity" parameterized by ##\theta## instead of time.

So the rotational analogue is angular velocity. Which is vector valued, and fits right into my Jacobian matrix. Duh. Although, given the relationship between the skew-symmetrical matrix, the generator of rotation, and the angular velocity, I was on the right track.

Also, my solvers of *course* work with rectangular right-hand sides. It's just a 4x24 matrix times a 9x1 matrix isn't a 4x4 matrix. Which should have tipped me off that my math was incorrect.