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()