python - 正向/反向运动学计算 2DOF 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

恐怕，无论是在您的代码中还是在您正在使用的方程中，这都存在很大的错误。如果 x 和 y 是距离，a1 和 a2 是角度，你的 theta2 方程对我来说没有任何意义(检查你的方程或给出来源)。即使它们应该是 d1 和 d2，该方程也涉及减去两个不同维度的量(长度^4 和长度^2)。

然后检查您的实现，它不会评估给定的方程。

我关于弧度/度数的建议是始终使用弧度:如果需要，可以接受以度为单位的角度，但随后立即转换为弧度进行计算，并将角度结果转换回度数进行输出。

更多建议:

• 您不需要使用 print 将 float 转换为字符串进行输出，只需使用 print('x: ', x) 等即可。

  <

• 为变量指定与其在公式中表示的符号相同的名称。这将使调试变得更容易(好吧，如果方程正确的话)。

希望有帮助。