def show_sensor_data(pylab, vehicle, robot_pose=None, col='r'):
if robot_pose is None:
robot_pose = SE3.from_yaml(vehicle['pose'])
for attached in vehicle['sensors']:
sensor_pose = from_yaml(attached['current_pose'])
sensor_t, sensor_theta = \
translation_angle_from_SE2(SE2_from_SE3(sensor_pose))
print('robot: %s' % SE2.friendly(SE2_from_SE3(robot_pose)))
print(' sens: %s' % SE2.friendly(SE2_from_SE3(sensor_pose)))
# sensor_theta = -sensor_theta
sensor = attached['sensor']
if sensor['type'] == 'Rangefinder':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
valid = np.array(observations['valid'])
# directions = directions[valid]
# readings = readings[valid]
x = []
y = []
rho_min = 0.05
for theta_i, rho_i in zip(directions, readings):
print('theta_i: %s' % theta_i)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
x.append(None)
y.append(None)
pylab.plot(x, y, color=col, markersize=0.5, zorder=2000)
elif sensor['type'] == 'Photoreceptors':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
luminance = np.array(observations['luminance'])
valid = np.array(observations['valid'])
readings[np.logical_not(valid)] = 0.6
rho_min = 0.5
for theta_i, rho_i, lum in zip(directions, readings, luminance):
x = []
y = []
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
pylab.plot(x, y, color=(lum, lum, lum),
markersize=0.5, zorder=2000)
else:
print('Unknown sensor type %r' % sensor['type'])
def test_poses_SE2(self):
dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
max_angular_velocity=1)
dt = 0.1
M = 1 # max
Z = 0 # zero
m = -1 # min
tests = [
# format ( (start_xy, start_theta), commands,
# (final_xy, final_theta))
(([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
(([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),
(([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),
(([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
(([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
(([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
(([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
(([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),
(([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
(([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))
# TODO: add some more tests with non-zero initial rotation
]
for initial, commands, final in tests:
start_pose = SE2_from_translation_angle(*initial)
final_pose = SE2_from_translation_angle(*final)
start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
commands = np.array(commands)
print('%s -> [%s] -> %s' %
(SE2.friendly(start_pose), commands,
SE2.friendly(final_pose)))
actual, dummy = dynamics.integrate(start_state, +commands, dt)
SE2.assert_close(actual, final_pose)
start2, dummy = dynamics.integrate(final_state, -commands, dt)
SE2.assert_close(start_pose, start2)
def test_poses_SE2(self):
from vehicles_dynamics import SE2Dynamics
dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
max_angular_velocity=1)
dt = 0.1
M = 1 # max
Z = 0 # zero
m = -1 # min
tests = [
# format ( (start_xy, start_theta), commands,
# (final_xy, final_theta))
(([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
(([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),
(([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),
(([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
(([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
(([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
(([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
(([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),
(([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
(([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))
# TODO: add some more tests with non-zero initial rotation
]
for initial, commands, final in tests:
start_pose = SE2_from_translation_angle(*initial)
final_pose = SE2_from_translation_angle(*final)
start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
commands = np.array(commands)
print(
'%s -> [%s] -> %s' %
(SE2.friendly(start_pose), commands, SE2.friendly(final_pose)))
actual, dummy = dynamics.integrate(start_state, +commands, dt)
SE2.assert_close(actual, final_pose)
start2, dummy = dynamics.integrate(final_state, -commands, dt)
SE2.assert_close(start_pose, start2)
def test_coords1():
vl = np.array([
0,
1,
0,
])
va = np.array([np.deg2rad(20), 0, 0])
vel = {
'F': vl,
'FL': vl + va,
'FR': vl - va,
'B': (-vl),
'BL': (-vl + va),
'BR': (-vl - va),
}
def make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
motions = dictmap(make_motion, vel)
print motions
for k, v in motions.items():
print(' - %s: %s -> %s' % (k, vel[k], SE2.friendly(v)))
names = sorted(vel.keys())
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
cD = construct_matrix_iterators((names, names), commuting)
aD = construct_matrix_iterators((names, names), anti)
D = construct_matrix_iterators((names, names), same)
r = Report('test_coords1')
r.table('D', data=D, cols=names, rows=names, fmt='%f')
r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
r.to_html('out/test_coords1/test_coords1.html')
def test_coords1():
vl = np.array([0, 1, 0, ])
va = np.array([np.deg2rad(20), 0, 0])
vel = {
'F': vl,
'FL': vl + va,
'FR': vl - va,
'B': (-vl),
'BL': (-vl + va),
'BR': (-vl - va),
}
def make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
motions = dictmap(make_motion, vel)
print motions
for k, v in motions.items():
print(' - %s: %s -> %s' % (k, vel[k], SE2.friendly(v)))
names = sorted(vel.keys())
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2),
SE2.multiply(q2, q1))
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
cD = construct_matrix_iterators((names, names), commuting)
aD = construct_matrix_iterators((names, names), anti)
D = construct_matrix_iterators((names, names), same)
r = Report('test_coords1')
r.table('D', data=D, cols=names, rows=names, fmt='%f')
r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
r.to_html('out/test_coords1/test_coords1.html')
def scenario_display(scenario, S, sim):
y0 = scenario['y0']
q0 = scenario['pose0']
y1 = scenario['y1']
q1 = scenario['pose1']
delta = scenario['delta']
print(SE2.friendly(SE2_from_SE3(q0)))
print(SE2.friendly(SE2_from_SE3(q1)))
print('increment: %s' % SE2.friendly(SE2_from_SE3(delta)))
with S.plot('data') as pylab:
pylab.plot(y0, label='y0')
pylab.plot(y1, label='y1')
pylab.axis((0, 180, -0.04, 1.04))
with S.plot('world') as pylab:
show_sensor_data(pylab, scenario['sim0']['vehicle'], col='r')
show_sensor_data(pylab, scenario['sim1']['vehicle'], col='g')
pylab.axis('equal')
# for pose in scenario['poses']:
# delta = pose_diff(q0, pose)
# print('increment: %s' % SE2.friendly(SE2_from_SE3(delta)))
#
S.array_as_image('M0', scenario['M0'])
S.array_as_image('M1', scenario['M1'])
pdfparams = dict(figsize=(4, 4), mime=MIME_PDF)
def display_all(S, name, sensels, mapp):
x = scenario['sim0']['vehicle']['sensors'][0]['sensor']
theta = x['directions']
theta = np.array(theta)
if len(theta) > len(sensels): # XXX: hack
theta = theta[::2]
theta_rad = theta
theta = np.rad2deg(theta)
sensels = sensels.copy()
sensels = sensels / sensels.max()
sec = S.section(name)
with sec.plot('readings', **pdfparams) as pylab:
pylab.plot(theta, sensels, 'b.')
pylab.plot([theta[0], theta[-1]], [0, 0], 'k--')
pylab.plot([theta[0], theta[-1]], [1, 1], 'k--')
pylab.axis((theta[0], theta[-1], -0.05, 1.01))
with sec.plot('minimap', **pdfparams) as pylab:
xs = np.cos(theta_rad) * sensels
ys = np.sin(theta_rad) * sensels
pylab.plot(xs, ys, 'k.')
L = 0.2
parms = dict(linewidth=2)
pylab.plot([0, L], [0, 0], 'r-', **parms)
pylab.plot([0, 0], [0, L], 'g-', **parms)
pylab.axis('equal')
R = 1.05
pylab.axis((-R, R, -R, R))
if mapp is not None:
sec.array_as_image('field', mapp, 'scale')
display_all(S, 'y0', y0, None)
display_all(S, 'm0y', map2sensels(scenario['M0']), scenario['M0'])
display_all(S, 'm1y', map2sensels(scenario['M1']), scenario['M1'])
with S.plot('poses') as pylab:
# for pose in scenario['poses']:
# # print pose
# draw_axes(pylab, SE2_from_SE3(pose))
#
for pose in scenario['poses']:
draw_axes(pylab, SE2_from_SE3(pose), 'k', 'k', size=0.5)
for pose in scenario['poses_important']:
draw_axes(pylab, SE2_from_SE3(pose), 'k', 'k', size=1)
draw_axes(pylab, SE2_from_SE3(q0), [0.3, 0, 0], [0, 0.3, 0], size=5)
draw_axes(pylab, SE2_from_SE3(q1), 'r', 'g', size=5)
# plot_sensor(pylab, sim.vehicle, q0, y0, 'g')
# plot_sensor(pylab, sim.vehicle, q1, y1, 'b')
pylab.axis('equal')
# print(scenario['commands'])
Se = S.section('exploration')
for name, M1est in scenario['exploration'].items():
Si = Se.section(name)
# Si.array_as_image('M1est', M1est)
Si.array_as_image('diff', M1est - scenario['M1'])
display_all(Si, 'M1est', map2sensels(M1est), M1est)
def run_simulation(task, vehicle, agent, log, dt, maxT):
world = task.get_world()
simulation = VehicleSimulation(world=world, vehicle=vehicle)
directions = simulation.vehicle.sensors[0].sensor.directions
vehicle_spec = VehicleSpec(directions=directions)
agent.init(vehicle_spec)
simulation.new_episode()
tmp_log = log + '.active'
ldir = os.path.dirname(log)
#last = os.path.join(ldir, 'last.yaml')
logger.info('Writing on log %r.' % log)
#logger.info(' (also accessible as %r)' % last)
if not os.path.exists(ldir):
os.makedirs(ldir)
#if os.path.exists(last):
# os.unlink(last)
logfile = open(tmp_log, 'w')
#assert not os.path.exists(last)
assert os.path.exists(tmp_log)
#logger.info('Link %s, %s' % (tmp_log, last))
#os.symlink(tmp_log, last)
logger.info('Simulation dt=%.3f max T: %.3f' % (dt, maxT))
while True:
simulation.compute_observations()
# TODO: perhaps this needs to be gerealized
luminance = simulation.vehicle.sensors[0].current_observations['luminance']
observations = VehicleObservations(time=simulation.timestamp,
dt=dt,
luminance=luminance)
agent.process_observations(observations)
commands = agent.choose_commands()
# TODO: check format
if logfile is not None:
y = simulation.to_yaml()
y['commands'] = commands.tolist()
logfile.write('---\n')
yaml.dump(y, logfile, Dumper=Dumper)
logger.info('t=%.3f pose: %s' %
(simulation.timestamp,
SE2.friendly(SE2_from_SE3(simulation.vehicle.get_pose()))))
if task.end_condition(simulation):
break
if simulation.timestamp > maxT:
# TODO: add why we finished
break
simulation.simulate(commands, dt)
logfile.close()
if os.path.exists(log):
os.unlink(log)
assert not os.path.exists(log)
os.rename(tmp_log, log)
assert not os.path.exists(tmp_log)
assert os.path.exists(log)
