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

程序使用以下等式计算具有正向运动学的末端效应器的点：

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函数。它们的参数应该是弧度，它们返回的答案是度。在某个地方，我把两者混在一起了。我只是不知道在哪里。谢谢

恐怕这有点不对劲，不管是在你的代码中还是在你使用的方程中。如果x和y是距离，a1和a2是角度，那么你的θ2方程对我来说没有任何意义（检查你的方程或给出一个来源）。即使它们应该是d1和d2，这个方程也需要减去两个尺寸不同的量（长度^4和长度^2）

然后检查它的实现，它没有按照给定的公式进行计算

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

还有一些建议：

• 您不需要使用print将浮点数转换为字符串进行输出，只需使用

  print（'x:'，x）

  等等

• 为变量指定与它们在公式中表示的符号相同的名称。这将使调试更容易（如果方程式正确的话）

希望有帮助