Analytic Two-Bone IK in 2D - Solving for angle 1

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

Page 7 of 8

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.