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 _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 _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 local_input_cb(self, msg): self.force_monitor.update_activity() if self.is_forced_retreat: return self.stop_moving() button_press = msg.data 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)) if button_press in ell_trans_params: self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press]) change_trans_ep = ell_trans_params[button_press] goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0], curr_ell_pos[1]+change_trans_ep[1], curr_ell_pos[2]+change_trans_ep[2]] ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat, goal_ell_pos, curr_ell_quat, velocity=ELL_LOCAL_VEL, min_jerk=True) elif button_press in ell_rot_params: self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press]) change_rot_ep = ell_rot_params[button_press] rot_quat = trans.quaternion_from_euler(*change_rot_ep) goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat) ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat, curr_ell_pos, goal_ell_quat, velocity=ELL_ROT_VEL, min_jerk=True) elif button_press == "reset_rotation": self.publish_feedback(Messages.ROT_RESET_START) ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat, curr_ell_pos,(0.,0.,0.,1.), velocity=ELL_ROT_VEL, min_jerk=True) else: rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command") cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path] cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in 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) self.force_monitor.update_activity()
def run_controller(self, button_press): start_pos, _ = self.cart_arm.get_ep() start_time = rospy.get_time() quat_gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0) if button_press in ell_trans_params: change_trans_ep = ell_trans_params[button_press] self.controller.execute_ell_move((change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0), quat_gripper_rot, ELL_LOCAL_VEL) elif button_press in ell_rot_params: change_rot_ep = ell_rot_params[button_press] self.controller.execute_ell_move(((0, 0, 0), change_rot_ep), ((0, 0, 0), 0), quat_gripper_rot, ELL_ROT_VEL) elif button_press == "reset_rotation": self.controller.execute_ell_move(((0, 0, 0), np.mat(np.eye(3))), ((0, 0, 0), 1), quat_gripper_rot, ELL_ROT_VEL) end_pos, _ = self.cart_arm.get_ep() end_time = rospy.get_time() dist = np.linalg.norm(end_pos - start_pos) time_diff = end_time - start_time print "%s, dist: %.3f, time_diff: %.2f" % (button_press, dist, time_diff) self.button_distances[button_press].append(dist) self.button_times[button_press].append(time_diff)
def run_controller(self, button_press): start_pos, _ = self.cart_arm.get_ep() start_time = rospy.get_time() quat_gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0) if button_press in cart_trans_params: ell_change_trans_ep = ell_trans_params[button_press] change_trans_ep = cart_trans_params[button_press] # get number of samples: ell_f, rot_mat_f = self._parse_ell_move((ell_change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0), quat_gripper_rot) traj = self._create_ell_trajectory(ell_f, rot_mat_f, orient_quat, velocity) num_samps = len(traj) # normalization occurs here: # TODO FIGURE THIS OUT? ell_dist = self.controller._get_ell_equiv_dist((ell_change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0), quat_gripper_rot, ELL_LOCAL_VEL) change_trans_ep = tuple(np.array(change_trans_ep)/np.sum(change_trans_ep) * ell_dist) self.controller.execute_cart_move((change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0), quat_gripper_rot, CART_TRANS_VEL, num_samps=num_samps) elif button_press in cart_rot_params: change_rot_ep = cart_rot_params[button_press] self.controller.execute_cart_move(((0, 0, 0), change_rot_ep), ((0, 0, 0), 0), quat_gripper_rot, CART_ROT_VEL) elif button_press == "reset_rotation": self.controller.execute_cart_move(((0, 0, 0), np.mat(np.eye(3))), ((0, 0, 0), 1), quat_gripper_rot, CART_ROT_VEL) end_pos, _ = self.cart_arm.get_ep() end_time = rospy.get_time() dist = np.linalg.norm(end_pos - start_pos) time_diff = end_time - start_time print "%s, dist: %.3f, time_diff: %.2f" % (button_press, dist, time_diff) self.button_distances[button_press].append(dist) self.button_times[button_press].append(time_diff)
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 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 enable_controller_cb(self, req): if req.enable: face_adls_modes = rospy.get_param('/face_adls_modes', None) params = face_adls_modes[req.mode] self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general self.trim_retreat = req.mode == "shaving" self.flip_gripper = self.face_side == 'r' and req.mode == "shaving" bounds = params['%s_bounds' % self.face_side] self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back #self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100)) success, e_params = self.register_ellipse(req.mode, self.face_side) if not success: self.publish_feedback(Messages.NO_PARAMS_LOADED) return EnableFaceControllerResponse(False) reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame) try: now = rospy.Time.now() reg_pose.header.stamp = now self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link', now, rospy.Duration(8.0)) ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e) self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E, e_params.is_oblate, e_params.height) global_poses_dir = rospy.get_param("~global_poses_dir", "") global_poses_file = params["%s_ell_poses_file" % self.face_side] global_poses_resolved = roslaunch.substitution_args.resolve_args( global_poses_dir + "/" + global_poses_file) self.global_poses = rosparam.load_file(global_poses_resolved)[0][0] self.global_move_poses_pub.publish(sorted(self.global_poses.keys())) #Check rotating gripper based on side of body if self.flip_gripper: self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0) print "Rotating gripper by PI around x-axis" else: self.gripper_rot = trans.quaternion_from_euler(0, 0, 0) print "NOT Rotating gripper around x-axis" # check if arm is near head # cart_pos, cart_quat = self.current_ee_pose(self.ee_frame) # ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat)) # equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos) # print ell_pos, equals # if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0: # arm_in_bounds = np.all(equals) # else: # ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi) # min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi) # arm_in_bounds = (equals[0] and # equals[2] and # (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1])) arm_in_bounds = True self.setup_force_monitor() success = True if not arm_in_bounds: self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD) success = False #Switch back to l_arm_controller if necessary if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False): print "Controller switch to l_arm_controller succeeded" self.publish_feedback(Messages.ENABLE_CONTROLLER) else: print "Controller switch to l_arm_controller failed" success = False self.controller_enabled_pub.publish(Bool(success)) req = EnableHapticMPCRequest() req.new_state = 'enabled' resp = self.enable_mpc_srv.call(req) else: self.stop_moving() self.controller_enabled_pub.publish(Bool(False)) rospy.loginfo(Messages.DISABLE_CONTROLLER) req = EnableHapticMPCRequest() req.new_state = 'disabled' self.enable_mpc_srv.call(req) success = False return EnableFaceControllerResponse(success)
def local_input_cb(self, msg): self.force_monitor.update_activity() if self.is_forced_retreat: return self.stop_moving() button_press = msg.data 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)) if button_press in ell_trans_params: self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press]) change_trans_ep = ell_trans_params[button_press] goal_ell_pos = [ curr_ell_pos[0] + change_trans_ep[0], curr_ell_pos[1] + change_trans_ep[1], curr_ell_pos[2] + change_trans_ep[2] ] ell_path = self.ellipsoid.create_ellipsoidal_path( curr_ell_pos, curr_ell_quat, goal_ell_pos, curr_ell_quat, velocity=ELL_LOCAL_VEL, min_jerk=True) elif button_press in ell_rot_params: self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press]) change_rot_ep = ell_rot_params[button_press] rot_quat = trans.quaternion_from_euler(*change_rot_ep) goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat) ell_path = self.ellipsoid.create_ellipsoidal_path( curr_ell_pos, curr_ell_quat, curr_ell_pos, goal_ell_quat, velocity=ELL_ROT_VEL, min_jerk=True) elif button_press == "reset_rotation": self.publish_feedback(Messages.ROT_RESET_START) ell_path = self.ellipsoid.create_ellipsoidal_path( curr_ell_pos, curr_ell_quat, curr_ell_pos, (0., 0., 0., 1.), velocity=ELL_ROT_VEL, min_jerk=True) else: rospy.logerr( "[face_adls_manager] Unknown local ellipsoidal command") cart_traj = [ self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path ] cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in 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) self.force_monitor.update_activity()
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 enable_controller_cb(self, req): if req.enable: face_adls_modes = rospy.get_param('/face_adls_modes', None) params = face_adls_modes[req.mode] self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general self.trim_retreat = req.mode == "shaving" self.flip_gripper = self.face_side == 'r' and req.mode == "shaving" bounds = params['%s_bounds' % self.face_side] self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height']) #TODO: Change Back #self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100)) success, e_params = self.register_ellipse(req.mode, self.face_side) if not success: self.publish_feedback(Messages.NO_PARAMS_LOADED) return EnableFaceControllerResponse(False) reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame) try: now = rospy.Time.now() reg_pose.header.stamp = now self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link', now, rospy.Duration(8.0)) ellipse_frame_base = self.tfl.transformPose( '/base_link', reg_pose) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn( "[face_adls_manager] TF Failure converting ellipse to base frame: %s" % e) self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E, e_params.is_oblate, e_params.height) global_poses_dir = rospy.get_param("~global_poses_dir", "") global_poses_file = params["%s_ell_poses_file" % self.face_side] global_poses_resolved = roslaunch.substitution_args.resolve_args( global_poses_dir + "/" + global_poses_file) self.global_poses = rosparam.load_file(global_poses_resolved)[0][0] self.global_move_poses_pub.publish(sorted( self.global_poses.keys())) #Check rotating gripper based on side of body if self.flip_gripper: self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0) print "Rotating gripper by PI around x-axis" else: self.gripper_rot = trans.quaternion_from_euler(0, 0, 0) print "NOT Rotating gripper around x-axis" # check if arm is near head # cart_pos, cart_quat = self.current_ee_pose(self.ee_frame) # ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat)) # equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos) # print ell_pos, equals # if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0: # arm_in_bounds = np.all(equals) # else: # ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi) # min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi) # arm_in_bounds = (equals[0] and # equals[2] and # (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1])) arm_in_bounds = True self.setup_force_monitor() success = True if not arm_in_bounds: self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD) success = False #Switch back to l_arm_controller if necessary if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False): print "Controller switch to l_arm_controller succeeded" self.publish_feedback(Messages.ENABLE_CONTROLLER) else: print "Controller switch to l_arm_controller failed" success = False self.controller_enabled_pub.publish(Bool(success)) req = EnableHapticMPCRequest() req.new_state = 'enabled' resp = self.enable_mpc_srv.call(req) else: self.stop_moving() self.controller_enabled_pub.publish(Bool(False)) rospy.loginfo(Messages.DISABLE_CONTROLLER) req = EnableHapticMPCRequest() req.new_state = 'disabled' self.enable_mpc_srv.call(req) success = False return EnableFaceControllerResponse(success)
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