用Python写一个4自由度机械臂的逆运动学代码

以下是一个示例的Python代码，实现了4自由度机械臂的逆运动学：

import math

# 机械臂参数
L1 = 1.0  # 第一节长度
L2 = 1.0  # 第二节长度
L3 = 1.0  # 第三节长度
L4 = 1.0  # 第四节长度

# 计算末端坐标的函数
def forward_kinematics(q1, q2, q3, q4):
    x = L2 * math.cos(q1) + L3 * math.cos(q1 + q2) + L4 * math.cos(q1 + q2 + q3)
    y = L2 * math.sin(q1) + L3 * math.sin(q1 + q2) + L4 * math.sin(q1 + q2 + q3)
    z = L1 + L2 * math.sin(q4) + L3 * math.sin(q2 + q4) + L4 * math.sin(q2 + q3 + q4)
    return x, y, z

# 计算逆运动学的函数
def inverse_kinematics(x, y, z):
    # 计算第一节的角度
    q1 = math.atan2(y, x)
    # 计算第二、三、四节的角度
    cos_q4 = (x**2 + y**2 + (z-L1)**2 - L2**2 - L3**2 - L4**2) / (2 * L3 * L4)
    sin_q4 = math.sqrt(1 - cos_q4**2)
    q4 = math.atan2(sin_q4, cos_q4)
    k1 = L3 + L4 * cos_q4
    k2 = L4 * sin_q4
    cos_q2 = (k1*z - k2*math.sqrt(x**2 + y**2)) / (k1**2 + k2**2)
    sin_q2 = math.sqrt(1 - cos_q2**2)
    q2 = math.atan2(sin_q2, cos_q2)
    k3 = L2 + k1 * cos_q2 + k2 * sin_q2
    k4 = (z - L1) - k1 * sin_q2 + k2 * cos_q2
    cos_q3 = k3 / math.sqrt(k3**2 + k4**2)
    sin_q3 = math.sqrt(1 - cos_q3**2)
    q3 = math.atan2(sin_q3, cos_q3)
    return q1, q2, q3, q4

# 示例
x, y, z = 2.0, 2.0, 3.0  # 目标末端坐标
q1, q2, q3, q4 = inverse_kinematics(x, y, z)  # 计算逆运动学
print("关节角度：", q1, q2, q3, q4)
print("末端坐标：", forward_kinematics(q1, q2, q3, q4))  # 验证结果

需要注意的是，这只是一个示例代码，实际情况中还需要考虑机械臂的运动范围、奇异点等问题。