Inverse Kinematics is the process of finding a set of joint angles that reach a goal position.

My favorite way of doing Inverse Kinematics is called “Quaternion Cyclic Coordinate Descent” or “CCDIK”:

CCDIK

At its core, the algorithm is very simple

foreach joint in jointsTipToBase {
  // Point the effector towards the goal
  directionToEffector = effector.position - joint.position;
  directionToGoal = goal.position - joint.position;
  joint.rotateFromTo(directionToEffector, directionToGoal);
}

We’re just telling each joint to point the end effector towards the goal position.

Of course, if it reaches for the goal without regard for the hinges, it looks unnatural!

Hinges

We can take the hinges into account by enforcing the hinge-axis after the CCD step

foreach joint in jointsTipToBase {
  // Point the effector towards the goal (See Above)

  // Constrain to rotate about the axis
  curHingeAxis = joint.rotation * joint.axis;
  hingeAxis = joint.parent.rotation * joint.axis;
  joint.rotateFromTo(curHingeAxis, hingeAxis);
}

Even at one iteration per frame, this is beginning to look pretty good! But real joints often have limits…

Limits

You can apply axis-aligned hinge limits in local-euler angle space

foreach joint in jointsTipToBase {
  // Point the effector towards the goal (See Above)
  // Constrain to rotate about the axis (See Above)

  // Enforce Joint Limits
  joint.localRotation.clampEuler(joint.minLimit, joint.maxLimit);
}

Full Source

This final aspect gives you an iterative 3D IK algorithm that beats nearly every other Heuristic Inverse Kinematics algorithm out there.

Properties of Various IK Algorithms

IK Algorithms Analytic Automatic Differentiation Jacobian Transpose FABRIK Quaternion CCDIK
Implementation Complexity? Extremely Complex Hard Hard Easy Easy
Speed Extremely Fast Slow To Converge Slow To Converge Fast Fast
Hinge Joints Only Hinges? Yes Yes No! Yes
Joint Limits Difficult Yes No Conical Limits Yes
Hits Singularities Never Often Often Never (w/out hinges) Rarely (often anneals through them)
Convergence Behaviour Instant Stable Stable Very Well Behaved Well Behaved across short distances
Number of Joints Max ~5 Arbitrary Arbitrary Arbitrary Arbitrary

Bonus: Direction

The astute among you might notice that this is a 5-DoF arm. This means it is over-actuated for just touching a 3-DoF point.

With 5-Dof, you can hypothetically touch any point from any direction.

foreach joint in jointsTipToBase {
  if(joint.id > 3)
    // Point the effector along the desired direction
    joint.rotateFromTo(effector.direction, goal.direction);
  } else {
    // Point the effector towards the goal (See Above)
  }
  // Constrain to rotate about the axis (See Above)
  // Enforce Joint Limits (See Above)
}

Now that the system is only “sufficiently actuated” (where the IK is using each degree of freedom the arm possesses), you may notice that it hits joint-limit based singularities more often.

These concavities are impossible to avoid in a heuristic IK algorithm (read: all of them except Analytic). However, it is possible to “jump out of” concavities by adding large random offsets to each joint, and then attempting the IK solve again. This is known as “Simulated Annealing”. Implementing this is left as an exercise to the reader.

While 3D Engines (Unity, Three.js) come with this function, it can be useful to see how it is implemented.

Sam Hocevar, Inigo Quilez, Jonathon Blow, and Marc B. Reynolds all give excellent implementations and derivations.

For convenience, this is a mirror of Sam Hocevar’s robust C++ implementation:

quat quat::fromtwovectors(vec3 u, vec3 v) {
    float norm_u_norm_v = sqrt(dot(u, u) * dot(v, v));
    float real_part = norm_u_norm_v + dot(u, v);
    vec3 w;

    if (real_part < 1.e-6f * norm_u_norm_v) {
        /* If u and v are exactly opposite, rotate 180 degrees
         * around an arbitrary orthogonal axis. Axis normalisation
         * can happen later, when we normalise the quaternion. */
        real_part = 0.0f;
        w = abs(u.x) > abs(u.z) ? vec3(-u.y, u.x, 0.f)
                                : vec3(0.f, -u.z, u.y);
    } else {
        /* Otherwise, build quaternion the standard way. */
        w = cross(u, v);
    }

    return normalize(quat(real_part, w.x, w.y, w.z));
}

and Marc’s Simple C++ implementation:

vec4 q_from_normals(vec3 a, vec3 b) {
  float k = 1.0+dot(a,b);         // 1+d
  float s = inversesqrt(k+k);     // 1/sqrt(2+2d)
  return vec4(s*cross(a,b), k*s); // (1+d)/sqrt(2+2d) + (a x b)/sqrt(2+2d)
}

Comments