def kuka2local(self, pose_kukaLocal, ref, target="cs"):
"""
kuka local cs to natrual local cs
"""
if isinstance(ref, str):
idx = self.link_names.index(ref)
else:
idx = ref
if target == "cs":
if len(pose_kukaLocal) == 6:
[x, y, z, A, B, C] = pose_kukaLocal
else:
[x, y, z, A, B, C] = np.hstack((pose_kukaLocal, ar([0, 0, 0])))
g_kLo = h(self.joints[idx].align, [0, 0, 0])
coord_kLo = h(rotation(A, B, C, order="zyx"), [x, y, z])
coord_local = g_kLo @ coord_kLo
A_lo, B_lo, C_lo = solveZYX(coord_local[0:3, 0:3])
[x_lo, y_lo, z_lo] = coord_local[0:3, 3]
return ar([x_lo, y_lo, z_lo, A_lo, B_lo, C_lo])
elif target == "vector":
v_lo = self.joints[idx].align @ pose_kukaLocal
return v_lo
else:
print("invalid input")
return
def local2world(self, pose_local, ref, target="cs"):
"""
natrual local cs to world cs
"""
if isinstance(ref, str):
idx = self.link_names.index(ref)
else:
idx = ref
if target == "cs":
if len(pose_local) == 6:
[x, y, z, A, B, C] = pose_local
else:
[x, y, z, A, B, C] = np.hstack((pose_local, ar([0, 0, 0])))
coord_local = h(rotation(A, B, C, order="zyx"),
ar([x, y, z]) + self.joints[idx].origin0)
coord_world = self.joints[idx].G_gl @ coord_local
A_gl, B_gl, C_gl = solveZYX(coord_world[0:3, 0:3])
[x_gl, y_gl, z_gl] = coord_world[0:3, 3]
return ar([x_gl, y_gl, z_gl, A_gl, B_gl, C_gl])
elif target == "vector":
v_gl = self.joints[idx].G_gl[0:3, 0:3] @ pose_local
return v_gl
else:
print("invalid input")
return
def ik_industry(tcp, geometry, st=None):
"""
inverse kinematics, not a universal method. Only for configuration with:
1) 6 rotation joints
2) z1 ^; z2 x; z3 x; z4 <; z5 x; z6 <
3) a common intersection point of A4, A5, A6 axes.
4) offset only on arm link (flange 2 is not on the axis of flange 1)
___l22____A5
l21 |A3
/
/
l1 /
/
A2/
Input:
tcp = {"origin":np.array([x, y, z]), "orientation":np.array([A, B, C])}
"""
if type(tcp) is dict:
x, y, z = tcp["tcp"]
A, B, C = tcp["orientation"]
elif len(tcp) == 3:
x, y, z = tcp[0:3]
A, B, C = 0, 0, 0
elif len(tcp) == 6:
x, y, z, A, B, C = tcp
d = np.linalg.norm(geometry[5].origin0[[0, 2]] - \
geometry[7].origin0[[0, 2]])
R06 = rotation(A, B, C, "zyx")
G_gl_tcp = h(R06, [x, y, z])
hand = (G_gl_tcp @ ar([-d, 0, 0, 1]))[0:3]
theta1 = np.arctan2(hand[1], hand[0])
A5 = geometry[5].origin0[[0, 2]]
A3 = geometry[3].origin0[[0, 2]]
A2 = geometry[2].origin0[[0, 2]]
l21 = A5[1] - A3[1]
l22 = A5[0] - A3[0]
theta31 = np.arctan2(l21, l22)
l23 = np.linalg.norm(A5 - A3)
l1 = np.linalg.norm(A3 - A2)
l2 = l23
joint1 = geometry[1]
joint2 = geometry[2]
origin2 = joint2.origin0
origin2[1] = 0
joint1.twist(-theta1)
joint2_newO = (joint1.G_indv @ np.hstack((origin2, ar([1]))))[0:3]
l3 = np.linalg.norm(hand - joint2_newO)
delta_big = triangle(l1, l2, l3)
theta21 = np.arctan2(hand[2] - joint2_newO[2],
np.linalg.norm(hand[[0, 1]] - joint2_newO[[0, 1]]))
theta22 = delta_big[1]
theta2 = -(theta21 + theta22)
theta32 = delta_big[2]
theta3 = np.pi + theta31 - theta32
R03 = rotation(theta1, theta2, theta3, "zyy")
R36 = R03.T @ R06
theta4, theta5, theta6 = solveEuler(R36)
return ar([-theta1, theta2, theta3, -theta4, theta5, -theta6])