def sample_tasks(self, low, high, num_tasks): # seeds = np.random.randint(low=low, high=high, size=num_tasks) seeds = [5, 44, 122, 134, 405, 587, 1401, 1408, 1693, 1796] #validation_seeds = [2262, 2302, 4151, 2480, 2628] tasks = [] env_param = EnvParams( iteration_timeout=300, goal_ang_dist=np.pi / 8, goal_spat_dist=1.0, # was 0.2 robot_name=StandardRobotExamples.INDUSTRIAL_TRICYCLE_V1) for s in seeds: env = EgocentricCostmap( self.env(params=env_param, turn_off_obstacles=False, draw_new_turn_on_reset=False, seed=s)) env = bc_gym_wrapper(env, normalize=True) tasks.append(env) return tasks
def choose_env(test=False, normalization=True): seeds = [5, 44, 122, 134, 405, 587, 1401, 1408, 1693, 1796] validation_seeds = [2262, 2302, 4151, 2480, 2628] if test: rand_index = np.random.randint(low=0, high=len(validation_seeds), size=1).item() seed = validation_seeds[rand_index] else: rand_index = np.random.randint(low=0, high=len(seeds), size=1).item() seed = seeds[rand_index] max_steps = 300 env = RandomMiniEnv env_param = EnvParams( iteration_timeout=max_steps, goal_ang_dist=np.pi / 8, goal_spat_dist=1, robot_name=StandardRobotExamples.INDUSTRIAL_TRICYCLE_V1) env = EgocentricCostmap( env(params=env_param, turn_off_obstacles=False, draw_new_turn_on_reset=False, seed=seed)) env = bc_gym_wrapper(env, normalize=normalization) return env, seed
def __init__(self, params=None, draw_new_turn_on_reset=True, seed=None, rng=None): """ Initialize Random Aisle Turn Planning Environment :param params EnvParams: environment parameters that can be used to customize the benchmark. These are parameters of the base PlanEnv, and they are passed down there. :param draw_new_turn_on_reset bool: should we draw a new turn on each reset, or just keep redoing the first one. :param seed int: random seed :param rng np.RandomState: random number generator """ if rng is None: self._rng = np.random.RandomState() else: self._rng = rng self.seed(seed) self._draw_new_turn_on_reset = draw_new_turn_on_reset turn_params = self._draw_random_turn_params() if params is None: params = EnvParams() self._env_params = params self.config = AisleTurnEnvParams(turn_params=turn_params, env_params=self._env_params) self._env = AisleTurnEnv(self.config) self.action_space = self._env.action_space
def __init__(self, params=None, draw_new_turn_on_reset=True, seed=None, rng=None): """ Initialize random mini environment. :param params RandomMiniEnvParams: parametrization of this environment. :param draw_new_turn_on_reset bool: should we initialize a new random environment on each reset :param seed int: the random seed :param rng np.random.RandomState: independent random state """ if params is None: self._params = RandomMiniEnvParams(env_params=EnvParams( goal_ang_dist=np.pi / 8., goal_spat_dist=0.2, )) else: self._params = params if rng is None: self._rng = np.random.RandomState(seed=0) else: self._rng = rng self.seed(seed) self._draw_new_turn_on_reset = draw_new_turn_on_reset mini_params = _sample_mini_env_params(self._params, self._rng) self._env = MiniEnv(mini_params) self.action_space = self._env.action_space
def deserialize(cls, state): ver = state.pop('version') assert ver == cls.VERSION init_costmap = CostMap2D.from_state(state['costmap']) init_path = state['path'] params = EnvParams.deserialize(state['params']) state = State.deserialize(state['state']) instance = cls(init_costmap, init_path, params) instance.set_state(state) return instance
def test_play_real_corridor_env(): map_index = 4 _, path, test_maps = get_random_maps_squeeze_between_obstacle_in_corridor_on_path() env = PlanEnv( costmap=test_maps[map_index], path=path, params=EnvParams() ) play = KeyCapturePlay(env) play._display = MagicMock(return_value=ord('w')) play.pre_main_loop() while not play.done(): play.before_env_step() play.env_step() play.post_env_step()
import numpy as np import time from bc_gym_planning_env.envs.base.env import PlanEnv from bc_gym_planning_env.envs.base.params import EnvParams from bc_gym_planning_env.envs.base.maps import example_config, generate_trajectory_and_map_from_config if __name__ == '__main__': map_config = example_config() path, costmap = generate_trajectory_and_map_from_config(map_config) env_params = EnvParams( iteration_timeout=1200, pose_delay=1, control_delay=0, state_delay=1, goal_spat_dist=1.0, goal_ang_dist=np.pi / 2, dt=0.05, # 20 Hz path_limiter_max_dist=5.0, ) env = PlanEnv(costmap=costmap, path=path, params=env_params) env.reset() env.render() t = time.time() done = False while not done:
def run_frontiers_in_gym(map_filename, params_filename, start_state, sensor_range, map_resolution, render=True, render_interval=10, max_exploration_iterations=None): """ Interface for running frontier exploration in the bc gym environment that is initialized via map_filename.. etc :param map_filename str: path of the map to load into the grid world environment, needs to be a uint8 png with values 127 for unexplored, 255 for free, 0 for occupied. :param params_filename str: path of the params file for setting up the frontier agent etc. See bc_exploration/params/ for examples :param start_state array(3)[float]: starting state of the robot in the map (in meters) [x, y, theta], if None the starting state is random :param sensor_range float: range of the sensor (LiDAR) in meters :param map_resolution float: resolution of the map desired :param render bool: whether or not to visualize :param render_interval int: visualize every render_interval iterations :param max_exploration_iterations int: number of exploration cycles to run before exiting :return Costmap: occupancy_map, final map from exploration, percentage explored, time taken to explore """ # some parameters frontier_agent = create_frontier_agent_from_params(params_filename) footprint = frontier_agent.get_footprint() # setup costmap map = exp_CostMap(data=cv2.imread(map_filename), resolution=map_resolution, origin=np.array([0., 0.])) map_data = to_brain_costmap(map).get_data() costmap = gym_CostMap(data=map_data[:, :, 0], resolution=map_resolution, origin=np.array([0., 0.])) padding = 0. map_shape = np.array(map_data.shape[:2]) + int( 2. * padding // map_resolution) exp_initial_map = exp_CostMap(data=exp_CostMap.UNEXPLORED * np.ones(map_shape, dtype=np.uint8), resolution=map_resolution, origin=[-padding - 0., -padding - 0.]) footprint_coords = footprint.get_ego_points( start_state[2], map_resolution) + start_state[:2] footprint_coords = xy_to_rc(footprint_coords, exp_initial_map).astype(np.int) footprint_coords = footprint_coords[which_coords_in_bounds( footprint_coords, exp_initial_map.get_shape())] exp_initial_map.data[footprint_coords[:, 0], footprint_coords[:, 1]] = exp_CostMap.FREE gym_initial_map = to_brain_costmap(exp_initial_map) # pick a sensor sensor = VirtualLidar(range_max=sensor_range, range_angular=250 * np.pi / 180, costmap=costmap, resolution_angular=1.0 * np.pi / 180) # define log-odds mapper mapper = LogOddsMapper(initial_map=exp_initial_map, sensor_range=sensor.get_range_max(), measurement_certainty=0.8, max_log_odd=8, min_log_odd=-8, threshold_occupied=.5, threshold_free=.5) # define planning environment parameters env_params = EnvParams( iteration_timeout=1200, pose_delay=0, control_delay=0, state_delay=0, goal_spat_dist=1.0, goal_ang_dist=np.pi / 2, dt=0.05, # 20 Hz path_limiter_max_dist=5.0, sensor=sensor) env = PlanEnv(costmap=gym_initial_map, path=start_state[None, :], params=env_params) obs = env.reset() if render: env.render() pose = obs.pose scan_angles, scan_ranges = obs.get_lidar_scan() occupancy_map = mapper.update(state=pose, scan_angles=scan_angles, scan_ranges=scan_ranges) iteration = 0 is_last_plan = False was_successful = True j = 0 while True: if max_exploration_iterations is not None and iteration > max_exploration_iterations: was_successful = False is_last_plan = True # using the current map, make an action plan path = frontier_agent.plan(state=pose, occupancy_map=occupancy_map, is_last_plan=is_last_plan) # if we get empty lists for policy/path, that means that the agent was # not able to find a path/frontier to plan to. if not path.shape[0]: print( "No more frontiers! Either the map is 100% explored, or bad start state, or there is a bug!" ) break # if we have a policy, follow it until the end. update the map sparsely (speeds it up) env = PlanEnv(costmap=to_brain_costmap(occupancy_map), path=path, params=env_params) path = list(path) done = False while len(path) != 0 or not done: desired_pose = path.pop(0) if footprint.check_for_collision(desired_pose, occupancy_map, unexplored_is_occupied=True): footprint_coords = footprint.get_ego_points( desired_pose[2], map_resolution) + desired_pose[:2] footprint_coords = xy_to_rc(footprint_coords, occupancy_map).astype(np.int) footprint_coords = footprint_coords[which_coords_in_bounds( footprint_coords, occupancy_map.get_shape())] occupancy_map.data[footprint_coords[:, 0], footprint_coords[:, 1]] = exp_CostMap.FREE obs, _, done, _ = env.simple_step(desired_pose) pose = obs.pose scan_angles, scan_ranges = obs.get_lidar_scan() occupancy_map = mapper.update(state=pose, scan_angles=scan_angles, scan_ranges=scan_ranges) # put the current laserscan on the map before planning occupied_coords, _ = sensor.get_scan_points(pose) occupied_coords = xy_to_rc(occupied_coords, occupancy_map).astype(np.int) occupied_coords = occupied_coords[which_coords_in_bounds( occupied_coords, occupancy_map.get_shape())] occupancy_map.data[occupied_coords[:, 0], occupied_coords[:, 1]] = exp_CostMap.OCCUPIED if render and j % render_interval == 0: state = env.get_state() state.costmap = to_brain_costmap(occupancy_map) env.set_state(state) env.render() j += 1 if is_last_plan: break iteration += 1 if render: cv2.waitKey(0) return occupancy_map, iteration, was_successful
:return the_class: the deserialized object """ with open('env.pkl', 'wb') as f: data = thing.serialize() pickle.dump(data, f, protocol=pickle.HIGHEST_PROTOCOL) with open('env.pkl', 'rb') as f: serialized = pickle.load(f) return the_class.deserialize(serialized) if __name__ == '__main__': path, costmap = generate_trajectory_and_map_from_config(example_config()) env = PlanEnv(costmap=costmap, path=path, params=EnvParams()) env.reset() t = time.time() env_two = identity_serialize_deserialize(env, PlanEnv) actions = [env.action_space.sample() for _ in range(100)] for action in actions: pre_step_state_1 = env.get_state() pre_step_state_2 = env_two.get_state() assert pre_step_state_1 == pre_step_state_2 obs1, r1, done1, _ = env.step(action)
""" Play the planning env as if it was a computer game""" from __future__ import print_function from __future__ import absolute_import from __future__ import division from bc_gym_planning_env.utilities.gui import KeyCapturePlay from bc_gym_planning_env.envs.base.env import PlanEnv from bc_gym_planning_env.envs.rw_corridors.tdwa_test_environments import\ get_random_maps_squeeze_between_obstacle_in_corridor_on_path from bc_gym_planning_env.envs.base.params import EnvParams if __name__ == '__main__': map_index = 4 _, path, test_maps = get_random_maps_squeeze_between_obstacle_in_corridor_on_path() env = PlanEnv( costmap=test_maps[map_index], path=path, params=EnvParams() ) play = KeyCapturePlay(env) play.pre_main_loop() while not play.done(): play.before_env_step() play.env_step() play.post_env_step()