December 29, 2008

Analytic Two-Bone IK in 2D

Due to their complexity, inverse kinematics (IK) problems are often solved with iterative solutions. While iterative solutions can handle different IK poblems with a single algorithm, they can also become computationally expensive. If a specific IK problem needs to be computed often, it is worth considering an analytic solution.

If we limit the problem to a two bone chain in a two-dimensions, we can derive the anaytic solution without much complexity. This will generally be more efficient than its iterative alternatives. While this specific case lends itself to a 2D world, it can also be used in 3D as long as the kinematic chain's motion is limited to a single plane.

The goal of is to create a robust function with the following requirements.

Let the user specify which direction to bend the bone chain (there can be up to two valid solutions, each bending in an opposite direction).
Notify the user when there was no valid solution based on the input.
When there is no valid solution, calcualte the closest alternative.
Produce the expected results for every target position. Most implementations found online or in books only function properly when the target is in a certain quadrant or when it remains in a specific range.

If you aren't interested in the exciting math ahead, jump to the end of the article for sample code. You can also download the RJ_Demo_IK application and source code to see a running implementation. If you're the type that likes to know how things work (like me), we will walk through the derivation in detail. It might look extensive at first, but it's only because I've broken it down step by step. Hopefully you'll be able to follow along without much difficulty.

You might also be interested in the following articles on different IK solvers:

Defining the problem

Our problem domain consists of a two bone chain where bone₁ is located at the origin and bone₂ is a child of bone₁. Each bone conistis of an angle \(\theta\) and a distance \(d\). The goal is to get the end of bone₂ to line up with the target point \((x,y)\). Because we have set bone1 at the origin, you will need to transform your destination point relative to bone₁. Figure 1 shows how our variables fit together. The distance from the orgin to the target point is defined as \(h\).

figure 1

The solvable domain

There will be cases in which our bone chain cannot reach the desired point. If we allow our bones to rotate without any constraints, we can only find a valid solution within a an explicit range from the origin (i.e. the base of bone₁). The minimum distance we can reach is the absolute value of the difference of our bone lengths, \(|d_1 - d_2|\). The maximum distance we can reach is the sum of our bone lengths, \(d_1 + d_2\). If the distance to the target, \(\sqrt { x^2 + y^2}\), is within this range, we can solve for \(\theta_1\) and \(\theta_2\). See figure 2 for a visual representation of the solvable area (a ring about the origin).

figure 2

Math review

Before we hop into the derivation, I want to review a few trigonometric identities that we will be using. For more information on the identities, Wikipedia has an extensive list and also has a page on the derivations.

Symmetry

\(\cos (\pi - \theta) = -\cos \theta\)

Right triangle ratios

\(\sin \theta = \frac {\mathrm{opposite}}{\mathrm{hypotenuse}}\)

\(\cos \theta = \frac {\mathrm{adjacent}}{\mathrm{hypotenuse}}\)

\(\tan \theta = \frac {\mathrm{opposite}}{\mathrm{adjacent}}\)

Pythagorean trigonometric identity

\(\sin^2 x + \cos^2 x = 1\)

Angle sum and difference identities

\(\sin(\alpha \pm \beta) = \sin \alpha \cos \beta \pm \cos \alpha \sin \beta\)

\(\cos(\alpha \pm \beta) = \cos \alpha \cos \beta \mp \sin \alpha \sin \beta\)

\(\tan(\alpha \pm \beta) = \frac{\tan \alpha \pm \tan \beta }{1 \mp \tan \alpha \tan \beta }\)

Pythagorean theorem

Given a right triange with side lengths \(a\), \(b\), and hypotenuse \(c\):

\(a^2 + b^2 = c^2\)

Law of cosines

Given a triange with side lengths \(a\), \(b\), \(c\) and angle \(\gamma\) across from side \(c\):

\(c^2 = a^2 + b^2 - 2ab \cos \gamma\)

Solving for θ₂

Our unknown variables are \(\theta_1\) and \(\theta_2\). Our known variables are \(d_1\), \(d_2\), \(x\) and \(y\). We are going to define the distance to the target point, \(h\), and the angle between \(d_1\) and \(d_2\), \(\beta\).

figure 3

Based on the law of cosines: \(h^2 = d_1^2 + d_2^2 - 2 d_1 d_2 \cos \beta\)

Based on the Pythagorean theorem: \(h^2 = x^2 + y^2\)

As shown in figure 3: \(\beta = \pi - \theta_2\)

Using the cosine symmetry: \(\cos \beta = \cos (\pi - \theta_2) = -\cos \theta_2\)

Through substitution: \(x^2 + y^2 = d_1^2 + d_2^2 + 2 d_1 d_2 \cos \theta_2\)

Solve for \(\theta_2\).

\(\cos \theta_2 = \frac{ x^2 + y^2 - d_1^2 + d_2^2 } { 2 d_1 d_2 }\)

\(\theta_2 = \arccos \frac{ x^2 + y^2 - d_1^2 - d_2^2 } { 2 d_1 d_2 }\)

We now have our analytic equation for \(\theta_2\), but there are a few nuances to discuss in more detail.

If \(\cos \theta_2\) is not within the range [-1,1], it is outside of the \(\arccos \theta\) domain. This will happen when our target point is a location the bones cannot reach. A value of -1 will give us an angle of 180° (i.e. bone₂ is fully bent) and a value of 1 will give us an angle of 0° (i.e. bone₂ is fully extended). When the value is less than -1, our bone is trying to bend father than is physically possible to reach a point that is too close to the origin. When the value is greater than 1, our bone is trying to extend farther than is physically possible to reach a point that is too far from the origin. This means that we can compute the best non-valid solution by clamping the value into the legal [-1,1] range.
If \(d_1\) or \(d_2\) is zero, we will divide by zero when computing \(\cos \theta_2\). When one or both bones have a zero length, we can set \(\theta_2\) to any value we desire (I suggest zero or the value used on a previous frame). When we solve for \(\theta_1\) it will correct itself according to our selected \(\theta_2\). The solvable domain for this case has also been reduced to a single circle centered at the origin with a radius of \(d_1 + d_2\).
When we can evaluate \(\arccos \theta\), our result is limited to the range \([0,\pi]\). If we use the result as is, we will always find the solution where our second bone turns in the positive direction. If it is our intention to find the solution that bends in the negative direction, we need to negate the new \(\theta_2\).

Solving for θ₁

When we solved for \(\theta_2\), it was a purely algebraic approach. In other words, we had a set of equations and kept massaging them until we ended up with the equation we wanted. To solve for \(\theta_1\) we are going to take a more geometric approach. We will use a diagram of our problem to infer relations between our variables.

Personally, I prefer an algebraic derivation because it is more likely to work in all cases. With the geometric derivation, we need to be very careful that what we infer from our diagram remains true for all diagrams. It is possible that what looks true at first, might be false when the bones are bent at unexpected angles or when they are of unexpected lengths. If anyone has a purely algebraic break down of this step, I would be interested in learning it. [See the comments section for an algebraic solution by Csala Tamás.]

figure 4

In figure 4, the angles \(\theta_3\) and \(\theta_4\) are defined so that \(\theta_1 = \theta_4 - \theta_3\). Our approach will use the angle sum and difference identity for tangent to solve for \(\tan \theta_1\) through \(\tan (\theta_4 - \theta_3)\).

We can use the right triangle ratios to solve for \(\tan\theta_3\) and \(\tan\theta_4\).

In figure 4, the magnitudes of \(\vec{DE}\) and \(\vec{BE}\) can be solved using the right triangle ratios with respect to triangle \(DBE\).

\(|\vec{DE}| = d_2 \cos \theta_2\)

\(|\vec{BE}| = d_2 \sin \theta_2\)

The magnitude of \(\vec{AE}\) is the sum of \(d_1\) and \(|\vec{DE}|\). Compute \(\tan \theta_3\) based on the right triangle \(ABE\).

\(\tan \theta_3 = \frac {d_2 sin \theta_2}{d_1 + d_2 \cos \theta_2} \)

Compute \(\tan \theta_4\) based on the right triangle \(ABC\).

\(\tan \theta_4 = \frac {y}{x} \)

Use the angle sum and difference identity for tangent to solve \(\tan \theta_1\).

\(\tan \theta_1 = \tan (\theta_4 - \theta_3) = \frac{\tan \theta_4 - \tan \theta_3}{1 + \tan \theta_4 \tan \theta_3}\)

So far things look pretty straight forward, but what happens if we decided to solve \(\theta_2\) for the negative solution instead of the diagramed positive value? In that case, \(\theta_1\) ends up as the largest angle instead of \(\theta_4\). At first glance, it looks like \(\theta_1 = \theta_4+ \theta_3\), but our equation for \(\theta_3\) above will also result in a negated angle due to the sine term in its numerator. This brings us back to our original euqation of \(\theta_1 = \theta_4- \theta_3\) and thus our solution is still valid.

Expand \(\tan \theta_3\) and \(\tan \theta_4\).

\(\tan \theta_1 = \frac{ \frac {y}{x} - \frac {d_2 sin \theta_2}{d_1 + d_2 \cos \theta_2} } { 1 + \frac {y}{x} \frac {d_2 sin \theta_2}{d_1 + d_2 \cos \theta_2} }\)

Simplify by multiplying the numerator and denominator by \(x(d_1 + d_2 \cos \theta_2)\).

\(\tan \theta_1 = \frac{ y(d_1 + d_2 \cos \theta_2) - x(d_2 sin \theta_2) } { x(d_1 + d_2 \cos \theta_2) + y(d_2 sin \theta_2) }\)

Our desired value for \(\theta_1\) could point in any direction, but if we were to solve for it with \(\arctan \theta\), we would only get a range of \([-\frac{\pi}{2},\frac{\pi}{2}]\). We would also be running the risk of dividing by zero.

In order to extract the proper angle, we can use the atan2 function. This will evaluate the principle value of \(\arctan( \frac{ \dot{y} } { \dot{x} } )\) by interpreting the numerator and denominator as a point \((\dot{x},\dot{y})\) and calculating its angle from the positive x-axis.

Separate the numerator and denominator into the coordinate values and evaluate for \(\theta_1\).

\(\dot{x} = x(d_1 + d_2 \cos \theta_2) + y(d_2 sin \theta_2)\)

\(\dot{y} = y(d_1 + d_2 \cos \theta_2) - x(d_2 sin \theta_2)\)

\(\theta_1 = \operatorname{atan2}(\dot{y}, \dot{x})\)

\(\theta_1 = \operatorname{atan2}(y(d_1 + d_2 \cos \theta_2) - x(d_2 sin \theta_2), x(d_1 + d_2 \cos \theta_2) + y(d_2 sin \theta_2))\)

In the case where \(\dot{x}\) and \(\dot{y}\) are both zero, \(\operatorname{atan2}(\dot{y}, \dot{x})\) is technically undefined (any angle would be a valid result), but it is generally implemented to return zero in this case. This is true for both the C and C# languages.

Writing the code

We can now write our algorithm in code. The function will calculate a valid IK solution if possible. If there is no valid solution, it will calculate one that gets the end of bone₂ as close to the target as possible. Check out the RJ_Demo_IK application for a functional implementation of this code.

These code samples are released under the following license.

/******************************************************************************
  Copyright (c) 2008-2009 Ryan Juckett
  http://www.ryanjuckett.com/
 
  This software is provided 'as-is', without any express or implied
  warranty. In no event will the authors be held liable for any damages
  arising from the use of this software.
 
  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:
 
  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
 
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
 
  3. This notice may not be removed or altered from any source
     distribution.
******************************************************************************/

///***************************************************************************************
/// CalcIK_2D_TwoBoneAnalytic
/// Given a two bone chain located at the origin (bone1 is the parent of bone2), this
/// function will compute the bone angles needed for the end of the chain to line up
/// with a target position. If there is no valid solution, the angles will be set to
/// get as close to the target as possible.
///  
/// returns: True when a valid solution was found.
///***************************************************************************************
public static bool CalcIK_2D_TwoBoneAnalytic
(
    out double angle1,   // Angle of bone 1
    out double angle2,   // Angle of bone 2
    bool solvePosAngle2, // Solve for positive angle 2 instead of negative angle 2
    double length1,      // Length of bone 1. Assumed to be >= zero
    double length2,      // Length of bone 2. Assumed to be >= zero
    double targetX,      // Target x position for the bones to reach
    double targetY       // Target y position for the bones to reach
)
{
    Debug.Assert(length1 >= 0);
    Debug.Assert(length2 >= 0);
 
    const double epsilon = 0.0001; // used to prevent division by small numbers
 
    bool foundValidSolution = true;
 
    double targetDistSqr = (targetX*targetX + targetY*targetY);
 
    //===
    // Compute a new value for angle2 along with its cosine
    double sinAngle2;
    double cosAngle2;
 
    double cosAngle2_denom = 2*length1*length2;
    if( cosAngle2_denom > epsilon )
    {
        cosAngle2 =   (targetDistSqr - length1*length1 - length2*length2)
                    / (cosAngle2_denom);
 
        // if our result is not in the legal cosine range, we can not find a
        // legal solution for the target
        if( (cosAngle2 < -1.0) || (cosAngle2 > 1.0) )
            foundValidSolution = false;
 
        // clamp our value into range so we can calculate the best
        // solution when there are no valid ones
        cosAngle2 = Math.Max(-1, Math.Min( 1, cosAngle2 ) );
 
        // compute a new value for angle2
        angle2 = Math.Acos( cosAngle2 );
 
        // adjust for the desired bend direction
        if( !solvePosAngle2 )
            angle2 = -angle2;
 
        // compute the sine of our angle
        sinAngle2 = Math.Sin( angle2 );
    }
    else
    {
        // At least one of the bones had a zero length. This means our
        // solvable domain is a circle around the origin with a radius
        // equal to the sum of our bone lengths.
        double totalLenSqr = (length1 + length2) * (length1 + length2);
        if(    targetDistSqr < (totalLenSqr-epsilon)
            || targetDistSqr > (totalLenSqr+epsilon) )
        {
            foundValidSolution = false;
        }
 
        // Only the value of angle1 matters at this point. We can just
        // set angle2 to zero. 
        angle2    = 0.0;
        cosAngle2 = 1.0;
        sinAngle2 = 0.0;
    }
 
    //===
    // Compute the value of angle1 based on the sine and cosine of angle2
    double triAdjacent = length1 + length2*cosAngle2;
    double triOpposite = length2*sinAngle2;
 
    double tanY = targetY*triAdjacent - targetX*triOpposite;
    double tanX = targetX*triAdjacent + targetY*triOpposite;
 
    // Note that it is safe to call Atan2(0,0) which will happen if targetX and
    // targetY are zero
    angle1 = Math.Atan2( tanY, tanX );
 
    return foundValidSolution;
}

Defining the problem

The solvable domain

Math review

Solving for θ2

Solving for θ1

Writing the code

Comments

Solving for θ₂

Solving for θ₁