Foward/Inverse Kinematics Calculations 2-DOF python

Question

The program calculates the points of an end-effector with forward kinematics using the equation,

x = d1cos(a1) + d2cos(a1+a2)

y = d1sin(a1) + d2sin(a1+a2)

where d1 is the length of the first joint, d2 is the length of the second joint, a1 is the angle of the first joint and a2 is the angle of the second joint.

It calculates inverse kinematics by this equation

So, by entering the input required for forward kinematics I should get the points of the end effector. By entering in the same input and the points found in forward kinematics for inverse kinematics, I should get the angles I entered in as input for forward kinematics. But I do not get them back. Here is my code,

'''
Created on Oct 5, 2015

@author: justin
'''
import math
def getOption():
    print('Select an option:\n')
    print('\t1) Forward Kinematics\n')
    print('\t2) Inverse Kinematics\n')
    option = input()
    try:
        option = int(option)
        if option == 1:
            fowardKinematics()
        elif option == 2:
            inverseKinematics()
        else:
            print('Not an option')
            return
    except ValueError:
        print('Not an integer/Point cannot be reached')
        return
def fowardKinematics():
    '''
    Ask user for input and computing points of end-effector 
    '''
    length1 = input('Enter length of joint 1 (a1):\n')  # Getting input from keyboard
    angle1 = input('Enter angle of joint 1 (theta1):\n')
    length2 = input('Enter length of joint 2 (a2):\n')
    angle2 = input("Enter angle of join 2 (theta2)\n")

    try:
        length1 = float(length1)  # Testing to see if user entered a number
        length2 = float(length2)  # Testing to see if user entered a number
        angle1 = float(angle1)  # Testing to see if user entered a number
        angle2 = float(angle2)  # Testing to see if user entered a number
    except ValueError:
        print('Invalid input, check your input again')
        return
    x = (length1 * math.cos(math.radians(angle1))) + (length2 * math.cos((math.radians(angle1 + angle2))))  # a1c1 + a2c12
    y = (length1 * math.sin(math.radians(angle1))) + (length2 * math.sin((math.radians(angle1 + angle2))))  # a1s1 + a2s12
    print('The position of the end-effector P(x,y) is:\n')
    print('X: ' + str(x))  # Convert x to string
    print('Y: ' + str(y))  # Convert y to string
def inverseKinematics():
    length1 = input('Enter length of joint 1 (a1):\n')
    length2 = input('Enter length of joint 2 (a2):\n')
    x = input('Enter position of X:\n')
    y = input('Enter position of Y:\n')
    try:
        length1 = float(length1)
        length2 = float(length2)
        x = float(x)
        y = float(y)
    except ValueError:
        print('Invalid input, check your input again')
        return
    # Computing angle 2 Elbow up/down
    numerator = ((length1 + length2)**2) - ((x**2) + (y**2))
    denominator = ((x**2) + (y**2)) - ((length1 - length2)**2)
    angle2UP = math.degrees(math.atan(math.sqrt(numerator/denominator)))
    angle2DOWN = angle2UP * -1

    # Angle 1 Elbow up
    numerator = (length2 * math.sin(math.radians(angle2UP)))
    denominator = ((length1 + length2) * math.cos(math.radians(angle2UP)))
    angle1UP = math.degrees(math.atan2(numerator, denominator))
    # Angle 1 Elbow down
    numerator = (length2 * math.sin(math.radians(angle2DOWN)))
    denominator = ((length1 + length2) * math.cos(math.radians(angle2DOWN)))
    angle1DOWN = math.degrees(math.atan2(numerator, denominator))
    print("Angle 1 Elbow up: " + str(angle1UP))
    print("Angle 1 Elbow down: " + str(angle1DOWN))
    print("Angle 2 Elbow up: " + str(angle2UP))
    print("Angle 2 Elbow down: " + str(angle2DOWN))
if __name__ == '__main__':
    getOption()
    pass

I think the problem is when the trig functions get introduced. The parameters for them are supposed to be in radians, they return the answer is degrees. Somewhere I am mixing up the two. I just don't know where. Thanks

score 1 · Answer 1 · answered Oct 07 '15 at 14:49

There is, I'm afraid, quite a bit wrong with this, either in your code or in the equations you're using. Your equation for theta2 doesn't make any sense to me if x and y are distances and a1 and a2 are angles (check your equation or give a source). Even if these should be d1 and d2, this equation involves subtracting two quantities with different dimensions (length^4 and length^2).

Then check your implementation of it, which does not evaluate the equation as given.

My advice about radians / degrees is to use radians throughout: accept the angles in degrees if you want, but then immediately convert to radians for the calculations, and convert angular results back to degrees for output.

Some more advice:

you don't need to cast your floats to strings for output using print, just use print('x: ', x) and so on.
Give your variables the same names as the symbols they represent in your formula. This would make it easier to debug (well, it would if the equations were correct).

Hope that helps.

Foward/Inverse Kinematics Calculations 2-DOF python

1 Answers1