1

该程序使用以下等式计算具有正向运动学的末端执行器的点，

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

我认为问题在于何时引入三角函数。它们的参数应该是弧度，它们返回的答案是度数。我在某处将两者混为一谈。我只是不知道在哪里。谢谢

4

1 回答 1

1

恐怕在您的代码或您正在使用的方程式中，这有点不对劲。如果 x 和 y 是距离而 a1 和 a2 是角度（检查你的方程或给出一个来源），那么你的 theta2 方程对我没有任何意义。即使这些应该是 d1 和 d2，这个等式也包括减去两个不同维度的量（长度 ^ 4 和长度 ^ 2）。

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

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

更多建议：

  • 您不需要使用打印将浮点数转换为字符串以进行输出，只需使用print('x: ', x)等等。

  • 为您的变量提供与它们在公式中表示的符号相同的名称。这将使调试变得更容易（好吧，如果方程式是正确的，它会）。

希望有帮助。

于 2015-10-07T14:49:38.673 回答