Foward/инверсной кинематики Расчеты 2-DOF Python

Question

Программа рассчитывает точки конечного эффектора с передними кинематики с использованием уравнения,

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

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

, где d1 длина из первый шарнир, d2 - длина второго шва, a1 - угол первого соединения, а a2 - угол второго сустава.

Он вычисляет обратную кинематику этого уравнения

Таким образом, введя нужный вход для передних кинематик я должен получить точки конечного эффектора. Введя один и тот же вход и точки, найденные в кинематике прямой кинематики для обратной кинематики, я должен получить углы, введенные мной в качестве входных сигналов для кинематики вперед. Но я не верну их. Вот мой код,

''' 
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 

Я думаю, что проблема, когда тригонометрические функции ознакомятся. Параметры для них должны быть в радианах, они возвращают ответ в градусы. Где-то я смешиваю их. Я просто не знаю, где. Спасибо

Answer 1

Существует, я боюсь, совсем немного неправильно с этим, как в вашем коде, так и в уравнениях, которые вы используете. Ваше уравнение для theta2 не имеет для меня никакого смысла, если x и y - расстояния, a1 и a2 - углы (проверьте ваше уравнение или укажите источник). Даже если они должны быть d1 и d2, это уравнение включает вычитание двух величин с разными размерами (длина^4 и длина^2).

Затем проверьте свою реализацию, которая не оценивает приведенное уравнение.

Мой совет о радианах/градусах - использовать радианы повсюду: примите углы в градусах, если хотите, но затем немедленно конвертируйте в радианы для расчетов и преобразуйте угловые результаты обратно в градусы для вывода.

Некоторые больше советов:

• вам не нужно, чтобы бросить свои поплавки строки для вывода с помощью печати, просто используйте print('x: ', x) и так далее.

• Дайте вашим переменным те же имена, что и символы, которые они представляют в формуле. Это облегчило бы отладку (ну, если бы уравнения были правильными).

Надеюсь, что это поможет.