def test_se3_se2():
for pose in SE2.interesting_points():
pose3 = SE3_from_SE2(pose)
vel3 = SE3.algebra_from_group(pose3)
vel2 = se2_from_se3(vel3)
pose2 = SE2.group_from_algebra(vel2)
assert_allclose(pose2, pose, atol=1e-8)
def update(self):
pose = self.input.pose
if self.state.pose0 is None:
self.state.pose0 = pose
rel_pose = SE2.multiply(SE2.inverse(self.state.pose0), pose)
self.output.rel_pose = rel_pose
def update(self):
pose = self.input.pose
if self.state.pose0 is None:
self.state.pose0 = pose
rel_pose = SE2.multiply(SE2.inverse(self.state.pose0), pose)
self.output.rel_pose = rel_pose
def graph_fix_node_test():
G = random_connected_pose_network(10)
target = random_SE2()
assert_exact(G)
graph_fix_node(G, 0, target)
SE2.assert_close(G.node[0]['pose'], target)
assert_exact(G)
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 velocity_from_poses(t1: float, q1: SE2v, t2: float, q2: SE2v) -> se2v:
delta = t2 - t1
if not delta > 0:
raise ValueError('invalid sequence')
x = SE2.multiply(SE2.inverse(q1), q2)
xt = SE2.algebra_from_group(x)
v = xt / delta
return v
def graph_fix_node_test():
G = random_connected_pose_network(10)
target = random_SE2()
assert_exact(G)
graph_fix_node(G, 0, target)
SE2.assert_close(G.node[0]['pose'], target)
assert_exact(G)
def servo_stats_summary(data_central, id_agent, id_robot, id_episode): # @UnusedVariable
from geometry import (SE2, SE2_from_SE3, translation_from_SE2,
angle_from_SE2, SE3)
log_index = data_central.get_log_index()
errors = []
timestamps = []
poses = []
dist_xy = []
dist_th = []
for observations in \
log_index.read_robot_episode(id_robot, id_episode,
read_extra=True):
extra = observations['extra'].item()
servoing = extra.get('servoing', None)
if servoing is None:
logger.error('Warning, no "servoing" in episode %r.' %
id_episode)
break
obs0 = np.array(servoing['obs0'])
pose0 = SE2_from_SE3(SE3.from_yaml(servoing['pose0']))
pose1 = SE2_from_SE3(SE3.from_yaml(servoing['pose1']))
poseK = SE2_from_SE3(SE3.from_yaml(servoing['poseK']))
pose1 = SE2.multiply(SE2.inverse(pose0), pose1)
poseK = SE2.multiply(SE2.inverse(pose0), poseK)
poses.append(poseK)
dist_xy.append(np.linalg.norm(translation_from_SE2(poseK)))
dist_th.append(np.abs(angle_from_SE2(poseK)))
# obs1 = np.array(servoing['obs1'])
obsK = np.array(servoing['obsK'])
err_L2 = np.linalg.norm(obs0 - obsK)
errors.append(err_L2)
timestamps.append(observations['timestamp'])
# last['time_from_episode_start'] = observations['time_from_episode_start']
initial_distance = np.linalg.norm(translation_from_SE2(pose1))
summary = {}
summary['pose0'] = pose0
summary['pose1'] = pose1
summary['poses'] = poses
summary['errors'] = errors
summary['timestamps'] = timestamps
summary['initial_translation'] = translation_from_SE2(pose1)
summary['initial_distance'] = initial_distance
summary['initial_rotation'] = angle_from_SE2(pose1)
summary['dist_xy'] = dist_xy
summary['dist_th'] = dist_th
return summary
def velocity_from_poses(t1: float, q1: SE2value, t2: float,
q2: SE2value) -> SE2value:
delta = t2 - t1
if not delta > 0:
msg = f"invalid delta {delta}"
raise ValueError(msg)
x = SE2.multiply(SE2.inverse(q1), q2)
xt = SE2.algebra_from_group(x)
v = xt / delta
return v
def graph_fix_node(G, x, target_pose):
assert G.has_node(x)
q = G.node[x]['pose']
diff = np.dot(target_pose, np.linalg.inv(q))
for n in G.nodes():
q = G.node[n]['pose']
q2 = np.dot(diff, q)
G.node[n]['pose'] = q2
SE2.assert_close(G.node[x]['pose'], target_pose)
def graph_fix_node(G, x, target_pose):
assert G.has_node(x)
q = G.node[x]['pose']
diff = np.dot(target_pose, np.linalg.inv(q))
for n in G.nodes():
q = G.node[n]['pose']
q2 = np.dot(diff, q)
G.node[n]['pose'] = q2
SE2.assert_close(G.node[x]['pose'], target_pose)
def simulate(self, dt, vehicle):
vehicle_pose = SE2_from_SE3(vehicle.get_pose())
target_pose = self.get_target_pose()
relative_pose = SE2.multiply(SE2.inverse(target_pose), vehicle_pose)
t, theta = translation_angle_from_SE2(relative_pose) #@UnusedVariable
# position on field of view
phi = np.arctan2(t[1], t[0])
diff = normalize_pi_scalar(self.target_stabilize_phi - phi)
# torque command
command = -np.sign(diff)
commands = [command]
self.target_state = self.target_dynamics.integrate(self.target_state,
commands, dt)
self.update_primitives()
return [self.target]
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def comparison_test_2():
''' Compares between se2_from_SE2 and se2_from_SE2_slow. '''
for pose in SE2.interesting_points():
se2a = se2_from_SE2(pose)
se2b = se2_from_SE2_slow(pose)
#printm('pose', pose, 'se2a', se2a, 'se2b', se2b)
assert_allclose(se2a, se2b, atol=1e-8)
def comparison_test_2():
''' Compares between se2_from_SE2 and se2_from_SE2_slow. '''
for pose in SE2.interesting_points():
se2a = se2_from_SE2(pose)
se2b = se2_from_SE2_slow(pose)
#printm('pose', pose, 'se2a', se2a, 'se2b', se2b)
assert_allclose(se2a, se2b, atol=1e-8)
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 draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not(0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
def draw(self, cr, joints=None, timestamp=None):
for js in self.joint_skins:
jointnum = js.get('joint', 0)
if not (0 <= jointnum < len(joints)):
msg = ('Invalid joint number #%d. Dynamics has '
'%d joints.' % (jointnum, len(joints)))
raise ValueError(msg)
skin = js.get('skin')
pose = js.get('pose', [0, 0, 0]) # TODO: honor this
skin_impl = get_conftools_skins().instance(skin)
robot_pose = SE2_from_SE3(joints[0][0])
joint_pose = SE2_from_SE3(joints[jointnum][0])
relative_pose = SE2.multiply(SE2.inverse(robot_pose), joint_pose)
# print('plotting skin %r at rel pose %r' %
# (skin, SE2.friendly(relative_pose)))
with cairo_rototranslate(cr, relative_pose):
skin_impl.draw(cr, timestamp=timestamp)
def __init__(self, lvel, avel, topology='plane', interval=1):
'''
:param lvel: Instantaneous linear velocity
:param avel: Instantaneous angular velocity
:param topology: Topology of the domain (plane/torus)
:param interval: Length of motion.
'''
self.topology_s = topology
SymbolicDiffeo.__init__(self, topology)
self.lvel = np.asarray(lvel)
self.avel = float(avel)
self.interval = interval
self.v = se2.algebra_from_velocities(avel=self.avel, lvel=self.lvel)
self.q = SE2.group_from_algebra(self.v)
self.R, self.t = geometry.rotation_translation_from_SE2(self.q)
def __init__(self, lvel, avel, topology='plane', interval=1):
'''
:param lvel: Instantaneous linear velocity
:param avel: Instantaneous angular velocity
:param topology: Topology of the domain (plane/torus)
:param interval: Length of motion.
'''
self.topology_s = topology
SymbolicDiffeo.__init__(self, topology)
self.lvel = np.asarray(lvel)
self.avel = float(avel)
self.interval = interval
self.v = se2.algebra_from_velocities(avel=self.avel, lvel=self.lvel)
self.q = SE2.group_from_algebra(self.v)
self.R, self.t = geometry.rotation_translation_from_SE2(self.q)
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 make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
def align(poses):
start = poses[0]
poses2 = [SE2.multiply(SE2.inverse(start), p) for p in poses]
return poses2
def integrate(self, state, command):
v = se2.algebra_from_vector(command)
delta = SE2.group_from_algebra(v * self.dt)
return np.dot(state, delta)
def default_state(self):
return SE2.identity()
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 test_conversions_se2_SE2(self):
self.check_conversion(SE2.interesting_points(),
se2_from_SE2,
SE2_from_se2)
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))
def make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2),
SE2.multiply(q2, q1))
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
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)
def test_conversions_se2_SE2(self):
self.check_conversion(SE2.interesting_points(), se2_from_SE2,
SE2_from_se2)
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)