class Simulator: ORIGIN = Point(0, 0) def __init__(self): self.robot = Robot(Simulator.ORIGIN, WindRose.NORTH_INDEX) self.grid = Grid() def run(self, steps): step = 0 if steps < 0: raise Exception('number of steps must be positive(%d)' % steps) while step < steps: print('Step %d' % step) original_cell = self.robot.position cell = self.grid.add_cell(original_cell) if cell.color == Color.WHITE: self.robot.clockwise_rotate() elif cell.color == Color.BLACK: self.robot.anti_clockwise_rotate() cell.flip() self.robot.move() step = step + 1 def export_grid(self, filename): print('export_grid') print(self.grid.x_range) print(self.grid.y_range) for cell in self.grid.cells: print('cell: %s - color:%s' % (cell, self.grid.cells[cell].color)) x_min = self.grid.x_range[0] - 2 x_max = self.grid.x_range[1] + 2 y_min = self.grid.y_range[0] - 2 y_max = self.grid.y_range[1] + 2 with open(filename, "w") as simulation_file: simulation_file.write('x range: [%d,%d]\n' % (x_min, x_max)) simulation_file.write('y range: [%d,%d]\n' % (y_min, y_max)) simulation_file.write('\n') y = y_max while y >= y_min: simulation_file.write('|') x = x_min while x <= x_max: cell = self.grid.cells.get(Point(x, y)) if cell is None or cell.color == Color.WHITE: simulation_file.write('W|') elif cell.color == Color.BLACK: simulation_file.write('B|') x = x + 1 y = y - 1 simulation_file.write('\n')
def test_move(self, mock_calc_x_y_vec, mock_is_within_boundary): mock_is_within_boundary.return_value = True mock_calc_x_y_vec.return_value = (0, 1) test_robot = Robot() test_robot.place(1, 1, NORTH) test_robot.move() result = test_robot.report() self.assertEqual((1, 2, NORTH), result)
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance): # check if data has been made complete = False while not complete: data = [] # make robot and landmarks r = Robot(world_size, measurement_range, motion_noise, measurement_noise) r.make_landmarks(num_landmarks) seen = [False for row in range(num_landmarks)] # guess an initial motion orientation = random.random() * 2.0 * pi dx = cos(orientation) * distance dy = sin(orientation) * distance for k in range(N - 1): # collect sensor measurements in a list, Z Z = r.sense() # check off all landmarks that were observed for i in range(len(Z)): seen[Z[i][0]] = True # move while not r.move(dx, dy): # if we'd be leaving the robot world, pick instead a new direction orientation = random.random() * 2.0 * pi dx = cos(orientation) * distance dy = sin(orientation) * distance # collect/memorize all sensor and motion data data.append([Z, [dx, dy]]) # we are done when all landmarks were observed; otherwise re-run complete = (sum(seen) == num_landmarks) print(' ') print('Landmarks: ', r.landmarks) print(r) return data
def move(self, current, input, delta): """ Parameters: ---------- current: np.array(x, y, theta) current pose input: np.array(v, omega) input vector of linear velocity and angular velocity delta: float time delta of this tick Notes: ---------- calculate actual pose with random noise """ moved = Robot.move(current, input, delta) self.actual = np.array([ np.random.normal(moved[0], Agent.actual_xy_sd), np.random.normal(moved[1], Agent.actual_xy_sd), np.random.normal(moved[2], Agent.actual_theta_sd) ])
def test_no_initial_placement(self): test_robot = Robot() test_robot.move() result = test_robot.report() self.assertEqual((None, None, None), result)
def get_input(cls, agent, current, destination, current_input, delta): """ Parameters: ---------- agent: src.agent.Agent agent of robot current: np.array(x, y, theta) current pose destination: np.array(x, y, theta) destination pose current_input: np.array(v, omega) current input vector delta: float time delta of this tick Returns: ---------- np.array(v, omega) next input vector of linear velocity and angular velocity """ max_accelarations = agent.get_max_accelarations(current) linear_velocities = agent.get_linear_velocities(current) angular_velocities = agent.get_angular_velocities(current) v_range, omega_range = cls._get_window(max_accelarations, linear_velocities, angular_velocities, current_input, delta) input_list = np.array([[]]).reshape(0, 2) heading_list = np.array([]) velocity_list = np.array([]) distance_list = np.array([]) theta_list = np.array([]) for v, omega in itertools.product(v_range, omega_range): input = np.array((v, omega)) next = Robot.move(current, input, delta) input_list = np.append(input_list, [input], axis=0) heading_list = np.append(heading_list, cls._eval_heading(next, destination)) velocity_list = np.append(velocity_list, cls._eval_velocity(input)) distance_list = np.append(distance_list, cls._eval_distance(next, destination)) theta_list = np.append(theta_list, cls._eval_theta(next, destination)) def _eval(a): if np.linalg.norm(current[:2] - destination[:2] ) < DWAwoObstacle.DISTANCE_THRESHOLD: error_angle_gain = DWAwoObstacle.NEAR_ERROR_ANGLE_GAIN velocity_gain = DWAwoObstacle.NEAR_VELOCITY_GAIN distance_gain = DWAwoObstacle.NEAR_DISTANCE_GAIN theta_gain = DWAwoObstacle.NEAR_THETA_GAIN else: error_angle_gain = DWAwoObstacle.FAR_ERROR_ANGLE_GAIN velocity_gain = DWAwoObstacle.FAR_VELOCITY_GAIN distance_gain = DWAwoObstacle.FAR_DISTANCE_GAIN theta_gain = DWAwoObstacle.FAR_THETA_GAIN return error_angle_gain * a[0] + velocity_gain * a[ 1] + distance_gain * a[2] + theta_gain * a[3] candidate_list = np.apply_along_axis( _eval, 0, np.array([ utils.normalize_min_max(heading_list), utils.normalize_min_max(velocity_list), utils.normalize_min_max(distance_list), utils.normalize_min_max(theta_list), ])) return input_list[np.argmin(candidate_list)]
def test__move__to_an_invalid_orientation__it_should_raise_invalid_movement_error( self): target = Robot([0, 0], 'W', [5, 5]) with pytest.raises(InvalidMovementError): target.move()