As promise, I post other inverse kinematic codes. Jacobian transpose, damped least square, SVD and 2 variant of CCDs in this thread. Feel free to refactor the code into a nice class and share with everyone here.
I saw we just have b3d animation save patch. Some one could use this code to create a character animation class or something and save it to b3d : ).
Anyway.
------------
I will give an example of numerical method to compute the jacobian in the future.
-----------
First off, we need to use Eigen version 3 library. For Matrice calculation (linear algebra for doing inverse. Well not doing that for now. But will be important in practice.).
You need to add this library yourself, I can't help you. Then include #include "Eigen/Eigen"
In this example I make the b3d character hands (shoulder, elbow and hands follows a flycircleanimator). Note I will not deal with singularity condition.
So don't ask me why the hand spin frantically if you move the character away from the moving object. Try readjust alpha value will help fix that.
1) Create a flyCircleAnimator.
Code: Select all
vNode = mVudu->device->getSceneManager()->addMeshSceneNode(mesh, NULL, -1, core::vector3df(0, 0, 0));
vNode->setScale(core::vector3df(0.5f, 0.5f, 0.5f)); // easier to adapt on testing than camera-code
vNode->setMaterialFlag(E_MATERIAL_FLAG::EMF_LIGHTING, false);
scene::ISceneNodeAnimator * flyCircleAnimator = smgr->createFlyCircleAnimator(core::vector3df(0.f, 17.f, 5.0f), 3.0f, 0.002f, vector3df(0, 0, 1));
vNode->addAnimator(flyCircleAnimator);
flyCircleAnimator->drop();
Add animated .b3d character and ik functions.
Code: Select all
#include "Eigen/Eigen"
void ResetBonePosition(ISceneNode *Node)
{
Node->setRotation(vector3df(0, 0, 0));
Node->updateAbsolutePosition();
core::list<ISceneNode*>::ConstIterator it = Node->getChildren().begin();
for (; it != Node->getChildren().end(); ++it)
{
UpdateAbsoluteTransformationAndChildren((*it));
}
}
void LoadCharacter( irr::scene::ISceneManager* smgr, irr::core::stringc path)
{
irr::scene::IAnimatedMesh* mesh = smgr->getMesh(path);
if (!mesh)
{
dev->drop();
return;
}
character = smgr->addAnimatedMeshSceneNode(mesh);
if (character)
{
character ->setJointMode(scene::EJUOR_CONTROL);
ResetBonePosition(character );
character ->setMaterialFlag(irr::video::E_MATERIAL_FLAG::EMF_LIGHTING, true);
}
}
//Function to compute jacobian
//Only rotation. If you want translation just add three more dof and use the calculated axis values.
Eigen::Matrix<double, 3, Eigen::Dynamic> :ComputeJacobiAxisAngleMethod(std::vector<irr::scene::ISceneNode *> joints, std::vector< irr::core::vector3df> lstRot, Eigen::Matrix<double, 3, Eigen::Dynamic> jacob, vector3df preViousEndEffectorPos, vector3df target, int &smallAngleCount)
{
int jointsCount = joints.size();
vector3df e = preViousEndEffectorPos;
for (int j = 0; j < jointsCount; ++j)
{
vector3df jPos = joints[j]->getAbsolutePosition();
vector3df axis = ((e - jPos).crossProduct(target - jPos))*lstRot[j];
axis.normalize();
vector3df JabValue = axis.crossProduct(e - jPos);
JabValue.normalize();
if (JabValue.getLength() <= 0.00001)
{
jacob(0, j) = 0;
jacob(1, j) = 0;
jacob(2, j) = 0;
smallAngleCount++;
continue;
}
jacob(0, j) = JabValue.X;
jacob(1, j) = JabValue.Y;
jacob(2, j) = JabValue.Z;
}
return jacob;
}
void TrimVectorValues(irr::core::vector3df &rot)
{
rot.X = fmodf(rot.X, 360);
rot.Y = fmodf(rot.Y, 360);
rot.Z = fmodf(rot.Z, 360);
}
void inverseKinematicAxisAngle(vector3df &target)
{
irr::scene::IBoneSceneNode *base = character ->getJointNode("Rshoulder"); //Make sure your b3d file has this otherwise you will have a segment fault error. Sorry I don't check for error.
irr::scene::IBoneSceneNode *endEffector =character ->getJointNode("Rhand"); //Make sure your b3d file has this otherwise you will have a segment fault error.
ISceneNode *current = endEffector;
std::vector<int> jointRotDof;
int count = endEffector->getChildren().getSize();
int jointscount = 0;
f32 epsilon = 0.05;
f32 alpha = 0.1; //For smooth iteration update. Play around with this parameter for smooth movement
std::vector<ISceneNode *> joints;
jointscount = 1;
current = endEffector;
while (current != base) //Get joint count to base joint. You would want to refactor and move this code elsewhere.
{
jointscount++;
current = current->getParent();
}
int totalDof = 0;
current = endEffector;
std::vector <vector3df> lstRot;
//Set joint rotation capability. You would want to refactor and move this code elsewhere. Maybe at init or create set function.
for (int i = 0; i < jointscount; ++i)
{
if (irr::core::stringc(current->getName()) == "Rhand" || i == 0)
{
joints.push_back(current); //ef
vector3df axis { 0, 0, 0 };
lstRot.push_back(axis);
}
else if (irr::core::stringc(current->getName()) == "Relbow")
{
joints.insert(joints.begin(),current); //ef
lstRot.insert(lstRot.begin(), vector3df(1, 1, 1));
//Or try this.... lstRot.insert(lstRot.begin(), vector3df(1, 1, 1));
}
else if (irr::core::stringc(current->getName()) == "Rwrist")
{
joints.insert(joints.begin(),current); //ef
lstRot.insert(lstRot.begin(),vector3df(1, 0, 0));
}
else
{
joints.insert(joints.begin(), current);
vector3df axis{1, 1, 1 };
lstRot.insert(lstRot.begin(), axis);
}
current = current->getParent();
}
//Need to do this in irrlicht to reset joints to zero pos.
//Otherwise matrix or quaternion calculation will cause a small twist,
//results in error after several revolutions.
for (int j = 0; j < jointscount; ++j)
{
joints[j]->setRotation(vector3df(0, 0, 0));
joints[j]->updateAbsolutePosition();
}
ISceneNode *lastjoint = endEffector;
Eigen::MatrixXd jacob(3, jointscount);
Eigen::MatrixXd delta_pos_new(6, jointscount);
int maxIteration = 300;
vector3df e = lastjoint->getAbsolutePosition();
double distance = e.getDistanceFrom(target);
//IK loop using jacobian transpose.
//To change it to other method. Just updating the equation where I change calculate dangle
while (distance >= epsilon && maxIteration > 0)
{
maxIteration--;
vector3df deltaE = target - e;
Eigen::Vector3d edeltaE(deltaE.X, deltaE.Y, deltaE.Z);
//Compute Jacobian by change in end effector position.
int smallAngleCount = 0;
//Calculate jacobian the simple way.
jacob = ComputeJacobiAxisAngleMethod(joints, lstRot, jacob, e, target, smallAngleCount);
if (smallAngleCount == jointscount)
{
return;
}
Eigen::MatrixXd transp = jacob.transpose();
//Compute angle by transpose jacobian matrix.
Eigen::MatrixXd dangle = transp*edeltaE; //For other method, just update this equation.
int index = -1;
for (int j = 0; j < jointscount; ++j)
{
float outAngle = dangle(j);
vector3df jPos = joints[j]->getAbsolutePosition();
vector3df p1 = e - jPos;
p1.normalize();
vector3df p2 = target - jPos;
p2.normalize();
vector3df axis = (p1).crossProduct(p2); //Joint rotation axis.
axis = axis *lstRot[j]; //Readjust to the correct rotatable axis.
axis.normalize();
irr::core::quaternion ch;
ch.makeIdentity();
ch = ch.fromAngleAxis(outAngle*DEGTORAD*alpha, axis); //Adjust alpha value for smooth rotation.
ch.normalize();
matrix4 ivtran;
joints[j]->getParent()->getAbsoluteTransformation().getInverse(ivtran); //Get inverse transform of parent.
quaternion qinv = quaternion(ivtran);
qinv.normalize();
vector3df rot = joints[j]->getRotation();
quaternion qa = quaternion(joints[j]->getAbsoluteTransformation());
qa.normalize();
ch = qa*ch*qinv;
ch.toEuler(rot);
rot *= RADTODEG;
TrimVectorValues(rot);
//Check for constraint here.
joints[j]->setRotation(rot);
// ((irr::scene::IBoneSceneNode *)joints[j])->updateAbsolutePositionOfAllChildren();
((irr::scene::IBoneSceneNode *) joints[j]->getParent())->updateAbsolutePositionOfAllChildren();
}
e = joints[jointscount - 1]->getAbsolutePosition();
distance = (double)e.getDistanceFrom(target);
}
}
Code: Select all
vector3df target = vNode->getAbsolutePosition();
inverseKinematicAxisAngle(target);
.
