该程序使用以下等式通过正向运动学计算末端执行器的点，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是角度，则theta2的等式对我来说没有任何意义（请检查您的等式或提供来源）。即使这些参数应为d1和d2，该等式也涉及减去两个具有不同尺寸的量（长度^ 4和长度^ 2）。
然后检查它的实现，该实现不会评估给定的方程式。
我对弧度/度的建议是在整个弧度中使用弧度：如果需要，请接受以度为单位的角度，然后立即将其转换为弧度进行计算，并将角度结果转换回度以进行输出。
更多建议：
您不需要将浮点数转换为字符串以使用print输出，只需使用print('x: ', x)等等。
为变量指定与公式中表示的符号相同的名称。这将使调试变得更容易（嗯，如果方程式正确的话）。
希望能有所帮助。
关于python - 正向/反向运动学计算2-DOF python，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/32978569/