Kinematics Analysis of Robot Arm

When doing competitions before, it was necessary to grab the specified object through the robotic arm,Because the action group that executes the specified robotic arm is not flexible enough,And it can not meet the actual needs very well,Therefore, the kinematics analysis of the manipulator is adopted,Realize the grasping of the robotic arm.

源代码如下：

import math
""" pythonRobotic arm analysis of procedural kinematics 输入的参数：The length of the three links、X,Y,Z的坐标点 输出的参数：The angle of rotation of the four servos(j0,j1,j2,j3) 注：j4、j5The angle is determined by vision """
RAD2ANG = 3.1415926535898 / 180.0
# The length of the three links
L1 = 10.5
L2 = 9.5
L3 = 17

""" Robotic arm documentation 注：The following is an explanation from the angle of the robotic arm:1代表着45° 0号舵机-->-90°~90°(向左为正) 0°is the center 1号舵机-->-90°~90°(向下为正) 0°is directly above 2号舵机-->-90°~90°(向后为正) 0°is directly above 3号舵机-->-90°~90°(向后为正) 0°is directly above 4号舵机-->-90°~90°(逆时针为正) 0°cross claw 5号舵机-->0°~45°(向右为正) 0°to close the paws """


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


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


def jointToAngle(joint):
    return joint * 45


def nowAngle():
    # What is obtained is the angle corresponding to the current rotation of each servojoint值
    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,ZAnd the length of the three-axis robotic arm to get the angle of rotation of each servo(Note that only the first four angles are found,The latter two angles need to be adjusted according to the attitude of the target) 注意：这里的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)
            # There is an error between the inverse solution and the positive solution(-0.1,0.1)It is considered that the analysis of kinematics has been completed
            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°The coordinates of the robotic arm change,That is, the angle at which the robotic arm is closest to the vertical downward direction
                    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__':
    # given coordinates
    x = 15
    y = 0
    z = -13.5
    # Convert coordinates to angles
    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))