3个自由度机械臂 Inverse Kinematics的代码

qqqweiweiqq

已于 2025-02-25 08:27:10 修改

阅读量37

点赞数

分类专栏：机器学习自传文章标签：机器人物联网机器学习

于 2025-02-22 22:24:16 首次发布

本文链接：https://blog.csdn.net/qqqweiweiqq/article/details/145802063

版权

机器学习同时被 2 个专栏收录

46 篇文章 ¥119.90 ¥299.90

订阅专栏

超级会员免费看

自传

27 篇文章

订阅专栏

from numpy import *

# Length of links in cm
a1= 5.2





a2 = 6.9



a3 = 6.8

# Desired Position of End effector


px = -14



py = 3

phi = 90
phi = deg2rad(phi)

# Equations for Inverse kinematics



wx = px - a3*cos(phi)
wy = py - a3*sin(phi)

delta = wx**2 + wy**2




c2 = ( delta -a1**2 -a2**2)/(2*a1*a2)




s2 = sqrt(1-c2**2)  # elbow down




theta_2 = arctan2(s2, c2)



s1 = ((a1+a2*c2)*wy - a2*s2*wx)/delta



c1 = ((a1+a2*c2)*wx + a2*s2*wy)/delta




theta_1 = arctan2(s1,c1)



theta_3 = phi-theta_1-theta_2



print('theta_1: ', rad2deg(theta_1))



print('theta_2: ', rad2deg(theta_2))
print('theta_3: ', rad2deg(theta_3))