Beispiel #1
0
def GST(arm, theta):
    j = theta

    joint_angles = np.array([
        [0,     0,      0   ], # mount->s0
        [0,     0,      0   ], # mount->s0
        [0,     0,      j[0]], # mount->s0
        [0,     0,      j[1]], # s0->s1
        [0,     0,      j[2]], # s1->e0
        [0,     0,      j[3]], # e0->e1
        [0,     0,      j[4]], # e1->w0
        [0,     0,      j[5]], # w0->w1
        [0,     0,      j[6]], # w1->w2
        [0,     0,      0   ], # w2->hand
        [0,     0,      0   ]  # hand->gripper
    ], dtype=np.float32)

    _joint_translate = joint_translate.copy()
    _joint_axis = joint_axis.copy()

    if arm == "right":
        _joint_translate[0][1] *= -1
        _joint_axis[1][2] *= -1

    # Set Axis
    joint_angles += _joint_axis

    start = np.array([0, 0, 0])
    end = start
    pre = np.eye(3)
    ret = []
    for i in range(0, 10):
        rm = np.dot(pre, mathlib.eular_to_rotation_matrix(joint_angles[i][0], joint_angles[i][1], joint_angles[i][2]))
        end = start+np.dot(rm, _joint_translate[i])

        TF = np.identity(4)
        TF[0:3,0:3] = rm
        TF[0:3,3] = start
        ret.append(TF)

        start = end

        pre = rm


    TF = np.identity(4)
    TF[0:3,0:3] = pre
    TF[0:3,3] = start
    ret.append(TF)

    return ret
Beispiel #2
0
def inv_kin(arm, init_joint, pos, ori):
    np.set_printoptions(precision=4,suppress=True)

    joint_limits = [
        [-0.8,    1.4],
        [-1.4,    0.6],
        [-3.14,    3.14],
        [0,       1.77],
        [-3.14,    3.14],
        [-1.56,    2.08],
        [-3.14,    3.14]
    ]

    kin = kdl_kinematics.create_kdl_kin(arm+"_arm_mount", arm+"_wrist", "baxter_urdf.xml")

    N = 3000

    th1 = np.array(init_joint, dtype=np.float32)

    g = GST(arm, th1)
    target = np.identity(4)
    target[0:3,3] = pos
    target[0:3,0:3] = mathlib.eular_to_rotation_matrix(ori[0], ori[1], ori[2])

    th = th1
    th = th.tolist()

    for i in range(N):
        dist = mathlib.tr2diff(target, g[10])
        dist[3] = dist[3]
        dist[4] = -dist[4]
        dist[5] = dist[5]

        J = kin.jacobian(th)
        J_d = np.linalg.pinv(J)

        end = True
        for j in range(3):
            if math.fabs(dist[j]) > 0.001:
                end = False
        #for j in range(2):
        if math.fabs(dist[3]) > math.radians(2) and math.fabs(dist[3]) < math.pi-math.radians(2):
            end = False
        if math.fabs(dist[4]) > math.radians(2) and math.fabs(dist[4]) < math.pi-math.radians(2):
            end = False
        if math.fabs(dist[5]) > math.radians(5) and math.fabs(dist[5]) < math.pi-math.radians(5):
            end = False

        if end:
            break

        #print i, dist
        #dist[0:3] = mathlib.unit_vector(dist[0:3])
        di = np.array([0, 0, 0, 0, 0, 0], dtype=np.float32)
        di[0:3] = np.array(dist[0:3])*0.1
        di[3:6] = np.array(dist[3:6])*0.01
        #di[0:3] = dist[0:3]*0.01
        #di[3:6] = dist[3:6]*0.01

        theta_dot = np.dot(J_d, di)

        th = th+(theta_dot)
        th = th.tolist()[0]

        for j in range(7):
            angle = th[j]
            th[j] = math.atan2(math.sin(angle), math.cos(angle))

        for j in range(7):
            angle = th[j]

            if angle < joint_limits[j][0]:
                th[j] = joint_limits[j][0]
            elif angle > joint_limits[j][1]:
                th[j] = joint_limits[j][1]

        g = GST(arm, th)

    print "[INVKIN] FIND SOLUTION at "+str(i)+", "+str(dist)
    return th, dist