Esempio n. 1
0
def test_line_intersect():
    assert line_intersect((0, 0), (0, 1), (0, 0), (1, 0))[:2] == (0, 0)
    assert line_intersect((0, 0), (0, 1), (0, 0), (0, 1))[2] == 0
    assert ray_segment_intersect(ray=((0, 0), 0),
                                 segment=((1, -1), (1, 1))) == (1, 0)
    assert ray_segment_intersect(ray=((0, 0), math.pi),
                                 segment=((1, -1), (1, 1))) is None
Esempio n. 2
0
    def get_current_maze_obs(self):
        # The observation would include both information about the robot itself as well as the sensors around its
        # environment
        robot_x, robot_y = self.wrapped_env.get_body_com("torso")[:2]
        ori = self.get_ori()
        ori = math.pi / 4

        structure = self.MAZE_STRUCTURE
        size_scaling = self.MAZE_SIZE_SCALING

        wall_readings = np.zeros(self._n_bins)
        goal_readings = np.zeros(self._n_bins)

        for ray_idx in range(self._n_bins):
            ray_ori = ori - self._sensor_span * 0.5 + 1.0 * (
                2 * ray_idx + 1) / (2 * self._n_bins) * self._sensor_span
            ray_segments = []
            for seg in self.segments:
                p = ray_segment_intersect(ray=((robot_x, robot_y), ray_ori),
                                          segment=seg["segment"])
                if p is not None:
                    ray_segments.append(
                        dict(
                            segment=seg["segment"],
                            type=seg["type"],
                            ray_ori=ray_ori,
                            distance=point_distance(p, (robot_x, robot_y)),
                        ))
            if len(ray_segments) > 0:
                first_seg = sorted(ray_segments,
                                   key=lambda x: x["distance"])[0]
                # print first_seg
                if first_seg["type"] == 1:
                    # Wall -> add to wall readings
                    if first_seg["distance"] <= self._sensor_range:
                        wall_readings[ray_idx] = (
                            self._sensor_range -
                            first_seg["distance"]) / self._sensor_range
                elif first_seg["type"] == 'g':
                    # Goal -> add to goal readings
                    # if first_seg["distance"] <= self._sensor_range:
                    #     goal_readings[ray_idx] = (self._sensor_range - first_seg["distance"]) / self._sensor_range
                    pass
                else:
                    assert False

        obs = np.concatenate([
            wall_readings,
            # goal_readings
        ])
        return obs
    def get_current_maze_obs(self):
        # The observation would include both information about the robot itself as well as the sensors around its
        # environment
        robot_x_mujoco, robot_y_mujoco = self.wrapped_env.get_body_com(
            "torso")[:2]  # mujoco coords
        # print("mujoco coord:", robot_x_mujoco, robot_y_mujoco)
        robot_x, robot_y = robot_x_mujoco + self._init_torso_x, robot_y_mujoco + self._init_torso_y  # index coords
        ori = self.get_ori()

        structure = self.MAZE_STRUCTURE
        size_scaling = self.MAZE_SIZE_SCALING

        segments = []
        # compute the distance of all segments

        # Get all line segments of the goal and the obstacles
        for i in range(len(structure)):
            for j in range(len(structure[0])):
                if structure[i][j] == 1 or structure[i][j] == 'g':
                    cx = j * size_scaling - self._init_torso_x
                    cy = i * size_scaling - self._init_torso_y
                    x1 = cx - 0.5 * size_scaling
                    x2 = cx + 0.5 * size_scaling
                    y1 = cy - 0.5 * size_scaling
                    y2 = cy + 0.5 * size_scaling
                    struct_segments = [
                        ((x1, y1), (x2, y1)),  # index coordinates
                        ((x2, y1), (x2, y2)),
                        ((x2, y2), (x1, y2)),
                        ((x1, y2), (x1, y1)),
                    ]
                    for seg in struct_segments:
                        segments.append(
                            dict(
                                segment=seg,
                                type=structure[i][j],
                            ))

        wall_readings = np.zeros(self._n_bins)
        goal_readings = np.zeros(self._n_bins)

        for ray_idx in range(self._n_bins):
            ray_ori = ori - self._sensor_span * 0.5 + 1.0 * (
                2 * ray_idx + 1) / (2 * self._n_bins) * self._sensor_span
            ray_segments = []
            for seg in segments:
                p = ray_segment_intersect(ray=((robot_x, robot_y), ray_ori),
                                          segment=seg["segment"])
                if p is not None:
                    ray_segments.append(
                        dict(
                            segment=seg["segment"],
                            type=seg["type"],
                            ray_ori=ray_ori,
                            distance=point_distance(p, (robot_x, robot_y)),
                        ))
            if len(ray_segments) > 0:
                first_seg = sorted(ray_segments,
                                   key=lambda x: x["distance"])[0]
                # print first_seg
                if first_seg["type"] == 1:
                    # Wall -> add to wall readings
                    if first_seg["distance"] <= self._sensor_range:
                        wall_readings[ray_idx] = (
                            self._sensor_range -
                            first_seg["distance"]) / self._sensor_range
                elif first_seg["type"] == 'g':
                    # Goal -> add to goal readings
                    if first_seg["distance"] <= self._sensor_range:
                        goal_readings[ray_idx] = (
                            self._sensor_range -
                            first_seg["distance"]) / self._sensor_range
                else:
                    assert False

        # print("wall readings", wall_readings)
        if any(goal_readings) == False:
            x, y = self.wrapped_env.model.data.qpos.flat[0:2]
            print("goal_readings", goal_readings)
            print("x,y", x, y)
            # break

        obs = np.concatenate([wall_readings, goal_readings])
        # return obs
        return obs
Esempio n. 4
0
    def get_current_obs(self):
        # The observation would include both information about the robot itself as well as the sensors around its
        # environment
        robot_x, robot_y = self.wrapped_env.get_body_com("torso")[:2]
        ori = self.get_ori()

        #print ori*180/math.pi

        structure = self.__class__.MAZE_STRUCTURE
        size_scaling = self.__class__.MAZE_SIZE_SCALING

        segments = []
        # compute the distance of all segments

        # Get all line segments of the goal and the obstacles
        for i in range(len(structure)):
            for j in range(len(structure[0])):
                if structure[i][j] == 1 or structure[i][j] == 'g':
                    cx = j * size_scaling - self._init_torso_x
                    cy = i * size_scaling - self._init_torso_y
                    x1 = cx - 0.5 * size_scaling
                    x2 = cx + 0.5 * size_scaling
                    y1 = cy - 0.5 * size_scaling
                    y2 = cy + 0.5 * size_scaling
                    struct_segments = [
                        ((x1, y1), (x2, y1)),
                        ((x2, y1), (x2, y2)),
                        ((x2, y2), (x1, y2)),
                        ((x1, y2), (x1, y1)),
                    ]
                    for seg in struct_segments:
                        segments.append(dict(
                            segment=seg,
                            type=structure[i][j],
                        ))

        wall_readings = np.zeros(self._n_bins)
        goal_readings = np.zeros(self._n_bins)

        for ray_idx in range(self._n_bins):
            ray_ori = ori - self._sensor_span * 0.5 + 1.0 * (2 * ray_idx + 1) / (2 * self._n_bins) * self._sensor_span
            ray_segments = []
            for seg in segments:
                p = ray_segment_intersect(ray=((robot_x, robot_y), ray_ori), segment=seg["segment"])
                if p is not None:
                    ray_segments.append(dict(
                        segment=seg["segment"],
                        type=seg["type"],
                        ray_ori=ray_ori,
                        distance=point_distance(p, (robot_x, robot_y)),
                    ))
            if len(ray_segments) > 0:
                first_seg = sorted(ray_segments, key=lambda x: x["distance"])[0]
                # print first_seg
                if first_seg["type"] == 1:
                    # Wall -> add to wall readings
                    if first_seg["distance"] <= self._sensor_range:
                        wall_readings[ray_idx] = (self._sensor_range - first_seg["distance"]) / self._sensor_range
                elif first_seg["type"] == 'g':
                    # Goal -> add to goal readings
                    if first_seg["distance"] <= self._sensor_range:
                        goal_readings[ray_idx] = (self._sensor_range - first_seg["distance"]) / self._sensor_range
                else:
                    assert False

        obs = np.concatenate([
            self.wrapped_env.get_current_obs(),
            wall_readings,
            goal_readings
        ])
        # print "wall readings:", wall_readings
        # print "goal readings:", goal_readings

        return obs
Esempio n. 5
0
def test_line_intersect():
    assert line_intersect((0, 0), (0, 1), (0, 0), (1, 0))[:2] == (0, 0)
    assert line_intersect((0, 0), (0, 1), (0, 0), (0, 1))[2] == 0
    assert ray_segment_intersect(ray=((0, 0), 0), segment=((1, -1), (1, 1))) == (1, 0)
    assert ray_segment_intersect(ray=((0, 0), math.pi), segment=((1, -1), (1, 1))) is None