机械臂运动学解析

之前做比赛的时候需要通过机械臂进行抓取指定的物体，由于执行指定的机械臂的动作组不够灵活，而且不能很好地满足实际的需求，因此采取机械臂的运动学解析，实现机械臂的抓取。

源代码如下：

import math
"""
python程序运动学的机械臂解析
输入的参数：三个连杆的长度、X,Y,Z的坐标点
输出的参数：四个舵机旋转的角度(j0,j1,j2,j3)
注：j4、j5的角度是通过视觉进行角度的确定

"""
RAD2ANG = 3.1415926535898 / 180.0
# 三个连杆的长度
L1 = 10.5
L2 = 9.5
L3 = 17

"""
    机械臂文档说明
    注：接下来是以机械臂的角度进行一个说明:1代表着45°
    0号舵机-->-90°~90°(向左为正)  0°为正中心
    1号舵机-->-90°~90°(向下为正)  0°为正上方
    2号舵机-->-90°~90°(向后为正)  0°为正上方
    3号舵机-->-90°~90°(向后为正)  0°为正上方
    4号舵机-->-90°~90°(逆时针为正)  0°为横爪
    5号舵机-->0°~45°(向右为正)    0°为闭合爪子

"""


def and2Rad(N):
    return (N) * (180.0 / 3.1415926535898)


def angleToJoint(angle):
    return angle * (1 / 45)


def jointToAngle(joint):
    return joint * 45


def nowAngle():
    # 获取的是当前各个舵机转动的角度对应的joint值
    print("now joint:")
    i = 0
    for joint in arm.GetJointState():
        print("joint" + str(i) + ":" + str(jointToAngle(joint)))
        i += 1


def inverseKinematics(x, y, z, L1, L2, L3):
    """
    运动学逆解：通过传入的X,Y,Z以及三轴机械臂的长度得到每个舵机转动的角度(注意只是求出了前四个的角度，后两个的角度需要根据目标的姿态进行一个调整)
    注意：这里的0,1,2,3表示的为j1,j2,j3,j4
    """
    global final_j2, final_j3, final_j1, final_j0
    i = 0
    nearest_j3 = 1000
    j0 = math.atan2(y, x)
    a = x / math.cos(j0)
    if (x == 0):
        a = y
    b = z
    for j1 in range(-90, 90):
        j1 *= RAD2ANG
        try:
            j3 = math.acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a * L1 * math.sin(
                j1) - 2 * b * L1 * math.cos(j1)) / (2 * L2 * L3))
            m = L2 * math.sin(j1) + L3 * math.sin(j1) * math.cos(j3) + L3 * math.cos(j1) * math.sin(j3)
            n = L2 * math.cos(j1) + L3 * math.cos(j1) * math.cos(j3) - L3 * math.sin(j1) * math.sin(j3)
            t = a - L1 * math.sin(j1)
            p = math.pow(math.pow(n, 2) + math.pow(m, 2), 0.5)
            q = math.asin(m / p)
            j2 = math.asin(t / p) - q
            x1 = (L1 * math.sin(j1) + L2 * math.sin(j1 + j2) + L3 * math.sin(j1 + j2 + j3)) * math.cos(j0)
            y1 = (L1 * math.sin(j1) + L2 * math.sin(j1 + j2) + L3 * math.sin(j1 + j2 + j3)) * math.sin(j0)
            z1 = L1 * math.cos(j1) + L2 * math.cos(j1 + j2) + L3 * math.cos(j1 + j2 + j3)
            j1 = and2Rad(j1)
            j2 = and2Rad(j2)
            j3 = and2Rad(j3)
            # 逆解与正解的误差在(-0.1,0.1)之间认为完成了运动学的解析
            if (x + 0.1) > x1 > (x - 0.1) and (y + 0.1) > y1 > (y - 0.1) and (z + 0.1) > z1 > (z - 0.1):
                if -90 < j1 < 90 and 0 < j2 < 180 and -90 < j3 < 180:
                    # 选取的是j3最接近90°的机械臂的坐标变化，即机械臂最接近垂直向下的角度
                    if abs(j3 - 90) < nearest_j3:
                        nearest_j3 = abs(j3 - 90)
                        # print("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n" %(and2Rad(j0), j1, j2, j3, x1, y1, z1))
                        final_j0 = and2Rad(j0)
                        final_j1 = j1
                        final_j2 = j2
                        final_j3 = j3
                        i = 1
        except:
            # print("值出现Nan")
            pass
    for j1 in range(-90, 90):
        j1 *= RAD2ANG
        try:
            j3 = math.acos((math.pow(a, 2) + math.pow(b, 2) + math.pow(L1, 2) - math.pow(L2, 2) - math.pow(L3,
                                                                                                           2) - 2 * a * L1 * math.sin(
                j1) - 2 * b * L1 * math.cos(j1)) / (2 * L2 * L3))
            m = L2 * math.sin(j1) + L3 * math.sin(j1) * math.cos(j3) + L3 * math.cos(j1) * math.sin(j3)
            n = L2 * math.cos(j1) + L3 * math.cos(j1) * math.cos(j3) - L3 * math.sin(j1) * math.sin(j3)
            t = a - L1 * math.sin(j1)
            p = math.pow(math.pow(n, 2) + math.pow(m, 2), 0.5)
            q = math.asin(m / p)
            j2 = -(math.asin(t / p) - q)
            x1 = (L1 * math.sin(j1) + L2 * math.sin(j1 + j2) + L3 * math.sin(j1 + j2 + j3)) * math.cos(j0)
            y1 = (L1 * math.sin(j1) + L2 * math.sin(j1 + j2) + L3 * math.sin(j1 + j2 + j3)) * math.sin(j0)
            z1 = L1 * math.cos(j1) + L2 * math.cos(j1 + j2) + L3 * math.cos(j1 + j2 + j3)
            j1 = and2Rad(j1)
            j2 = and2Rad(j2)
            j3 = and2Rad(j3)
            if (x + 0.1) > x1 > (x - 0.1) and (y + 0.1) > y1 > (y - 0.1) and (z + 0.1) > z1 > (z - 0.1):
                if -90 < j1 < 90 and 0 < j2 < 180 and -90 < j3 < 180:
                    if abs(j3 - 90) < nearest_j3:
                        nearest_j3 = abs(j3 - 90)
                        # print("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n" %(and2Rad(j0), j1, j2, j3, x1, y1, z1))
                        final_j0 = and2Rad(j0)
                        final_j1 = j1
                        final_j2 = j2
                        final_j3 = j3
                        i = 1

        except:
            # print("值出现Nan")
            pass

    if i == 0:
        print("无解")
        return None
    else:
        return final_j0, final_j1, final_j2, final_j3


if __name__ == '__main__':
    # 给定的坐标
    x = 15
    y = 0
    z = -13.5
    # 坐标转换为角度
    if inverseKinematics(x, y, z, L1, L2, L3) is None:
        print("无解")
    else:
        j0, j1, j2, j3 = inverseKinematics(x, y, z, L1, L2, L3)
        print("j0=%f,j1=%f,j2=%f,j3=%f\n" % (j0, j1, j2, j3))