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

我认为问题是引入trig函数的时候。它们的参数应该是弧度，它们的回答是度数。在某个地方我混淆了两个。我只是不知道在哪里。感谢

Answer 1

我担心，在你的代码或你正在使用的等式中，这有点错误。如果x和y是距离并且a1和a2是角度（检查你的等式或给出一个来源），你对theta2的等式对我没有任何意义。即使这些应该是d1和d2，这个等式也包括减去两个不同尺寸的数量（长度^ 4和长度^ 2）。

然后检查你的实现，它没有评估给定的等式。

我对弧度/度数的建议是在整个过程中使用弧度：如果需要，接受以度为单位的角度，然后立即转换为弧度进行计算，并将角度结果转换回输出的度数。

更多建议：

您不需要使用print将浮动元素转换为字符串以输出，只需使用print('x: ', x)等等。
为变量指定与其在公式中表示的符号相同的名称。这样可以更容易调试（好吧，如果方程是正确的话）。

希望有所帮助。

Foward /反向运动学计算2-DOF python

1 个答案: