IK CCD Problem

[clauchiorean]

Hi guys.
I have question for you.

I'm trying to implement ik with CCD and I got in some type of problem or i found a bug in CCD ( probably not the last :) )

Here is my ccd algorithm:


bool CCD::Compute(Joint* base, Joint* end, Target target)
{
//TO DO
/*
add constraints
do something when the cross product is 0
this happens when the curVector and targetVector are colinears


*/
Vector3 rootPos, curEnd, targetVector, desiredEnd, curVector, crossResult, endPos = target.GetPosition();
double cosAngle,turnAngle;
Joint* link;
int tries;

// start at the last link in the chain
link = end->GetParent();
tries = 0;
do
{
rootPos = link->GetWorldPosition();
curEnd = end->GetWorldPosition();
desiredEnd = endPos;
double distance = Vector3::DistanceSqared(curEnd, desiredEnd);

// see if i'm already close enough
if (distance > 0.1)
{
// create the vector to the current effector pos
curVector = curEnd - rootPos;
// create the desired effector position vector
targetVector = endPos - rootPos;

// normalize the vectors (expensive, requires a sqrt)
curVector.Normalize();
targetVector.Normalize();

// the dot product gives me the cosine of the desired angle
cosAngle = curVector.DotProduct(targetVector);
// if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
if (cosAngle < 1.0)
{
// use the cross product to check which way to rotate
crossResult = curVector.CrossProduct(targetVector);

crossResult.Normalize();
turnAngle = acos(cosAngle); // get the angle

Quaternion rotation = Quaternion::FromAngleAxis(turnAngle, crossResult);
rotation.Normalize();

link->Rotate(rotation);
link->Update();
}

if (link->GetParent() == NULL)
{
link = end->GetParent(); // start of the chain, restart
}
else
{
link = link->GetParent();
}
}
// quit if i am close enough or been running long enough
} while (++tries < maxTries &&
Vector3::DistanceSqared(curEnd, desiredEnd) > 0.1);

if (tries == maxTries)
{
return false;
}

return true;
}


Basically it's identical with the one from here:
http://www.darwin3d.com/gdm1998.htm

My problem is when curVector and targetVector become collinear
Then the crossproduct between them will be zero and you don't know the rotation axis.

Basically the quaternion will give a 0 degree rotation

This happens when the bones are in a configuration like this:

[n] being the joints
- being the links
x being the target

[1]----[2]----[3]--x-[4]----[5]

and the target is on the same line lets say between joint [3] an [4]

For now i did not impose any constraints on the joints , i just left them to move freely

I well man research thesis it says that the CCD method works well around singularities.

Well this is a singularity situation i apparently the algorithm does not work
or is my implementation which is not good.

Hope I'm wrong and I need to change something in the implementation.

Where is a picture with situation:
http://img530.imageshack.us/my.php?image=problemdq9.jpg

If someone has an idea about this :d

[Omni]

ever thought of the case that cosAngle == -1 ? :D
also, checking for exactly < 1.0 seems a bit unsave to me. maybe you should introduce a small epsilon-value

if( fabs(fabs(cosAngle) - 1.0) > epsilon )

[clauchiorean]

I thought about that and that is my problem right now.
I've changed the condition o something like 0.999999

But the problem is how to treat this case. Make a slight variation of the target position or move a little the joint so that the 2 vectors are not collinear any more?

In the papers I've read they don't mention anything about this problem ( the singular position in which the system is right now )

[Omni]

well the problem is solely, that you cannot easily calculate a rotation axis. why? because in the case of curVector collinear to targetVector, the set of vectors perpendicular to those 2 vectors doesn't reduce to a single vector. but that should'n be a problem. all you have to do is choose one.
one way of choice could be the following:
we wanna choose a rotation axis (x,y,z)
1. request dot product to vanish with curVector (a,b,c) [normalized]
xa+by+cz = 0
2. find the smallest component of curVector: min(|a|,|b|,|c|)
[ w.l.o.g. be min = |a| ]
request, that the corresponding component of rotation axis is 0.5
[ w.l.o.g. x = 0.5 ]
3. put this into the dot product, solve for component of rotation axis corresponding to the biggest component of curVector ( max(|a|,|b|,|c|) )
[ w.l.o.g. be max = b => y = (-cz - 0.5*a)/b ]
4. request that rotation axis is normalized
x²+y²+z² = 1
[ w.l.o.g. 0.25+(-cz - 0.5*x)²/b² + z² = 1 ]

solve for z and you have your rotation axis

little example:
curVector = (1,0,0)
from dot product follows
=> x = 0
min(|1|,|0|,|0|) is 0
so y = 0.5
normalization
y²+z² = 0.25 + z² = 1
=> z = sqrt( 0.75 )

[VladAndreev]

I well man research thesis it says that the CCD method works well around singularities.


CCD doesn't work well around anything. Making it work well in practice is downright nasty.

Omni's suggestion is based on the observation that in the absense of a unique frame, picking an arbitrary frame is a reasonable thing to do. This is fine, but the particular choice of frame given may exacerbate one of the more unpleasant sides of CCD: it's numerically unstable, in that a tiny change in the end effector's position may produce a drastic change in the configuration.

One solution is to cache the last good rotation axis for each joint, and Gram-Schmidt it if one doesn't exist. A side benefit is that this translates nicely to applying IK to canned animation with no explicit joint constraints.