def _normal_to_ellipse_prolate(self, lat, lon, height): pos = self.ellipsoidal_to_cart(lat, lon, height) df_du = self.partial_height(lat, lon, height) nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1./(1.+ny*ny/(nz*nz))) k = -ny*j/nz norm_rot = np.mat([[-nx, ny*k - nz*j, 0], [-ny, -nx*k, j], [-nz, nx*j, k]]) _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2]) #print norm_rot quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot) norm_rot_ortho = np.mat(trans.quaternion_matrix(norm_quat_ortho)[:3,:3]) if norm_rot_ortho[2, 2] > 0: flip_axis_ang = 0 else: flip_axis_ang = np.pi quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0) norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip) pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped) #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" % # (lat, lon, height) + # str(PoseConv.to_homo_mat(pose))) return pose
def _normal_to_ellipse_oblate(self, lat, lon, height): pos = self.ellipsoidal_to_cart(lat, lon, height) df_du = self.partial_height(-lat, lon, height) nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1./(1.+ny*ny/(nz*nz))) k = -ny*j/nz norm_rot = np.mat([[-nx, ny*k - nz*j, 0], [-ny, -nx*k, j], [-nz, nx*j, k]]) _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2]) #print norm_rot quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot) quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2) if lon >= np.pi: quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi) norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip) pose = PoseConv.to_pos_quat(pos, norm_quat_ortho) #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" % # (lat, lon, height) + # str(PoseConv.to_homo_mat(pose))) return pose
def _normal_to_ellipse_oblate(self, lat, lon, height): pos = self.ellipsoidal_to_cart(lat, lon, height) df_du = self.partial_height(-lat, lon, height) nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1. / (1. + ny * ny / (nz * nz))) k = -ny * j / nz norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j], [-nz, nx * j, k]]) _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2]) #print norm_rot quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot) quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi / 2, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2) if lon >= np.pi: quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi) norm_quat_ortho = trans.quaternion_multiply( norm_quat_ortho, quat_flip) pose = PoseConv.to_pos_quat(pos, norm_quat_ortho) #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" % # (lat, lon, height) + # str(PoseConv.to_homo_mat(pose))) return pose
def _normal_to_ellipse_prolate(self, lat, lon, height): pos = self.ellipsoidal_to_cart(lat, lon, height) df_du = self.partial_height(lat, lon, height) nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du) j = np.sqrt(1. / (1. + ny * ny / (nz * nz))) k = -ny * j / nz norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j], [-nz, nx * j, k]]) _, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot) rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2]) #print norm_rot quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0) norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot) norm_rot_ortho = np.mat( trans.quaternion_matrix(norm_quat_ortho)[:3, :3]) if norm_rot_ortho[2, 2] > 0: flip_axis_ang = 0 else: flip_axis_ang = np.pi quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0) norm_quat_ortho_flipped = trans.quaternion_multiply( norm_quat_ortho, quat_flip) pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped) #print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" % # (lat, lon, height) + # str(PoseConv.to_homo_mat(pose))) return pose
def pose_to_ellipsoidal(self, pose): pose_pos, pose_rot = PoseConv.to_pos_rot(pose) lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0, 0], pose_pos[1, 0], pose_pos[2, 0]) _, ell_rot = PoseConv.to_pos_rot( self.normal_to_ellipse(lat, lon, height)) _, quat_rot = PoseConv.to_pos_quat( np.mat([0] * 3).T, ell_rot.T * pose_rot) return [lat, lon, height], quat_rot
def add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None): vis_pub = rospy.Publisher(name, Marker, queue_size=10) marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time() marker.ns = "my_namespace" marker.id = 0 marker.type = type marker.action = Marker.ADD if type == Marker.LINE_LIST: for point in points: p = Point() p.x = point[0] p.y = point[1] p.z = point[2] marker.points.append(p) else: r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1]) rot = pose[0:3,0:3] * r pose2 = numpy.matrix(numpy.identity(4)) pose2[0:3,0:3] = rot pose2[0:3,3] = pose[0:3,3] quat_pose = PoseConv.to_pos_quat(pose2) marker.pose.position.x = quat_pose[0][0] marker.pose.position.y = quat_pose[0][1] marker.pose.position.z = quat_pose[0][2] marker.pose.orientation.x = quat_pose[1][0] marker.pose.orientation.y = quat_pose[1][1] marker.pose.orientation.z = quat_pose[1][2] marker.pose.orientation.w = quat_pose[1][3] marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.a = .5 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] vis_pub.publish(marker)
def global_input_cb(self, msg): self.force_monitor.update_activity() if self.is_forced_retreat: return rospy.loginfo("[face_adls_manager] Performing global move.") if type(msg) == String: self.publish_feedback(Messages.GLOBAL_START %msg.data) goal_ell_pose = self.global_poses[msg.data] elif type(msg) == PoseStamped: try: self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0)) goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg) goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose) flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0) goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat) goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped)) self.publish_feedback('Moving around ellipse to clicked position).') goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e) return curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame') curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat)) retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT] retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.]) approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT] approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1]) final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST] final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1]) cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat)) cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose) cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat)) cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose) cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat)) cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose) retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat, retreat_ell_pos, retreat_ell_quat, velocity=0.01, min_jerk=True) transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat, approach_ell_pos, approach_ell_quat, velocity=0.01, min_jerk=True) approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat, final_ell_pos, final_ell_quat, velocity=0.01, min_jerk=True) full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj] cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj] head = Header() head.frame_id = '/ellipse_frame' head.stamp = rospy.Time.now() pose_array = PoseArray(head, cart_traj_msg) self.pose_traj_pub.publish(pose_array) ps = PoseStamped() ps.header = head ps.pose = cart_traj_msg[0] self.force_monitor.update_activity()
def pose_to_ellipsoidal(self, pose): pose_pos, pose_rot = PoseConv.to_pos_rot(pose) lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0]) _, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height)) _, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot) return [lat, lon, height], quat_rot
def ellipsoidal_to_pose(self, pose): ell_pos, ell_quat = PoseConv.to_pos_quat(pose) if not self.is_oblate: return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat) else: return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)
def global_input_cb(self, msg): self.force_monitor.update_activity() if self.is_forced_retreat: return rospy.loginfo("[face_adls_manager] Performing global move.") if type(msg) == String: self.publish_feedback(Messages.GLOBAL_START % msg.data) goal_ell_pose = self.global_poses[msg.data] elif type(msg) == PoseStamped: try: self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0)) goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg) goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat( goal_cart_pose) flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0) goal_cart_quat_flipped = trans.quaternion_multiply( goal_cart_quat, flip_quat) goal_cart_pose = PoseConv.to_pose_stamped_msg( '/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped)) self.publish_feedback( 'Moving around ellipse to clicked position).') goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal( goal_cart_pose) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn( "[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" % e) return curr_cart_pos, curr_cart_quat = self.current_ee_pose( self.ee_frame, '/ellipse_frame') curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal( (curr_cart_pos, curr_cart_quat)) retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT] retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1., 0., 0., 0.]) approach_ell_pos = [ goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT ] approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1]) final_ell_pos = [ goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST ] final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1]) cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose( (retreat_ell_pos, retreat_ell_quat)) cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame', cart_ret_pose) cart_app_pose = self.ellipsoid.ellipsoidal_to_pose( (approach_ell_pos, approach_ell_quat)) cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame', cart_app_pose) cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose( (final_ell_pos, final_ell_quat)) cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame', cart_goal_pose) retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path( curr_ell_pos, curr_ell_quat, retreat_ell_pos, retreat_ell_quat, velocity=0.01, min_jerk=True) transition_ell_traj = self.ellipsoid.create_ellipsoidal_path( retreat_ell_pos, retreat_ell_quat, approach_ell_pos, approach_ell_quat, velocity=0.01, min_jerk=True) approach_ell_traj = self.ellipsoid.create_ellipsoidal_path( approach_ell_pos, approach_ell_quat, final_ell_pos, final_ell_quat, velocity=0.01, min_jerk=True) full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj full_cart_traj = [ self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj ] cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj] head = Header() head.frame_id = '/ellipse_frame' head.stamp = rospy.Time.now() pose_array = PoseArray(head, cart_traj_msg) self.pose_traj_pub.publish(pose_array) ps = PoseStamped() ps.header = head ps.pose = cart_traj_msg[0] self.force_monitor.update_activity()
#!/usr/bin/python import numpy as np import roslib roslib.load_manifest("hrl_geom") import rospy from hrl_geom.pose_converter import PoseConv if __name__ == "__main__": rospy.init_node("test_poseconv") homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2]) pose_msg = PoseConv.to_pose_msg(homo_mat) pos, quat = PoseConv.to_pos_quat(pose_msg) pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat) pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat]) tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg) p, R = PoseConv.to_pos_rot("/new_link", tf_stamped) for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", "pose_stamped_msg2", "tf_stamped", "p", "R"]: print "%s: \n" % name, locals()[name]
def ellipsoidal_to_pose(self, pose): ell_pos, ell_quat = PoseConv.to_pos_quat(pose) if not self.is_oblate: return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat) else: return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)