Foward/cinématique inverse Calculs 2-DOF python

Question

Le programme calcule les points d'un effecteur d'extrémité avec une cinématique à terme en utilisant l'équation,

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

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

où d1 est la longueur de la premier joint, d2 est la longueur de la deuxième articulation, est l'angle de la première articulation et a2 est l'angle de la deuxième articulation.

Il calcule la cinématique inverse par cette équation

Ainsi, en entrant l'entrée nécessaire pour la cinématique avant que je devrais obtenir les points de l'effecteur. En entrant la même entrée et les points trouvés dans la cinématique directe pour la cinématique inverse, je devrais obtenir les angles que j'ai entrés comme entrée pour la cinématique vers l'avant. Mais je ne les récupère pas. Voici mon 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 

Je pense que le problème est lorsque les fonctions trigonométriques vous présenter. Les paramètres pour eux sont supposés être en radians, ils retournent la réponse est des degrés. Quelque part je mélange les deux. Je ne sais pas où. Merci

Answer 1

Il y a, je le crains, un peu de mal à cela, que ce soit dans votre code ou dans les équations que vous utilisez. Votre équation pour theta2 n'a aucun sens pour moi si x et y sont des distances et a1 et a2 sont des angles (vérifiez votre équation ou donnez une source). Même si celles-ci doivent être d1 et d2, cette équation consiste à soustraire deux quantités de dimensions différentes (longueur^4 et longueur^2).

Ensuite, vérifiez votre implémentation, qui n'évalue pas l'équation comme donnée. Mon conseil à propos des radians/degrés est d'utiliser des radians partout: acceptez les angles en degrés si vous le souhaitez, mais convertissez immédiatement en radians pour les calculs, et convertissez les résultats angulaires en degrés pour la sortie.

Quelques autres conseils:

• vous n'avez pas besoin de jeter vos flotteurs pour les chaînes de sortie en utilisant l'impression, il suffit d'utiliser print('x: ', x) et ainsi de suite.

• Donnez à vos variables le même nom que les symboles qu'elles représentent dans votre formule. Cela faciliterait le débogage (enfin, si les équations étaient correctes).

Espérons que ça aide.