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
