Help with Inverse Kinematics Algorithm - c++

Question

I'm trying to implement CCD Inverse Kinematics in 2D
This function is supposed to do 1 iteration of CCD
Right now as a test case I start it on a left foot and have it stop at the pelvis.
every time this function is called, the skeleton's bones are updated.
The way my bones work is:
getFrameX,Y,Angle return the absolute positions of the end of the bone / effector. These are updated every iteraton of CCD.
getAngle,X, Y returns the relative values.
Same for setters.
Right now it never stays in one spot, every time I giggle the mouse a bit it moves the bones randomly counterclockwise.
I was wondering if there was something bluntly obviously wrong that could point me in the right direction for debugging.
void inverseKinematics(float targetX, float targetY, skl::Bone* targetBone)
{
std::string stopBone = "Pelvis";
//===
// Track the end effector position (the final bone)
double endX = targetBone->getFrameX();
double endY = targetBone->getFrameY();
//===
// Perform CCD on the bones by optimizing each bone in a loop
// from the final bone to the root bone
bool modifiedBones = false;
targetBone = targetBone->getParent();
while(targetBone->getName() != stopBone)
{
// Get the vector from the current bone to the end effector position.
double curToEndX = endX - targetBone->getFrameX();
double curToEndY = endY - targetBone->getFrameY();
double curToEndMag = sqrt( curToEndX*curToEndX + curToEndY*curToEndY );
// Get the vector from the current bone to the target position.
double curToTargetX = targetX - targetBone->getFrameX();
double curToTargetY = targetY - targetBone->getFrameY();
double curToTargetMag = sqrt( curToTargetX*curToTargetX
+ curToTargetY*curToTargetY );
// Get rotation to place the end effector on the line from the current
// joint position to the target position.
double cosRotAng;
double sinRotAng;
double endTargetMag = (curToEndMag*curToTargetMag);
if( endTargetMag <= 0.1f )
{
cosRotAng = 1.0f;
sinRotAng = 0.0f;
}
else
{
cosRotAng = (curToEndX*curToTargetX + curToEndY*curToTargetY) / endTargetMag;
sinRotAng = (curToEndX*curToTargetY - curToEndY*curToTargetX) / endTargetMag;
}
// Clamp the cosine into range when computing the angle (might be out of range
// due to floating point error).
double rotAng = acosf( max(-1.0f, min(1.0f,cosRotAng) ) );
if( sinRotAng < 0.0f )
rotAng = -rotAng;
// Rotate the end effector position.
endX = targetBone->getFrameX() + cosRotAng*curToEndX - sinRotAng*curToEndY;
endY = targetBone->getFrameY() + sinRotAng*curToEndX + cosRotAng*curToEndY;
// Rotate the current bone in local space (this value is output to the user)
targetBone->setAngle(SimplifyAngle(targetBone->getAngle() + rotAng));
// Check for termination
double endToTargetX = (targetX-endX);
double endToTargetY = (targetY-endY);
if( endToTargetX*endToTargetX + endToTargetY*endToTargetY <= 1.0f )
{
// We found a valid solution.
return;
}
// Track if the arc length that we moved the end effector was
// a nontrivial distance.
if( !modifiedBones && fabs(rotAng)*curToEndMag > 0.0001f )
{
modifiedBones = true;
}
targetBone = targetBone->getParent();
}
Thanks

Answer 1

No, there is nothing obviously wrong in the program listing you have given. You are correctly computing the change of angle rotAng and the new position (endX, endY) of the end-effector.
You can compute rotAng more simply as
double rotAng =
atan2(curToTargetY, curToTargetX) - atan2(curToEndY, curToEndX);
which gives identical results (assuming the vectors are non-zero).
I suspect the error is somewhere outside of the program listing you have given. Maybe there is a discrepancy between the forward kinematics assumed in inverseKinematics() and the actual forward kinematics used in the display routines and elsewhere. Try recomputing the forward kinematics at the end of the procedure to see if the rest of the system agrees that the end-effector is at (endX, endY).