def __init__(self, costmap, path, params):
"""
:param costmap CostMap2D: costmap denoting obstacles
:param path array(N, 3): oriented path, presented as way points
:param params EnvParams: parametrization of the environment
"""
# Stateful things
self._robot = TricycleRobot(dimensions=get_dimensions_example(params.robot_name))
reward_provider_example = get_reward_provider_example(params.reward_provider_name)
self._reward_provider = reward_provider_example(params=params.reward_provider_params)
# Properties, things without state
self.action_space = spaces.Box(
low=np.array([-self._robot.get_max_front_wheel_speed() / 2, -np.pi/2]),
high=np.array([self._robot.get_max_front_wheel_speed(), np.pi/2]),
dtype=np.float32)
self.reward_range = (0.0, 1.0)
self._gui = OpenCVGui()
self._params = params
# State
self._state = make_initial_state(path, costmap, self._robot, self._reward_provider, params)
self._initial_state = self._state.copy()
self.set_state(self._state)
def deserialize(cls, state):
""" Deserialize the object from the dict of basic types.
:param state dict: dict (serialized) representation of the object
:return TricycleRobot: the deserialized object
"""
ver = state.pop('version')
assert ver == cls.VERSION
state['state'] = TricycleRobotState.deserialize(state['state'])
state['dimensions'] = get_dimensions_example((state['dimensions']))
return cls(**state)
def _sample_mini_env_params(gen_params, rng):
"""
Sample mini env instance based on passed params and check if the start and end poses
are not colliding.
If you can't sample, raise an error.
:param gen_params RandomMiniEnvParams: params of the random mini env
:param rng np.random.RandomState: independent random state
:return MiniEnvParams: sampled params of the environment
"""
for _ in range(1000):
# We need to check for collisions of start and
try:
drawn_params = _sample_mini_env_params_no_final_check(
gen_params, rng)
except SpaceSeemsEmptyError:
continue
costmap, static_path = prepare_map_and_path(drawn_params)
robot = TricycleRobot(dimensions=get_dimensions_example(
gen_params.env_params.robot_name))
robot.set_front_wheel_angle(gen_params.env_params.initial_wheel_angle)
x, y, angle = static_path[0]
first_pose_collides = pose_collides(x, y, angle, robot, costmap)
x, y, angle = static_path[1]
second_pose_collides = pose_collides(x, y, angle, robot, costmap)
collides = first_pose_collides or second_pose_collides
# are beg and goal poses not immediately too close
cart_dist, ang_dist = pose_distances(static_path[0], static_path[1])
cart_near = cart_dist < gen_params.env_params.goal_spat_dist
ang_near = ang_dist < gen_params.env_params.goal_ang_dist
too_close = cart_near and ang_near
if not collides and not too_close:
return drawn_params
raise ValueError("Something went wrong, the sampling space looks empty.")
def create_standard_robot(robot_name, footprint_scale=1., **kwargs):
"""
Given a robot name (along with construction parameters common to all robots), create a new robot.
:param robot_name: A string robot name: One of the RobotNames enum
:param footprint_scale: Factor by which to scale the size of the footprint
:param kwargs: Angle of the front wheel (only used when the robot is a tricycle
:return: An IRobot object
"""
dimensions = get_dimensions_example(robot_name)
if robot_name in StandardRobotExamples.INDUSTRIAL_TRICYCLE_V1:
robot = TricycleRobot(dimensions=dimensions,
footprint_scale=footprint_scale,
**kwargs)
return robot
elif robot_name == StandardRobotExamples.INDUSTRIAL_DIFFDRIVE_V1:
return DiffDriveRobot(dimensions=dimensions,
footprint_scale=footprint_scale,
**kwargs)
else:
raise Exception('No robot named "{}" exists'.format(robot_name))