IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post those lines of code you feel like sharing or find what you require for your project here; or simply use them as tutorials.
Post Reply
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post by thanhle »

Hi guys
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);
         }
     }
 
 
In your game loop do this.

Code: Select all

 
        vector3df target = vNode->getAbsolutePosition();
    inverseKinematicAxisAngle(target);
 
https://www.youtube.com/watch?v=JHk3Qe7 ... e=youtu.be
Last edited by thanhle on Sun Jun 12, 2016 6:55 am, edited 4 times in total.
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

Re: Inverse Kinematic with Irrlicht Jacobian Transpose

Post by thanhle »

Minor update to make IK work with any parent node orientation.
Inverse transform should be perform on every parent node of the all the manipulate joints.

For those who likes CCD IK, two variant of the methods are given below.

No rotational axis is enforce on the joints in that implementation. Just multiple the axis with a rotational axes to limit the dof.

CCD IK version not as realistic as Jacobian.

Code: Select all

 
 
 bool ccd_IK(core::vector3df target)
     {
        
 
         core::vector3df   rootPos, curEnd, targetVector, desiredEnd, curVector, axis, endPos;
         double         cosAngle, turnAngle;
 
         endPos = target;
 
 
 
         irr::scene::IBoneSceneNode *base = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rshoulder");
         irr::scene::IBoneSceneNode *endEffector = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rhand");
 
 
 
         irr::scene::IBoneSceneNode *rootBoneNode = base;
         irr::scene::IBoneSceneNode *link;
         irr::scene::IBoneSceneNode *end;
 
         // start at the last link in the chainr
         int jointCount = 7;
         end = endEffector;
         link = (irr::scene::IBoneSceneNode*)end->getParent();
 
 
         //return true;
         int tries = 0;
         double norm = 6;
 
         int maxTries = 50;
         double error = 2;
 
         bool startDebug = false;
 
         while (tries<maxTries && norm > error)
         {
             tries++;
             link->updateAbsolutePosition();
             end->updateAbsolutePosition();
             rootPos = link->getAbsolutePosition();
             curEnd = end->getAbsolutePosition();
 
 
             norm = curEnd.getDistanceFromSQ(endPos);
             // see if i'm already close enough
             if (norm > error)
             {
                 // create the vector to the current effector pos
                 curVector = curEnd - rootPos;     //Link end pos and joint coord.
 
                 // create the desired effector position vector
                 targetVector = endPos - rootPos;  //From joint coord to target point.
 
                 // 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 < 0.9999)
                 {
 
                     turnAngle = std::acos(cosAngle);   // get the angle
                     float absAngle = std::abs(turnAngle);
                     if (absAngle > 0.00001)
                     {
                         turnAngle *= 0.2f;   //Just for smother movement.
 
                         // use the cross product to check which way to rotate
                         axis = curVector.crossProduct(targetVector);
                         axis.normalize();
                         quaternion ch;
                         ch.fromAngleAxis(turnAngle, axis);
                         ch.normalize();
 
                         matrix4 ivtran;
                         link->getParent()->getAbsoluteTransformation().getInverse(ivtran);
                         quaternion qinv = quaternion(ivtran);
                         qinv.normalize();
 
                         quaternion qa = quaternion(link->getAbsoluteTransformation());
                         qa.normalize();
 
                         vector3df rot;
                         ch = qa*ch*qinv;
 
                         ch.toEuler(rot);
                         
                         rot *= RADTODEG;
 
                         link->setRotation(rot);
                         ((irr::scene::IBoneSceneNode *) link->getParent())->updateAbsolutePositionOfAllChildren();
                     }
 
 
                     if (link == rootBoneNode)
                     {
                         link = end;   // start of the chain, restart
 
 
                     }
                     else
                     {
                         link = (irr::scene::IBoneSceneNode *)link->getParent();
                     }
                     // return true;
                 }
 
 
             }
         }
 
 
CCD start from base link. Looks slightly better.

Code: Select all

 
 bool Character::ccd_IKback(core::vector3df target)
     {
         //if(done == true) return true;
         //eturn true;
 
         core::vector3df   rootPos, curEnd, targetVector, desiredEnd, curVector, axis, endPos;
         double         cosAngle, turnAngle;
 
         endPos = target;
 
 
 
         irr::scene::IBoneSceneNode *base = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rshoulder");
         irr::scene::IBoneSceneNode *endEffector = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rhand");
 
 
 
         irr::scene::IBoneSceneNode *rootBoneNode = (irr::scene::IBoneSceneNode*)endEffector->getParent();
         irr::scene::IBoneSceneNode *link;
         irr::scene::IBoneSceneNode *end;
 
         // start at the last link in the chainr
         int jointCount = 7;
         end =  endEffector;
         link = base; // (irr::scene::IBoneSceneNode*)end->getParent();
 
 
         //return true;
         int tries = 0;
         double norm = 6;
 
         int maxTries = 50;
         double error = 2;
 
         bool startDebug = false;
 
         while (tries<maxTries && norm > error)
         {
             tries++;
             link->updateAbsolutePosition();
             end->updateAbsolutePosition();
             rootPos = link->getAbsolutePosition();
             curEnd = end->getAbsolutePosition();
 
 
             norm = curEnd.getDistanceFromSQ(endPos);
             // see if i'm already close enough
             if (norm > error)
             {
                 // create the vector to the current effector pos
                 curVector = curEnd - rootPos;     //Link end pos and joint coord.
 
                 // create the desired effector position vector
                 targetVector = endPos - rootPos;  //From joint coord to target point.
 
                 // 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 < 0.9999)
                 {
 
                     turnAngle = std::acos(cosAngle);   // get the angle
                     float absAngle = std::abs(turnAngle);
                     if (absAngle > 0.00001)
                     {
                         if (absAngle > 30)
                         {
 
                             // turnAngle = copysignf(1.0, turnAngle) * 30;
                         }
                         turnAngle *= 0.1;
                         // use the cross product to check which way to rotate
                         axis = curVector.crossProduct(targetVector);
                         axis.normalize();
                         quaternion ch;
                         ch.fromAngleAxis(turnAngle, axis);
                         ch.normalize();
 
                         matrix4 ivtran;
                         link->getParent()->getAbsoluteTransformation().getInverse(ivtran);
                         quaternion qinv = quaternion(ivtran);
                         qinv.normalize();
 
 
                         quaternion qa = quaternion(link->getAbsoluteTransformation());
                         qa.normalize();
 
 
                         vector3df rot;
                         ch = qa*ch*qinv;
 
                         ch.toEuler(rot);
 
                         rot *= RADTODEG;
 
 
                         link->setRotation(rot);
                         ((irr::scene::IBoneSceneNode *) link->getParent())->updateAbsolutePositionOfAllChildren();
                     }
 
 
                     if (link == rootBoneNode)
                     {
                         link = base;   // start of the chain, restart
                     }
                     else
                     {
                         link = (irr::scene::IBoneSceneNode *)*link->getChildren().begin();
                     }
                    
                 }
 
 
             }
         }
        
         return true;
     }
 
Regards
thanh
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

Re: Inverse Kinematic with Irrlicht Jacobian Transpose + CCD

Post by thanhle »

I'll add other variant of jacobian next, when I need look at these again.

Hey I need to get back to work on shadows with deferred rendering.

But I think something is wrong with render to texture/target now. It seems to run a little slow compares to before the rendertarget was update.

Regards
thanh
Nadro
Posts: 1648
Joined: Sun Feb 19, 2006 9:08 am
Location: Warsaw, Poland

Re: Inverse Kinematic with Irrlicht Jacobian Transpose + CCD

Post by Nadro »

You should use new IRenderTarget interface for the best performance. If you will use old solution performance will be slightly worse than it was before.
Library helping with network requests, tasks management, logger etc in desktop and mobile apps: https://github.com/GrupaPracuj/hermes
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

IK Jacobian inverse LS, SVD, Transpose + CCDs

Post by thanhle »

Damped least square pseudo inverse.

Code: Select all

 
 
 void  dampedleastSQrPseudoInverseKinematicAxisAngle(vector3df &target)
     {
         //  if (!isEnableIK) return;
 
         isEnableIK = false;
         irr::scene::IBoneSceneNode *base = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rshoulder");
         irr::scene::IBoneSceneNode *endEffector = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rhand");
 
         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);
         double lamda = 1.5;
 
         //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 JT = jacob.transpose();
             //Compute angle by transpose jacobian matrix.
 
             //Levenberg - Marquardt algorithm
 
             Eigen::MatrixXd JInv = JT*(jacob*JT + lamda*lamda*Eigen::MatrixXd::Identity(jacob.rows(), jacob.rows())).inverse();   //damp least square psuedo Inverse. We can make alpha non square. Still work.
 
             Eigen::MatrixXd dangle = JInv*edeltaE;
 
 
            // Eigen::MatrixXd dangle = JT*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);    //We can remove the alpha value or set it to 1 above.
                 ch.normalize();
 
                 matrix4 ivtran;
                 joints[j]->getParent()->getAbsoluteTransformation().getInverse(ivtran);
                 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 are the same only the matrix calculation section is different. I would refactor these.


VSD method pseudo inverse. This method greate but can be slow.

Code: Select all

 
 bool pvinv(Eigen::JacobiSVD<Eigen::MatrixXd> &svdA, Eigen::MatrixXd &a_pinv)
     {
         // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
 
         //if (svdA.rows()<svdA.cols())
         // return false;
 
         // SVD
         Eigen::MatrixXd vSingular = svdA.singularValues();
 
         // Build a diagonal matrix with the Inverted Singular values
         // The pseudo inverted singular matrix is easy to compute :
         // is formed by replacing every nonzero entry by its reciprocal (inversing).
         Eigen::MatrixXd vPseudoInvertedSingular(svdA.matrixV().cols(), 1);
 
         for (int iRow = 0; iRow<vSingular.rows(); iRow++)
         {
             if (fabs(vSingular(iRow)) <= 1e-10) // Todo : Put epsilon in parameter
             {
                 vPseudoInvertedSingular(iRow, 0) = 0.;
             }
             else
             {
                 vPseudoInvertedSingular(iRow, 0) = 1. / vSingular(iRow);
             }
         }
 
         // A little optimization here
         Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());
 
         // Pseudo-Inversion : V * S * U'
         a_pinv = (svdA.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
 
         return true;
     }
 
 void Character::VSDpseudoInverseKinematicAxisAngle(vector3df &target)
     {
 
 
         //  if (!isEnableIK) return;
 
         isEnableIK = false;
         irr::scene::IBoneSceneNode *base = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rshoulder");
         irr::scene::IBoneSceneNode *endEffector = ((IAnimatedMeshSceneNode *)this->sceneNode)->getJointNode("Rhand");
 
         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);
         double lamda = 1.5;
 
         //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::JacobiSVD<Eigen::MatrixXd>  svd = Eigen::JacobiSVD<Eigen::MatrixXd>(jacob, Eigen::ComputeThinU | Eigen::ComputeThinV);
             Eigen::MatrixXd JInv;
             pvinv(svd, JInv);
 
             Eigen::MatrixXd dangle = JInv*edeltaE;
             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);    //We can remove the alpha value or set it to 1 above.
                 ch.normalize();
 
                 matrix4 ivtran;
                 joints[j]->getParent()->getAbsoluteTransformation().getInverse(ivtran);
                 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);
 
         }
     }
REDDemon
Developer
Posts: 1044
Joined: Tue Aug 31, 2010 8:06 pm
Location: Genova (Italy)

Re: IK Jacobian inverse LS, SVD, Transpose + CCDs

Post by REDDemon »

O_O. very nice. Irrlicht miss good animation framework. It miss IK and rebinding wich are de-facto standards.
Junior Irrlicht Developer.
Real value in social networks is not about "increasing" number of followers, but about getting in touch with Amazing people.
- by Me
Vectrotek
Competition winner
Posts: 1087
Joined: Sat May 02, 2015 5:05 pm

Re: IK Jacobian inverse LS, SVD, Transpose + CCDs

Post by Vectrotek »

Fantastic! Really worth a look!
Great Work!
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

Re: IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post by thanhle »

Basic Fabrik code: No multi base sub chains.

Code: Select all

 
 
    class Link
    {
    public:
        float length_;
 
    };
 
    class Joint
    {
        public: irr::core::vector3df position_;
                irr::scene::ISceneNode *joint_;
    };
 
    class IKChain
    {
 
    const double ik_iteration  = 30;
    const double ik_error  = 0.01;
 
    public:
        IKChain()
        {
 
        }
        ~IKChain()
        {
        };
 
 
        void setUpIKChain(irr::scene::ISceneNode *base, irr::scene::ISceneNode *endEffector)
        {
            irr::scene::ISceneNode *rootBoneNode = base;
            irr::scene::ISceneNode *joint;
            irr::scene::ISceneNode *jend;
 
            // start at the last link in the chainr
            //int jointCount = 7;
            jend = endEffector;
            joint = endEffector;
            
            Joint j;
            j.position_ = jend->getAbsolutePosition();
            j.joint_ = jend;
            joints_.push_back(j);
 
            int i = 0;
 
            while (joint != rootBoneNode)
            {
                
                joint = joint->getParent();
                i++;
                Joint j;
                j.position_ = joint->getAbsolutePosition();
                j.joint_ = joint;
                joints_.insert(joints_.begin(), j);
 
                Link l;
                l.length_ = joints_[0].position_.getDistanceFrom(joints_[1].position_);
                links_.insert(links_.begin(), l);
            
            
            } 
 
        }
 
 
        bool Fabrik_IK(irr::core::vector3df target)
        {
            
            int iteration = 0;
            float error = 10000;
 
            int njoints = joints_.size() - 1;
 
            float totalLength = 0;
            for (int i = 0; i < links_.size(); ++i)
            {
                totalLength += links_[i].length_;
            }
 
        
 
            float length = target.getDistanceFrom(joints_[0].position_);
 
            if (totalLength <= length)
            {
                vector3df dir = target - joints_[0].position_;
                dir = dir.normalize();
                joints_[0].position_.set(joints_[0].joint_->getAbsolutePosition());
                for (int i = 1; i <= njoints; ++i)
                {
                    joints_[i].position_ = joints_[i - 1].position_ + dir*links_[i-1].length_;
                }
 
            }
            else
            {
 
 
                while (iteration < ik_iteration && error > ik_error)
                {
                    iteration++;
                    //BackwardReaching
                    core::vector3df mTarget = target;
 
                    joints_[njoints].position_.set(mTarget);
 
 
                    for (int i = njoints - 1; i >= 0; --i)
                    {
 
                        vector3df jvec = joints_[i].position_ - joints_[i + 1].position_;
                        vector3df jnorm = jvec.normalize();
 
                        vector3df joinIPos = joints_[i + 1].position_ + jnorm*links_[i].length_;
 
                        joints_[i].position_.set(joinIPos);
 
                    }
 
 
 
                    //ForwardReaching.
                    joints_[0].position_.set(joints_[0].joint_->getAbsolutePosition());
 
                    for (int i = 0; i < njoints; ++i)
                    {
 
                        vector3df jvec = joints_[i + 1].position_ - joints_[i].position_;
                        vector3df jnorm = jvec.normalize();
 
                        vector3df joinIPos = joints_[i].position_ + jnorm*links_[i].length_;
 
                        joints_[i + 1].position_.set(joinIPos);
 
                    }
 
 
                    error = joints_[njoints].position_.getDistanceFrom(target);
                }
            }
 
            
            for (int i = 0; i <= njoints; ++i)
            {
                
                if (i == 0)
                {
                    //joints_[i].joint_->setPosition(joints_[i].position_);
                }
                else
                {
                    
                
                    irr::scene::ESCENE_NODE_TYPE t = joints_[i].joint_->getType();
 
                    if (t == irr::scene::ESCENE_NODE_TYPE::ESNT_UNKNOWN)
                    {
                        //joints_[i].joint_->updateAbsolutePosition();
 
                        vector3df orJointPos = joints_[i - 1].joint_->getAbsolutePosition();
 
 
                        vector3df orieLinkpos = joints_[i].joint_->getAbsolutePosition() - orJointPos;
                        vector3df neweLinkpos = joints_[i].position_ - orJointPos;
                        orieLinkpos.normalize();
                        neweLinkpos.normalize();
 
                        float cosAngle = orieLinkpos.dotProduct(neweLinkpos);
                        if (cosAngle < 0.9999)
                        {
                            float turnAngle = std::acos(cosAngle);   // get the angle
                            vector3df axis = orieLinkpos.crossProduct(neweLinkpos);
 
 
                            axis.normalize();
                            quaternion ch;
                            ch.fromAngleAxis(turnAngle, axis);
                            ch.normalize();
 
 
 
                            matrix4 ivtran;
                            joints_[i-1].joint_->getParent()->getAbsoluteTransformation().getInverse(ivtran);
                            quaternion qinv = quaternion(ivtran);
                            qinv.normalize();
 
 
                            quaternion qa = quaternion(joints_[i-1].joint_->getAbsoluteTransformation());
                            qa.normalize();
 
 
                            vector3df rot;
                            ch = qa*ch*qinv;
 
                            ch.toEuler(rot);
 
                            rot *= RADTODEG;
 
 
                            joints_[i - 1].joint_->setRotation(rot);
                    
                            ((irr::scene::IBoneSceneNode *) joints_[i - 1].joint_->getParent())->updateAbsolutePositionOfAllChildren();
                        }
                    }
                    else
                    {
                        joints_[i].joint_->setPosition(joints_[i].position_ - joints_[i - 1].position_);
                    }
 
                }
            
            
        
        
            }
 
            return true;
        }
 
        
        private: vector<Link> links_;
            vector<Joint> joints_;
 
 
    };
 
 

Usage example:


const int len = 50;
ISceneNode *joint1[len];

for (int i = 0; i < len; ++i)
{
joint1 = device->getSceneManager()->addSphereSceneNode(0.5f);
joint1->setPosition(vector3df(0, 0.f, 0));
joint1->setName("j" + i);

if (i > 0)
{
joint1->addChild(joint1);

joint1->setPosition(vector3df(0, 1.f, 0));
joint1->updateAbsolutePosition();
}
}

NVuDu::IKChain chain;
chain.setUpIKChain(joint1[0], joint1[len-1]);

Put below function inside update loop and see IK in action.
chain.Fabrik_IK(target);

Not I'm not a pretty programmer. I just dump the code here.
Also the code will work for animate character as well. However, you have to manage resetting the postures.

By the way.
I think Irrlicht need to update it's matrix calculation a little.

Inverse is not needed if calculation are done in relative transformation.

Sorry for the unprofessional code :).


Update: Add Fabrik method.
https://youtu.be/WbiQMjKXKgI

The other IK methods above should be fast as well.
Vectrotek
Competition winner
Posts: 1087
Joined: Sat May 02, 2015 5:05 pm

Re: IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post by Vectrotek »

Hey! This is cool! more?
thanhle
Posts: 325
Joined: Wed Jun 12, 2013 8:09 am

Re: IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post by thanhle »

Hi I'm not using Irrlicht at the moment.
I'm using Urho3d, since it updates more often.

Best regards,
Vectrotek
Competition winner
Posts: 1087
Joined: Sat May 02, 2015 5:05 pm

Re: IK Fabrik, Jacobian inverse LS, SVD, Transpose + CCDs

Post by Vectrotek »

Hey! Thats cool, but when it gets cold outside you're allways welcome back home! :mrgreen:
Physics is physics so there'll probably be some clandestine loitering that side too.

I'm slowly getting all my old (chaotic) engine's loose and unorganised extras worked into a new framework that uses Irrlicht as a base.
(the IRR devs, considering how few they are, are doing a fantastic job though)

Anyway, enjoy!
Post Reply