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