Written by Ryan Juckett

Monday, 29 December 2008 07:46

Article Index
Analytic Two-Bone IK in 2D
Defining the problem
The solvable domain
Math review
Describing the problem with math
Solving for angle 2
Solving for angle 1
Writing the code
All Pages

title 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.

fig_1

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).

fig_2

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.

Right triange 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 }$

Describing the problem with math

To solve the problem, we need to write it out in equation form. Our unknown variables are $\theta_1$ and $\theta_2$ . Our known variables are $d_1$ , $d_2$ , $x$ and $y$ . We are going to create two equations that define $x$ and $y$ in terms of the two bones.

Based on the right triangle ratios for $\sin\theta$ and $cos\theta$ , we can use the following equations to extract the adjacent and opposite sides of a right triangle.

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

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

fig_3

figure 3

By breaking the problem into a couple right triangles as seen in figure 3, we can now define $x$ as a sum of the adjacent sides and $y$ as a sum of opposite sides.

$x = d_1 \cos \theta_1 + d_2 \cos(\theta_1 + \theta_2)$

$y = d_1 \sin \theta_1 + d_2 \sin(\theta_1 + \theta_2)$

Solving for θ₂

Our first step will be solving for $\theta_2$ . In order to do this, we need to cancel $\theta_1$ out of our equations. By squaring and adding the equations for $x$ and $y$ , we can use the Pythagorean trigonometric identity to cancel out terms.

First off, lets square the equations.

$x^2 = d_1^2 \cos^2 (\theta_1) \quad + \quad 2 d_1 d_2 \cos (\theta_1) \cos(\theta_1 + \theta_2) \quad + \quad d_2^2 \cos^2(\theta_1 + \theta_2)$

$y^2 = d_1^2 \sin^2 (\theta_1) \quad + \quad 2 d_1 d_2 \sin (\theta_1) \sin(\theta_1 + \theta_2) \quad + \quad d_2^2 \sin^2(\theta_1 + \theta_2)$

Add the squared equations together.

$x^2+y^2 = d_1^2 \Big(\sin^2(\theta_1) \quad+ \quad \cos^2(\theta_1)\Big)\\ \qquad\qquad\qquad\qquad\qquad\qquad + \quad 2 d_1 d_2 \Big(\sin(\theta_1) \sin(\theta_1 + \theta_2) \quad+ \quad \cos(\theta_1) \cos(\theta_1 + \theta_2)\Big)\\ \qquad\qquad\qquad\qquad\qquad\qquad + \quad d_2^2 \Big(\sin^2(\theta_1 + \theta_2) \quad + \quad \cos^2(\theta_1 + \theta_2)\Big)$

Using Pythagorean trigonometric identity, simplify the $d_1^2$ and $d_2^2$ terms.

$x^2+y^2 = d_1^2 \quad + \quad d_2^2 \quad + \quad 2 d_1 d_2 \Big(\sin(\theta_1) \sin(\theta_1 + \theta_2) \quad+ \quad \cos(\theta_1) \cos(\theta_1 + \theta_2)\Big)$

Using the angle sum and difference identities for cosine and sine, expand $\sin(\theta_1 + \theta_2)$ and $\cos(\theta_1 + \theta_2)$ .

$x^2+y^2 = d_1^2 \quad + \quad d_2^2 \\ \qquad\qquad\qquad\qquad\qquad\qquad + \quad 2 d_1 d_2 \Big(\sin(\theta_1) ( \sin \theta_1 \cos \theta_2 + \cos \theta_1 \sin \theta_2 ) + \cos(\theta_1) ( \cos \theta_1 \cos \theta_2 - \sin \theta_1 \sin \theta_2) \Big)$

Distribute the $\sin (\theta_1)$ and $\cos (\theta_1)$ terms.

$x^2+y^2 = d_1^2 \quad + \quad d_2^2 \\ \qquad\qquad\qquad\qquad\qquad\qquad + \quad 2 d_1 d_2 \Big(\sin^2(\theta_1) \cos \theta_2 + \cos \theta_1 \sin \theta_1 \sin \theta_2 + \cos^2(\theta_1) \cos \theta_2 - \cos \theta_1 \sin \theta_1 \sin \theta_2 \Big)$

The positive and negative $\cos \theta_1 \sin \theta_1 \sin \theta_2$ terms cancel each other out, and we can factor $\cos \theta_2$ out of the remaining terms.

$x^2+y^2 = d_1^2 \quad + \quad d_2^2\quad + \quad 2 d_1 d_2 \Big( (\sin^2(\theta_1) + \cos^2(\theta_1)) \cos \theta_2 \Big)$

Use the Pythagorean trigonometric identity to simplify.

$x^2+y^2 = d_1^2 \quad + \quad d_2^2\quad + \quad 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.

fig_4

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)$ .

So far things look pretty straight forward, but what happens if our previously solved value for $\theta_2$ is negative instead of the diagramed positive value? This will cause $\theta_1$ to be the largest angle instead of $\theta_4$ resulting in $\theta_1 = \theta_4+ \theta_3$ . This will require us to solve for $\tan \theta_1$ as $\tan (\theta_4+ \theta_3)$ .

Because $\tan(-\theta) = \tan \theta$ , it turns out that $\tan (\theta_4+ \theta_3)$ will equal $\tan (\theta_4- \theta_3)$ when the angle sum and difference identity for tangent is expanded. Lets do a quick proof of this.

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

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

Now that we know solving for $\tan (\theta_4 - \theta_3)$ will suffice for all angles, 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}$

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 denominatoras 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.

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.
******************************************************************************/

C# Code

///***************************************************************************************
/// 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 leaset 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;
}

Last Updated ( Sunday, 02 May 2010 06:32 )

blog comments powered by Disqus

RyanJuckett.com site content by Ryan Juckett is licensed under a Creative Commons Attribution 3.0 United States License.

RyanJuckett.com