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