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