def pose_to_transform(self, pos, rot):
"""
:param pos: position (m)
:param rot: quaternion (qx, qy, qz, qw)
:return:
"""
T = np.zeros((4, 4))
R = U.quaternion_to_R(rot[0], rot[1], rot[2], rot[3])
T[:3, :3] = R
T[:3, -1] = np.transpose(pos)
T[-1, -1] = 1
return T
def pose_to_transform(cls, pos, quat):
"""
:param pos: position (m)
:param rot: quaternion (qx, qy, qz, qw)
:return:
"""
R = U.quaternion_to_R(quat)
if np.shape(R) == (3, 3):
T = np.zeros((4, 4))
T[:3, :3] = R
T[:3, -1] = pos
T[-1, -1] = 1
else:
T = np.zeros((len(R), 4, 4))
T[:, :3, :3] = R
T[:, :3, -1] = pos
T[:, -1, -1] = 1
return T
# # jaw2 = [0.0]
joint1 = [0.4, 0.0, 0.15, 0.0, 1.0, 0.0]
joint2 = [0.4, 0.0, 0.15, 0.0, -1.0, 0.0]
# q5 = joint1[4]
# q6 = (0.830634273)/(1.01857984)*q5
# joint1[-1] = q6
# q5 = joint2[4]
# q6 = (0.830634273)/(1.01857984)*q5
# joint2[-1] = q6
# # p1.set_joint(joint=joint1)
# # p1.set_joint(joint=joint2)
print ("started")
from scipy.spatial.transform import Rotation
joint = [0.0, 0.0, 0.15, 0.0, 0.0, 0.5]
pos, rot = dvrkKinematics.joint_to_pose(joint)
RR = U.quaternion_to_R(rot)
print (RR)
while True:
pass
# time.sleep(1)
# arm1.set_joint(joint=joint2, wait_callback=True)
# print ("cleared")
# arm1.set_joint(joint=joint2, wait_callback=False)
# print ("joint 2 cleared")
# arm1.set_pose_interpolate(pos=pos1, rot=q1)
# arm1.set_pose_interpolate(pos=pos2, rot=q2)
# # arm1.set_joint_dinterpolate(joint=joint1, method='LSPB')
# # arm1.set_joint_interpolate(joint=joint2, method='LSPB')
# # print ("moved")