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