Inverse Kinematics for a 2DOF planar robot

Question

I have a simple 2DOF planar robot with this configuration: L1 and L2 are the lengths of my 2 arms. L1 is 245.5mm and L2 is 160mm. Alpha is the first angle (the one of the base) and beta is the second angle attached to the first arm.

When I programmed an Arduino and tried my math, I just got inconsistent results. The points were just completely off. Even linear motion along one axis didn't work. So I wrote a simple python script with the exact same math, where I keep x or y constant and increase the other one in small increments. Then I plotted a graph of the alpha and beta angle which were calculated. I would expect them to increase or decrease linearly (approximately like the dashed lines). But instead I get these results: I am using the standard formulas which can be found all over the internet (Source to original), but clearly they don't work. Here are they:

$\beta = cos^{-1}(\frac{x^2+y^2-L_{1}^2-L_{2}^2}{2*L_{1}*L_2})$

$\alpha = tan^{-1}(\frac{y}{x})-tan^{-1}(\frac{L_2sin(\beta)}{L_1+L_2cos(\beta)})$

Also here is my Python code with which I do the calculations:

import math

L1 = 245.5
L2 = 160

x = 100
y = 70

def calc_IK_beta(x, y):
    beta = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    beta = math.acos(beta)

    beta_d = beta * 180 / math.pi

    if x < 0 and y > 0:
        beta_d = -beta_d
    elif x < 0 and y < 0:
        beta_d = -180 - beta_d
    elif x > 0 and y < 0:
        beta_d = -beta_d

    return beta #in radians

def calc_IK_alpha(x, y):
    beta = calc_IK_beta(x, y)

    alpha = math.atan2(y, x) - math.atan2(L2 * math.sin(beta), L1 + L2 * math.cos(beta))

    alpha_d = alpha * 180 / math.pi

    if x < 0 and y > 0:
        alpha_d = -alpha_d
    elif x < 0 and y < 0:
        alpha_d = -180 - alpha_d
    elif x > 0 and y < 0:
        alpha_d = 180 - alpha_d

    return alpha #in radians

When I programmed an Arduino and tried my math, I just got inconsistent results. The points were just completely off. Even linear motion along one axis didn't work. So I wrote a simple python script with the exact same math, where I keep x or y constant and increase the other one in small increments. Then I plotted a graph of the alpha and beta angle which were calculated. I would expect them to increase or decrease linearly (approximately like the dashed lines).

Answer 1

Your first formula (for beta) is correct ... but note that there are two angles beta that satisfy it. I don't know whether your second formula (for alpha) is correct, but note that inverting the tan function is open to errors from choosing the wrong solution in (-180,180] (or any other 360 degree angle range). I came up with a different expression for alpha (given in the code below).

In the code below:

there are TWO pairs of solutions for (alpha, beta) for each (x,y), as there must be (draw the sides that complete the "kite" shape);

I'm open to someone pointing out bugs, but I've tried to "back-check" by recomputing (x,y) from each of the two angle pairs.

from math import pi, cos, sin, acos, atan2


RAD_TO_DEG = 180.0 / pi


def angles_to_xy( L1, L2, alpha, beta ):
    x = L1 * cos( alpha ) + L2 * cos( alpha - beta )
    y = L1 * sin( alpha ) + L2 * sin( alpha - beta )
    return x, y


def xy_to_angles( L1, L2, x, y ):      # WARNING: there are TWO pairs of angles that satisfy this
    cosbeta = ( x ** 2 + y ** 2 - L1 ** 2 - L2 ** 2 ) / ( 2.0 * L1 * L2 )
    beta1 = acos( cosbeta )
    beta2 = -beta1
    A, B = L1 + L2 * cosbeta, L2 * sin( beta1 )
    alpha1 = atan2( y * A + x * B, x * A - y * B )
    alpha2 = atan2( y * A - x * B, x * A + y * B )
    return alpha1, beta1, alpha2, beta2


def checker( L1, L2, x, y ):
    alpha1, beta1, alpha2, beta2 = xy_to_angles( L1, L2, x, y )
    alpha1_deg, beta1_deg = alpha1 * RAD_TO_DEG, beta1 * RAD_TO_DEG
    alpha2_deg, beta2_deg = alpha2 * RAD_TO_DEG, beta2 * RAD_TO_DEG
    print( "alpha1, beta1, alpha2, beta2 (degs)= ", ( 4 * "{:7.2f}  " ).format( alpha1_deg, beta1_deg, alpha2_deg, beta2_deg ) )
    X1, Y1 = angles_to_xy( L1, L2, alpha1, beta1 )
    X2, Y2 = angles_to_xy( L1, L2, alpha2, beta2 )
    print( "Back-check X, Y (twice) = ", ( 4 * "{:7.2f}  " ).format( X1, Y1, X2, Y2 ) )
    print( "----------" )


L1, L2 = 245.5, 160.0

x, y = 100.0,  70.0;   checker( L1, L2, x, y )
x, y = 113.0, 359.0;   checker( L1, L2, x, y )

Output:

alpha1, beta1, alpha2, beta2 (degs)=    69.19   154.61     0.79  -154.61  
Back-check X, Y (twice) =   100.00    70.00   100.00    70.00  
----------
alpha1, beta1, alpha2, beta2 (degs)=    89.95    44.76    55.11   -44.76  
Back-check X, Y (twice) =   113.00   359.00   113.00   359.00  
----------