def _extract_twist_msg(twist_msg): pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z] rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z] quat = trans.quaternion_from_euler(*rot_euler, axes='sxyz') homo_mat = np.mat(trans.euler_matrix(*rot_euler)) homo_mat[:3,3] = np.mat([pos]).T return homo_mat, quat, rot_euler
def get_head_pose(self, ell_coords_rot, gripper_rot=0.): lat, lon, height = ell_coords_rot[0] roll, pitch, yaw = ell_coords_rot[1] pos, rot = PoseConv.to_pos_rot( self.ell_space.ellipsoidal_to_pose(lat, lon, height)) rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3] return pos, rot
def execute_cart_move(self, change_ep, abs_sel, velocity=0.001, num_samps=None, blocking=True): if not self.cmd_lock.acquire(False): return False cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) change_pos_ep, change_rot_ep = change_ep abs_cart_ep_sel, is_abs_rot = abs_sel pos_f = np.where(abs_cart_ep_sel, change_pos_ep, np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0]) if is_abs_rot: rot_mat_f = change_rot_ep else: rpy = change_rot_ep _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3] rot_mat_f = cur_rot * rot_mat traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps) retval = self._run_traj(traj, blocking=blocking) self.cmd_lock.release() return retval
def _make_generic(args): try: if type(args[0]) == str: frame_id = args[0] header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic( args[1:]) if homo_mat is None: return None, None, None, None if header is None: header = [0, rospy.Time.now(), ''] header[2] = frame_id return header, homo_mat, rot_quat, rot_euler if len(args) == 2: return PoseConv._make_generic(((args[0], args[1]), )) elif len(args) == 1: pose_type = PoseConv.get_type(*args) if pose_type is None: return None, None, None, None if pose_type == 'pose_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg( args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'pose_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg( args[0].pose) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'tf_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg( args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'tf_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg( args[0].transform) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'point_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg( args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'point_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg( args[0].point) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'twist_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg( args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'twist_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg( args[0].twist) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'homo_mat': return (None, np.mat(args[0]), trans.quaternion_from_matrix(args[0]).tolist(), trans.euler_from_matrix(args[0])) elif pose_type in [ 'pos_rot', 'pos_euler', 'pos_quat', 'pos_axis_angle' ]: pos_arg = np.mat(args[0][0]) if pos_arg.shape == (1, 3): # matrix is row, convert to column pos = pos_arg.T elif pos_arg.shape == (3, 1): pos = pos_arg if pose_type == 'pos_axis_angle': homo_mat = np.mat(np.eye(4)) homo_mat[:3, :3] = axis_angle_to_rot_mat( args[0][1][0], args[0][1][1]) quat = trans.quaternion_from_matrix(homo_mat) rot_euler = trans.euler_from_matrix(homo_mat) elif pose_type == 'pos_rot': # rotation matrix homo_mat = np.mat(np.eye(4)) homo_mat[:3, :3] = np.mat(args[0][1]) quat = trans.quaternion_from_matrix(homo_mat) rot_euler = trans.euler_from_matrix(homo_mat) else: rot_arg = np.mat(args[0][1]) if rot_arg.shape[1] == 1: rot_arg = rot_arg.T rot_list = rot_arg.tolist()[0] if pose_type == 'pos_euler': # Euler angles rotation homo_mat = np.mat(trans.euler_matrix(*rot_list)) quat = trans.quaternion_from_euler(*rot_list) rot_euler = rot_list elif pose_type == 'pos_quat': # quaternion rotation homo_mat = np.mat( trans.quaternion_matrix(rot_list)) quat = rot_list rot_euler = trans.euler_from_quaternion(quat) homo_mat[:3, 3] = pos return None, homo_mat, np.array(quat), rot_euler except: pass return None, None, None, None
def homo_mat_from_2d(x, y, rot): mat2d = np.mat(trans.euler_matrix(0, 0, rot)) mat2d[0,3] = x mat2d[1,3] = y return mat2d
def get_head_pose(self, ell_coords_rot, gripper_rot=0.): lat, lon, height = ell_coords_rot[0] roll, pitch, yaw = ell_coords_rot[1] pos, rot = PoseConv.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height)) rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3] return pos, rot
def _make_generic(args): try: if type(args[0]) == str: frame_id = args[0] header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic(args[1:]) if homo_mat is None: return None, None, None, None if header is None: header = [0, rospy.Time.now(), ''] header[2] = frame_id return header, homo_mat, rot_quat, rot_euler if len(args) == 2: return PoseConv._make_generic(((args[0], args[1]),)) elif len(args) == 1: pose_type = PoseConv.get_type(*args) if pose_type is None: return None, None, None, None if pose_type == 'pose_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'pose_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0].pose) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'tf_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'tf_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0].transform) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'point_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'point_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0].point) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'twist_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif pose_type == 'twist_stamped_msg': homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0].twist) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif pose_type == 'homo_mat': return (None, np.mat(args[0]), trans.quaternion_from_matrix(args[0]).tolist(), trans.euler_from_matrix(args[0])) elif pose_type in ['pos_rot', 'pos_euler', 'pos_quat']: pos_arg = np.mat(args[0][0]) rot_arg = np.mat(args[0][1]) if pos_arg.shape == (1, 3): # matrix is row, convert to column pos = pos_arg.T elif pos_arg.shape == (3, 1): pos = pos_arg if pose_type == 'pos_rot': # rotation matrix homo_mat = np.mat(np.eye(4)) homo_mat[:3,:3] = rot_arg quat = trans.quaternion_from_matrix(homo_mat) rot_euler = trans.euler_from_matrix(homo_mat) else: if rot_arg.shape[1] == 1: rot_arg = rot_arg.T rot_list = rot_arg.tolist()[0] if pose_type == 'pos_euler': # Euler angles rotation homo_mat = np.mat(trans.euler_matrix(*rot_list)) quat = trans.quaternion_from_euler(*rot_list) rot_euler = rot_list elif pose_type == 'pos_quat': # quaternion rotation homo_mat = np.mat(trans.quaternion_matrix(rot_list)) quat = rot_list rot_euler = trans.euler_from_quaternion(quat) homo_mat[:3, 3] = pos return None, homo_mat, np.array(quat), rot_euler except: pass return None, None, None, None