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
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