Using Jacobian For Inverse Kinematics

 

A good time ago, I posted a video which detailed how to implement IK using Cyclic Coordinate Descent or CCD for short. While I have lost the source code for that original implementation, you can still follow the video and implement the concept detailed in the video. Or, better yet, you can follow along on this tutorial on how to implement an even better and more accurate version of inverse kinematics.

Am not going to lie, but this version is slightly more complicated. That is mostly because I inverse knowing a little calculus. For full disclosure, I have not taken a calculus course and yet am able to not only understand it, but better still, implement it! If I did it, so can you.

Basics

Note, I will mistakenly use the term bone instead of joint sometimes, when you see this just pretend I wrote “joint”.  A joint is defined as only a global position, if it has no parent, otherwise an offset to its parent and a local space rotation, using quaternion.

Some quick boring details.  IK or inverse kinematics is all about moving joints around automatically by specifying only a goal that the effector joint(tip of an IK chain) in the chain(list of joints) will reach.  After setting a goal that the effector joint will reach, then all other joints will automatically calculate their rotation angles in a manner to ensure the effector reaches the goal.

From the image below, you can see a sample IK chain.  It is comprise of a root joint, which is the base, effector, the last joint in the chain, and the joints between root and effector.
sample_ki

So inverse kinematics is the inverse of forward kinematics, where you individually specify the rotation values of each joint starting from the root joint all the way to the effector joint.

The formula for this is below.  Where Q is an array of of quaternions(rotation), such that Q[0] is root joint, Q[n] is a joint between root and effector, and Q[n-1] is the effector.  Each Q[i], is defined in its on local bone space.  The function f takes Q[0] to Q[n-1] and concatenates them together.  In doing so, we can then found out the final effector position.

fk So, with inverse kinematics, we would like to not have to specify each individual Q for each joint, and then cconcatenate from root to effect all in the name of finding out where the effector position will be located.  Thus, it stands to reason that we could just invert the process, f.

ifk

 

Now, the equation states that we want to take some target position  x and put it into our inverse function and magically calculate the individual Q[i] from Q[0], which is root, to Q[n-1], which is effector.  Not just that, but these individual Q are themselves in local space of each joint[i].

But the problem is that the function f is “non-linear”!  Crap.  This means that we can cannot find a unique value from x to Q, whereas we can from Q to x.  So, if its non-linear, we need a way to linearize it!

We will linearize the problem by using derivatives.  Mainly we will take the rate of change of each joint’s position, joint[i],  and relate it to the rate of change of the effector’s positional change.  In calculating this rate of change, derivative, we will use a jacobian, which is a matrix containing each derivative of each joint in the system.

jacob  So J tells us how the effector changes its position  as we apply changes to the joint orientation.  J tell us that if we change joint[i] rotation by one degree, that the effect’s position will change by 4 units in whatever direction.  Since we want to know how a position change will affect the rotation values (Q[i]) we will take the inverse of jacobian.

Some more details about J is in order.  J, the jacobian, will be structured as a column major matrix. Where each column entry will describe the derivative of joint[i] with respect to the effector.

jacobian_definition

As you can see from the image, the Jacobian is a 6 by n matrix.  Where n indicates the number of joints in the IK chain.  The first 3 rows of each jacobian entry, jointi, describes the amount of change or force fulling the effect towards the goal.  The last 3 rows of each jacobian entry are not not covered in this tutorial.

The variable p is the world position of the effector, jn is the world position of joint[i], and axis, is the local axis of rotation for joint[i].

The Real Stuff

Okay, we covered a lot of stuff there.  Now let us put this information to good use.  But before we do so, lets cover the general algorithm behind this IK system’s implementation.

Inverse Kinematic programmatic algorithm

  1. Begin by calculating the vector from current effector position to target (goal)
  2. If the vector distance between current effector position to target is less than threshold, exit
  3. Using the information above, calculate the jacobian by calculating each entry of the jacobian
  4. Invert the Jacobian
  5. Determine the force F( vector offset between effector position and goal )
  6. Compute joint velocities
  7. Integrate joint velocities to obtain joint rotation and apply rotations
  8. Check if goal has been reach and exit
  9. Apply changes

 

Now: the IK Code

Enjoy the code view below. I commented the code so it should be easy to follow.

    public void solve(Vector3f goalPosition,  double dt){
        int nJoint = ikChain.size();
        IKJoint ikEffector = getIKEffector();

        //
        // STEP 1:
        //Set the IK Configuration from last known joint configuration of the REAL BONES
        for (int i=0; i < nJoint; i++){             IKJoint IJ = ikChain.get(i);             IJ.copyOrientAndTranslationOfRealJoint();         }         int dof = 6;         float fError = 0f;         ikSolver.iTries=0;         int currentJointIndex, 		//current joint being worked on                 iRowJac, 				//Row into jacobian ( x,y,z, axis_x, axis_y, axis_z)                 iColumnJac, 			//Column into jacobian ( joint_i/bone_i )                 iIndex;         //----------------------------------------------------------------------------------         //----------------------------------------------------------------------------------         Vector3f temp = new Vector3f();         Quaternion Q = new Quaternion();         do {             // STEP 2:             //  build ik system using preset configuration             buildIKMatrix();             // Step 3:             //  get Effector Position, vectorToGoal, and distance to Goal             effectorPosition.set      (ikEffector.getPosition());             vecToGoal               = Vector3f.sub(effectorPosition, goalPosition, null);             fError				    = vecToGoal.length();             // Is the effector position too far from the target             if( fError > ikSolver.m_fTargetThreshold   ){
                //We are too far;
                IKJoint curIKBone;
                //----------------------------------------------------------------------------
                // Construct the jacobian ; one column at a time
                //----------------------------------------------------------------------------
                for ( iColumnJac = 0, currentJointIndex = 0; iColumnJac < nJoint; ++iColumnJac, ++currentJointIndex ) {
                    //Get the current joint world poisition
                    curIKBone = ikChain.get(currentJointIndex);
                    curJointLocation = curIKBone.getPosition();
                    //Get the rotation axis of this joint
                    vecJointToEffector                    = Vector3f.sub(effectorPosition,  curJointLocation, vecJointToEffector);
                    vecJointToGoal                        = Vector3f.sub( goalPosition   ,	  curJointLocation, vecJointToGoal);
                    ikSolver.m_arrAxis[currentJointIndex] = Vector3f.cross(vecJointToGoal, vecJointToEffector, ikSolver.m_arrAxis[currentJointIndex]);

                    if(ikSolver.m_arrAxis[currentJointIndex].length() == 0){
                        ikSolver.m_arrAxis[currentJointIndex].set(0.0f, 0.0f, 1.0f);
                    }
                    ikSolver.m_arrAxis[currentJointIndex].normalise();

                    //Calculate change(Derivative) of direction need to force this
                    //curIKBone to approach the target; describes how Effector changes with joint i change
                    dxEffectorNCurrentJoint = Vector3f.cross(vecJointToEffector, ikSolver.m_arrAxis[iColumnJac], dxEffectorNCurrentJoint);


                    //Calculate the Jacobian: 
                    // e =  f ( q )	  { where q is orientation, and e is position }
                    // the jacobian is partial derivatives stateing how what changes in orientation will produce changes
                    // in position; for joint_i the first 3 elements are the changes in position between joint and effector
                    // the last 3 elements are the rotation axis needed to get closer to target
                    for( iRowJac=0; iRowJac<3; iRowJac++){
                        ikSolver.m_Jacobian.set(iRowJac, iColumnJac, dxEffectorNCurrentJoint.get(iRowJac) );
                        ikSolver.m_Jacobian.set(iRowJac + 3, iColumnJac, vecJointToGoal.get(iRowJac)  );
                    }
                }
                // Jacobian: derivative of: e = f ( q ), however
                // we want:  q
                // thus: q = f`(e)
                // this is achieved with the transpose
                ikSolver.m_JacobianTranspose = ikSolver.m_Jacobian.transpose();

                ikSolver.m_farrError.set(0, 0, vecToGoal.getX());
                ikSolver.m_farrError.set(1, 0, vecToGoal.getY());
                ikSolver.m_farrError.set(2, 0, vecToGoal.getZ());
                ikSolver.m_farrError.set(3, 0, 0.0f );//twist
                ikSolver.m_farrError.set(4, 0, 0.0f );//twist
                ikSolver.m_farrError.set(5, 0, 0.0f );//twist
                //Compute q'
                //   e =  f ( q )
                //   e' =  J ( q' ) , where J is the derivative(jacobian) of f
                //   q' =  J ( e' )
                //Compute Joint velocities
                for (iRowJac = 0; iRowJac < nJoint; ++iRowJac){
                    ikSolver.m_QDerivative.set(iRowJac, 0, 0.0f);

                    for (iColumnJac=0; iColumnJac < 6; ++iColumnJac){
                        float prev = ikSolver.m_QDerivative.get(iRowJac,0);
                        float product = ikSolver.m_JacobianTranspose.get(iRowJac,iColumnJac)  * ikSolver.m_farrError.get(iColumnJac,0);
                        // The derivate is the sum of all changes of individual joints/bones
                        float globalChange = prev+product;
                        //Dt is the rate at which the application is running
                        ikSolver.m_QDerivative.setIncrement(iRowJac, 0, globalChange * (float) dt);// ikSolver.m_fTargetThreshold );
        updateRealBones();
    }

profConcept_small

 

Click here to view the matrix used in the above code.

Click here to view the IKSolver class referenced in the above code.

Leave a Reply