def example_config():
"""
Generates an example config.
:return MapConfig: an example config
"""
traj = np.array([
[2., 2.],
[2., 4.],
[4., 4.],
[6., 4.],
[6., 8.],
])
obs = generate_zigzag_walls(trajectory=traj,
corridor_y_span=0.65,
corridor_x_span=0.975)
traj = refine_path(orient_path(traj), 0.05)
config = MapConfig(trajectory=traj,
obstacles=obs,
size=(10, 10),
resolution=0.03,
origin=(0, 0))
return config
def make_initial_state(path, costmap, robot, reward_provider, params):
""" Prepare the initial full state of the planning environment
:param path: the static path to follow
:param costmap: the static costmap containg all the obstacles
:param robot: robot - we will execute the motion based on its model
:param reward_provider: an instance of the reward computing class
:param params: parametriztion of the environment
:return State: the full initial state of the environment
"""
if params.refine_path:
path = refine_path(path, params.path_delta)
assert path.shape[1] == 3
# generate robot_state, poses,
initial_pose = path[0]
robot_state = robot.get_initial_state()
robot_state.set_pose(initial_pose)
initial_reward_provider_state = reward_provider.generate_initial_state(
path, params.reward_provider_params)
return State(
reward_provider_state=initial_reward_provider_state,
path=np.ascontiguousarray(
initial_reward_provider_state.current_path()),
original_path=np.copy(np.ascontiguousarray(path)),
costmap=costmap,
iter_timeout=params.iteration_timeout,
current_time=0.0,
current_iter=0,
robot_collided=False,
pose=initial_pose,
poses_queue=[],
robot_state=robot_state,
robot_state_queue=[],
control_queue=[],
)