def _update_mesh(self, quat): q = Quaternion(quat[1], quat[2], quat[3], quat[4]) axis = q.get_axis(undefined=[1, 0, 0]) angle = q.angle * 180.0 / pi def do_things(target): target.resetTransform() self._default_transform(target) target.rotate(angle, axis[0], axis[1], axis[2]) do_things(self.mesh) do_things(self.plane_axis)
def from_mat4(cls, m): """ Construct Transform from matrix m :param m: 4x4 affine transformation matrix :return: rotation (quaternion), translation, scale """ translation = m[:3, 3] scale_rotation = m[:3, :3] u, _ = scipy.linalg.polar(scale_rotation) rotation_q = Quaternion(matrix=u) r_axis = rotation_q.get_axis(undefined=[0, 1, 0]) # ensure that rotation axis is +y if np.array(r_axis).dot(np.array([0, 1, 0])) < 0: rotation_q = -rotation_q scale = np.linalg.norm(scale_rotation, axis=0) return cls(translation=translation, rotation=rotation_q, scale=scale)
def _get_orientation_from_quaternion(elements): q = Quaternion(elements) return q.get_axis()[2] * q.angle
def quat2aa(q): # from pyQuaternion my_quaternion = Quaternion(q) # pyQuaternion ax = my_quaternion.get_axis() theta = my_quaternion.radians return theta * ax
move_to_pose = rospy.ServiceProxy( 'panda_grasp_server/panda_move_pose', PandaMove) move_success = move_to_pose(pose).success if move_success: poses_reached += 1 get_robot_state = rospy.ServiceProxy( 'panda_grasp_server/panda_get_state', PandaGetState) robot_state = get_robot_state().robot_state eef_pose = robot_state.eef_state.pose print(eef_pose) # we need the pose in axis-angle representation eef_orient = Quaternion(eef_pose.orientation.w, eef_pose.orientation.x, eef_pose.orientation.y, eef_pose.orientation.z) eef_orient_rotvec = eef_orient.get_axis() * eef_orient.angle # pose_euler = euler_from_quaternion([eef_pose.orientation.x, eef_pose.orientation.x, eef_pose.orientation.z, eef_pose.orientation.w], axes=) cv_image = bridge.imgmsg_to_cv2(camera_image, desired_encoding='passthrough') cv2.imwrite( '/home/icub/calib_data/image-{}.png'.format(poses_reached), cv_image) with open( '/home/icub/calib_data/pose_fPe_{}.yaml'.format( poses_reached), 'w') as f: s = ("rows: 6\n" "cols: 1\n" "data:\n" " - [{:.6f}]\n" " - [{:.6f}]\n" " - [{:.6f}]\n"