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