def kinematics2d_test():
q0 = geo.SE2_from_translation_angle([0, 0], 0)
v0 = geo.se2.zero()
c0 = q0, v0
s0 = GenericKinematicsSE2.initialize(c0)
k = 0.4
radius = 1.0 / k
print("radius: %s" % radius)
v = 3.1
perimeter = 2 * np.pi * radius
dt_loop = perimeter / v
w = 2 * np.pi / dt_loop
dt = dt_loop * 0.25
commands = geo.se2_from_linear_angular([v, 0], w)
s1 = s0.integrate(dt, commands)
s2 = s1.integrate(dt, commands)
s3 = s2.integrate(dt, commands)
s4 = s3.integrate(dt, commands)
seq = [s0, s1, s2, s3, s4]
for _ in seq:
q1, v1 = _.TSE2_from_state()
print("%s" % geo.SE2.friendly(q1))
assert_almost_equal(geo.translation_from_SE2(s1.TSE2_from_state()[0]),
[radius, radius])
assert_almost_equal(geo.translation_from_SE2(s2.TSE2_from_state()[0]),
[0, radius * 2])
assert_almost_equal(geo.translation_from_SE2(s3.TSE2_from_state()[0]),
[-radius, radius])
assert_almost_equal(geo.translation_from_SE2(s4.TSE2_from_state()[0]),
[0, 0])
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 dynamics_function(t):
state = self.dynamics.integrate(self._get_state(), commands, t)
# compute center of robot
j_pose, _ = self.dynamics.joint_state(state, 0)
center = translation_from_SE2(SE2_project_from_SE3(j_pose))
# print('t=%f, center=%s' % (t, center))
return center
def test_collisions():
L = 10
world = Box(L, L)
# id_vehicle = 'd_SE2_rb_v-rf360'
id_vehicle = 'd_SE2_rb_v-random_5'
get_vehicles_config().load('default')
vehicles = get_conftools_vehicles()
# vehicles.load('default')
vehicle = vehicles.instance(id_vehicle) # @UndefinedVariable
vehicle.set_world_primitives(world.get_primitives())
vehicle.set_pose(SE3.identity())
commands = np.array([1, 0, 0])
# go straight
simulation_length = 10
dt = 0.1
time = 0
steps = int(np.ceil(simulation_length / dt))
for _ in range(steps):
vehicle.simulate(commands, dt)
pose = SE2_from_SE3(vehicle.get_pose())
translation = translation_from_SE2(pose)
# print('t=%.3f %s' % (time, SE2.friendly(pose)))
time += dt
assert translation[0] <= 9.5
assert translation[0] >= 9.4
assert translation[1] == 0
def test_collisions():
L = 10
world = Box(L, L)
# id_vehicle = 'd_SE2_rb_v-rf360'
id_vehicle = 'd_SE2_rb_v-random_5'
get_vehicles_config().load('default')
vehicles = get_conftools_vehicles()
# vehicles.load('default')
vehicle = vehicles.instance(id_vehicle) # @UndefinedVariable
vehicle.set_world_primitives(world.get_primitives())
vehicle.set_pose(SE3.identity())
commands = np.array([1, 0, 0])
# go straight
simulation_length = 10
dt = 0.1
time = 0
steps = int(np.ceil(simulation_length / dt))
for _ in range(steps):
vehicle.simulate(commands, dt)
pose = SE2_from_SE3(vehicle.get_pose())
translation = translation_from_SE2(pose)
# print('t=%.3f %s' % (time, SE2.friendly(pose)))
time += dt
assert translation[0] <= 9.5
assert translation[0] >= 9.4
assert translation[1] == 0
def dynamics_function(t):
state = self.dynamics.integrate(self._get_state(), commands, t)
# compute center of robot
j_pose, _ = self.dynamics.joint_state(state, 0)
center = translation_from_SE2(SE2_project_from_SE3(j_pose))
# print('t=%f, center=%s' % (t, center))
return center
def update_(self, msg):
child_frame_id = msg.child_frame_id
if child_frame_id == '/base_footprint':
new_pose = pose_from_ROS_transform(msg.transform)
if False:
if self.pose is not None:
delta = SE3.multiply(SE3.inverse(self.pose), new_pose)
x, y = translation_from_SE2(SE2_from_SE3(delta))
interval = (msg.header.stamp - self.stamp).to_sec()
print('base_delta: delta %.3f seconds %.4f y %.4f' % (interval, x, y))
self.pose = new_pose
self.stamp = msg.header.stamp
def step(self, number_ticks_left, number_ticks_right, pose1: SE2) -> SE2:
d_l = 2 * np.pi * self.radius * number_ticks_left / self.max_number_ticks_encoder
d_r = 2 * np.pi * self.radius * number_ticks_right / self.max_number_ticks_encoder
d = (d_l + d_r) / 2
delta_theta = (d_r - d_l) / (self.baseline)
translation1 = translation_from_SE2(pose1)
theta1 = angle_from_SE2(pose1)
theta2 = theta1 + delta_theta
x2 = translation1[0] + d * math.cos(theta2)
y2 = translation1[1] + d * math.sin(theta2)
pose2 = SE2_from_xytheta([x2, y2, theta2])
return pose2
def colliding_pose(self, pose, safety_margin=1):
'''
Checks that the given pose does not give collisions.
Returns None or a CollisionInfo structure.
safety_margin: multiplies the radius
'''
state = self.dynamics.pose2state(pose)
j_pose = self.dynamics.joint_state(state, 0)[0]
center = translation_from_SE2(SE2_project_from_SE3(j_pose))
collision = collides_with(self.primitives,
center, self.radius * safety_margin)
return collision
def colliding_pose(self, pose, safety_margin=1):
'''
Checks that the given pose does not give collisions.
Returns None or a CollisionInfo structure.
safety_margin: multiplies the radius
'''
state = self.dynamics.pose2state(pose)
j_pose = self.dynamics.joint_state(state, 0)[0]
center = translation_from_SE2(SE2_project_from_SE3(j_pose))
collision = collides_with(self.primitives, center,
self.radius * safety_margin)
return collision
def vehicles_cairo_set_coordinates(cr, width, height, world_bounds,
robot_pose, zoom,
world_bounds_pad=0,
first_person=False):
bx = world_bounds[0]
by = world_bounds[1]
if zoom == 0:
m = world_bounds_pad
extents = [bx[0] - m,
bx[1] + m,
by[0] - m,
by[1] + m]
else:
t = translation_from_SE2(robot_pose)
# don't go over the world side
if not first_person:
t[0] = np.maximum(t[0], bx[0] + zoom)
t[0] = np.minimum(t[0], bx[1] - zoom)
t[1] = np.maximum(t[1], by[0] + zoom)
t[1] = np.minimum(t[1], by[1] - zoom)
m = 0 # extra space
extents = [t[0] - zoom - m,
t[0] + zoom + m,
t[1] - zoom - m,
t[1] + zoom + m]
cairo_set_axis(cr, width, height, extents)
if first_person and zoom != 0:
angle = angle_from_SE2(robot_pose)
t = translation_from_SE2(robot_pose)
cr.translate(t[0], t[1])
cr.rotate(-angle)
cr.rotate(+np.pi / 2) # make the robot point "up"
cr.translate(-t[0], -t[1])
def evaluate(self, context: RuleEvaluationContext, result: RuleEvaluationResult):
poses = context.get_ego_pose_global()
p0 = poses.values[0].as_SE2()
values = []
timestamps = []
values_improv = []
d_max = 0.0
for timestamp, v in poses:
r = relative_pose(p0, v.as_SE2())
t = geo.translation_from_SE2(r)
di = float(np.linalg.norm(t))
# print(d_max, di)
improvement = max(di - d_max, 0.0)
d_max = max(d_max, di)
values_improv.append(improvement)
values.append(d_max)
timestamps.append(timestamp)
cumulative = SampledSequence[float](timestamps, values)
incremental = SampledSequence[float](timestamps, values_improv)
d_tot = d_max
title = "Distance from initial point"
description = textwrap.dedent(
"""\
Distance from the starting point.
"""
)
result.set_metric(
name=(),
total=d_tot,
incremental=incremental,
title=title,
description=description,
cumulative=cumulative,
)
def get_grid(robot, vsim, resolution, debug=False):
# Note: use robot to get the observations, be cause it might
# include some nuisances that you don't get if you use vsim
# directly
from vehicles_boot import BOVehicleSimulation
if not isinstance(vsim, BOVehicleSimulation):
msg = ('I really require a VehicleSimulation; obtained %s.' %
describe_type(vsim))
raise ValueError(msg)
world = vsim.world
vehicle = vsim.vehicle
from geometry import (translation_from_SE2,
SE2_from_translation_angle, SE2_from_xytheta, SE3_from_SE2)
from vehicles.simulation.collision import collides_with
bounds = world.bounds
bx = bounds[0]
by = bounds[1]
wx = float(bx[1] - bx[0])
wy = float(by[1] - by[0])
nx = int(np.ceil(wx / resolution))
ny = int(np.ceil(wy / resolution))
cx = wx / nx
cy = wy / ny
primitives = world.get_primitives()
vehicle.set_world_primitives(primitives)
grid = np.zeros((nx, ny), int)
grid.fill(-1)
locations = []
for i, j in itertools.product(range(nx), range(ny)):
x = bx[0] + (i + 0.5) * cx
y = by[0] + (j + 0.5) * cy
theta = 0
pose = SE3_from_SE2(SE2_from_xytheta([x, y, theta]))
loc = dict(cell=(i, j), pose=pose)
grid[i, j] = len(locations)
locations.append(loc)
def collision_at(pose):
center = translation_from_SE2(SE2_from_SE3(pose))
collision = collides_with(primitives, center=center,
radius=vehicle.radius * 2.5)
# print('center %s: %s' % (center, collision))
return collision.collided
def cell_free(node):
loc = locations[grid[node]]
if not 'collision' in loc:
loc['collision'] = collision_at(loc['pose'])
# print('Checking %s: %s' % (node, loc['collision']))
return not loc['collision']
def cost(node1, node2):
p1 = np.array(node1)
p2 = np.array(node2)
dist = np.linalg.norm(p1 - p2)
return dist
def node2children(node):
return node2children_grid(node, shape=grid.shape,
cell_free=cell_free,
cost=cost)
def heuristics(node, target):
return cost(node, target)
def get_start_cells():
order = range(len(locations))[::5]
for a in order:
cell = locations[a]['cell']
if cell_free(cell):
yield cell
else:
raise Exception("No free space at all")
def get_target_cells(start):
""" Enumerate end cells rom the bottom """
def goodness(cell):
return cost(start, cell)
order = np.argsort([-goodness(l['cell']) for l in locations])
found = False
for k in order[::5]:
cell = locations[k]['cell']
if cell_free(cell):
found = True
# print('TRying %s (%s)' % (str(cell), goodness(cell)))
yield cell
if not found:
raise Exception("No free space at all")
# Find a long path
def get_path():
for start in get_start_cells():
for target in get_target_cells(start):
path, _ = astar(start, target, node2children, heuristics)
if path:
return path
else:
raise Exception('Could not find any path.')
path = get_path()
locations = [locations[grid[c]] for c in path]
# adjust poses angle
for i in range(len(locations) - 1):
loc1 = locations[i]
loc2 = locations[i + 1]
t1 = translation_from_SE2(SE2_from_SE3(loc1['pose']))
t2 = translation_from_SE2(SE2_from_SE3(loc2['pose']))
d = t2 - t1
theta = np.arctan2(d[1], d[0])
diff = SE3_from_SE2(SE2_from_translation_angle(t=[0, 0], theta=theta))
loc1['pose'] = np.dot(loc1['pose'], diff)
poses = [l['pose'] for l in locations]
# project to SE2
poses_se2 = map(SE2_from_SE3, poses)
# smooth path
poses_se2 = elastic(SE2, poses_se2, alpha=0.1, num_iterations=20)
poses = map(SE3_from_SE2, poses_se2)
# compute observations
print('Found path of length %s' % len(poses))
# poses = interpolate_sequence(poses, max_theta_diff_deg=30)
# compute observations
print('Upsampled at %s' % len(poses))
locations = [dict(pose=pose) for pose in poses]
for loc in locations:
pose = loc['pose']
vehicle.set_pose(pose)
if not debug:
loc['observations'] = mean_observations(robot, n=10)
return locations
def collision_at(pose):
center = translation_from_SE2(SE2_from_SE3(pose))
collision = collides_with(primitives, center=center,
radius=vehicle.radius * 2.5)
# print('center %s: %s' % (center, collision))
return collision.collided
def servonav_episode(
id_robot,
robot,
id_servo_agent,
servo_agent,
writer,
id_episode,
max_episode_len,
save_robot_state,
interval_write=1,
interval_print=5,
resolution=0.5, # grid resolution
delta_t_threshold=0.2, # when to switch
MIN_PATH_LENGTH=8,
MAX_TIME_FOR_SWITCH=20.0,
fail_if_not_working=False,
max_tries=10000,
):
"""
:arg:displacement: Time in seconds to displace the robot.
"""
from geometry import SE2_from_SE3, translation_from_SE2, angle_from_SE2, SE3
stats_write = InAWhile(interval_print)
# Access the vehicleSimulation interface
vsim = get_vsim_from_robot(robot)
for _ in xrange(max_tries):
# iterate until we can do this correctly
episode = robot.new_episode()
locations = get_grid(robot=robot, vsim=vsim, resolution=resolution)
if len(locations) < MIN_PATH_LENGTH:
logger.info("Path too short, trying again.")
else:
break
else:
msg = "Could not find path in %d tries." % max_tries
raise Exception(msg)
locations_yaml = convert_to_yaml(locations)
vsim.vehicle.set_pose(locations[0]["pose"])
current_goal = 1
current_goal_obs = locations[current_goal]["observations"]
servo_agent.set_goal_observations(current_goal_obs)
counter = 0
time_last_switch = None
num_written = 0
for robot_observations, boot_observations in run_simulation_servonav(
id_robot,
robot,
id_servo_agent,
servo_agent,
100000,
max_episode_len,
id_episode=id_episode,
id_environment=episode.id_environment,
raise_error_on_collision=fail_if_not_working,
):
current_time = boot_observations["timestamp"].item()
if time_last_switch is None:
time_last_switch = current_time
time_since_last_switch = float(current_time - time_last_switch)
def obs_distance(obs1, obs2):
return float(np.linalg.norm(obs1.flatten() - obs2.flatten()))
curr_pose = robot_observations.robot_pose
curr_obs = boot_observations["observations"]
curr_goal = locations[current_goal]["observations"]
prev_goal = locations[current_goal - 1]["observations"]
curr_err = obs_distance(curr_goal, curr_obs)
prev_err = obs_distance(prev_goal, curr_obs)
current_goal_pose = locations[current_goal]["pose"]
current_goal_obs = locations[current_goal]["observations"]
delta = SE2_from_SE3(SE3.multiply(SE3.inverse(curr_pose), current_goal_pose))
delta_t = np.linalg.norm(translation_from_SE2(delta))
delta_th = np.abs(angle_from_SE2(delta))
if stats_write.its_time():
msg = " deltaT: %.2fm deltaTh: %.1fdeg" % (delta_t, np.rad2deg(delta_th))
logger.debug(msg)
# If at the final goal, go closer
is_final_goal = current_goal == len(locations) - 1
if is_final_goal:
delta_t_threshold *= 0.3
# TODO: should we care also about delta_th?
time_to_switch = (delta_t < delta_t_threshold) or (time_since_last_switch > MAX_TIME_FOR_SWITCH)
# does not work: curr_err < SWITCH_THRESHOLD * prev_err:
if time_to_switch:
current_goal += 1
logger.info("Switched to goal %d." % current_goal)
time_last_switch = current_time
if current_goal >= len(locations):
# finished
logger.info("Finished :-)")
break
threshold_lost_m = 3
if delta_t > threshold_lost_m:
msg = "Breaking because too far away."
if not (fail_if_not_working):
logger.error(msg)
break
else:
raise Exception(msg)
servo_agent.set_goal_observations(current_goal_obs)
extra = {}
extra["servoing_base"] = dict(goal=curr_goal.tolist(), current=curr_obs.tolist())
extra["servoing_poses"] = dict(goal=SE3.to_yaml(current_goal_pose), current=SE3.to_yaml(curr_pose))
extra["servonav"] = dict(
poseK=SE3.to_yaml(curr_pose),
obsK=boot_observations["observations"].tolist(),
pose1=SE3.to_yaml(current_goal_pose),
locations=locations_yaml,
current_goal=current_goal,
curr_err=curr_err,
prev_err=prev_err,
time_last_switch=time_last_switch,
time_since_last_switch=time_since_last_switch,
)
if counter % interval_write == 0:
if save_robot_state:
extra["robot_state"] = robot.get_state()
writer.push_observations(observations=boot_observations, extra=extra)
num_written += 1
counter += 1
if num_written == 0:
msg = "This log was too short to be written (%d observations)" % counter
raise Exception(msg)
def update_primitives(self):
target_pose = self.get_target_pose()
self.target.set_center(translation_from_SE2(target_pose))
def servo_stats_report(data_central, id_agent, id_robot, summaries,
phase='servo_stats'):
from reprep import Report
from reprep.plot_utils import x_axis_balanced
from reprep.plot_utils import (style_ieee_fullcol_xy,
style_ieee_halfcol_xy)
from geometry import translation_from_SE2
if not summaries:
raise Exception('Empty summaries')
def extract(key):
return np.array([s[key] for s in summaries])
initial_distance = extract('initial_distance')
initial_rotation = extract('initial_rotation')
dist_xy_converged = 0.25
dist_th_converged = np.deg2rad(5)
for s in summaries:
dist_xy = s['dist_xy']
dist_th = s['dist_th']
# converged = (dist_xy[0] > dist_xy[-1]) and (dist_th[0] > dist_th[-1])
converged = ((dist_xy[-1] < dist_xy_converged) and
(dist_th[-1] < dist_th_converged))
s['converged'] = converged
s['dist_th_deg'] = np.rad2deg(s['dist_th'])
s['color'] = 'b' if converged else 'r'
trans = [translation_from_SE2(x) for x in s['poses']]
s['x'] = np.abs([t[0] for t in trans])
s['y'] = np.abs([t[1] for t in trans])
s['dist_x'] = np.abs(s['x'])
s['dist_y'] = np.abs(s['y'])
basename = 'servo_analysis-%s-%s-%s' % (id_agent, id_robot, phase)
r = Report(basename)
r.data('summaries', s, caption='All raw statistics')
f = r.figure(cols=3)
with f.plot('image_L2_error') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
errors = s['errors']
pylab.plot(errors, s['color'])
with f.plot('dist_xy') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(s['dist_xy'], s['color'] + '-')
with f.plot('xy') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(s['x'], s['y'], s['color'] + '-')
pylab.xlabel('x')
pylab.ylabel('y')
with f.plot('dist_th') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(s['dist_th'], s['color'] + '-')
with f.plot('dist_th_deg') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(np.rad2deg(s['dist_th']), s['color'] + '-')
with f.plot('dist_xy_th') as pl:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pl.plot(s['dist_xy'], s['dist_th_deg'], s['color'] + '-')
pl.xlabel('dist x-y')
pl.ylabel('dist th (deg)')
with f.plot('dist_xy_th_log') as pl:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pl.semilogx(s['dist_xy'],
s['dist_th_deg'], s['color'] + '.')
pl.xlabel('dist x-y')
pl.ylabel('dist th (deg)')
with f.plot('dist_y') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(s['dist_y'], s['color'] + '-')
with f.plot('dist_x') as pylab:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pylab.plot(s['dist_x'], s['color'] + '-')
mark_start = 's'
mark_end = 'o'
with f.plot('dist_xy_th_start',
caption="Initial error (blue: converged)"
) as pl:
style_ieee_fullcol_xy(pylab)
for s in summaries:
pl.plot([s['dist_xy'][0], s['dist_xy'][-1]],
[s['dist_th_deg'][0],
s['dist_th_deg'][-1]], s['color'] + '-')
pl.plot(s['dist_xy'][0], s['dist_th_deg'][0],
s['color'] + mark_start)
pl.plot(s['dist_xy'][-1], s['dist_th_deg'][-1],
s['color'] + mark_end)
pl.xlabel('dist x-y')
pl.ylabel('dist th (deg)')
with f.plot('dist_xy_th_start2',
caption="Trajectories. If converged, plot square at beginning"
" and cross at end. If not converged, plot trajectory (red)."
) as pl:
style_ieee_fullcol_xy(pylab)
for s in summaries:
if s['converged']:
continue
pl.plot([s['dist_xy'][0], s['dist_xy'][-1]],
[s['dist_th_deg'][0], s['dist_th_deg'][-1]], 'r-')
for s in summaries:
pl.plot(s['dist_xy'][0], s['dist_th_deg'][0], s['color'] + mark_start)
pl.plot(s['dist_xy'][-1], s['dist_th_deg'][-1], s['color'] + mark_end)
pl.xlabel('dist x-y')
pl.ylabel('dist th (deg)')
with f.plot('initial_rotation') as pylab:
style_ieee_halfcol_xy(pylab)
pylab.hist(np.rad2deg(initial_rotation))
x_axis_balanced(pylab)
pylab.xlabel('Initial rotation (deg)')
with f.plot('initial_distance') as pylab:
style_ieee_halfcol_xy(pylab)
pylab.hist(initial_distance)
pylab.xlabel('Initial distance (m)')
ds = data_central.get_dir_structure()
filename = ds.get_report_filename(id_agent=id_agent,
id_robot=id_robot,
id_state='servo_stats',
phase=phase)
resources_dir = ds.get_report_res_dir(id_agent=id_agent,
id_robot=id_robot,
id_state='servo_stats',
phase=phase)
save_report(data_central, r, filename, resources_dir, save_pickle=True)
def distance_to_centroid(pose):
t = translation_from_SE2(pose)
d = np.linalg.norm(t - centroid)
return d
def servoing_episode(id_robot, robot,
id_servo_agent, servo_agent,
writer, id_episode,
displacement,
max_episode_len,
save_robot_state,
converged_dist_t_m=0.1,
converged_dist_th_deg=1,
max_tries=10000):
'''
:arg:displacement: Time in seconds to displace the robot.
'''
from geometry import SE3
def mean_observations(n=10):
obss = []
for _ in range(n): # XXX: fixed threshold
obss.append(robot.get_observations().observations)
return np.mean(obss, axis=0)
def robot_pose():
return robot.get_observations().robot_pose
def timestamp():
return robot.get_observations().timestamp
def episode_ended():
return robot.get_observations().episode_end
def simulate_hold(cmd0, displacement):
t0 = timestamp()
nsteps = 0
while timestamp() < t0 + displacement:
nsteps += 1
source = BootOlympicsConstants.CMD_SOURCE_SERVO_DISPLACEMENT
robot.set_commands(cmd0, source)
if episode_ended():
logger.debug('Collision after %d steps' % ntries)
return False
logger.debug('%d steps of simulation to displace by %s' %
(nsteps, displacement))
return True
for ntries in xrange(max_tries):
# iterate until we can do this correctly
episode = robot.new_episode()
obs0 = mean_observations()
cmd0 = robot.get_spec().get_commands().get_random_value()
pose0 = robot_pose()
ok = simulate_hold(cmd0, displacement)
if ok:
pose1 = robot_pose()
logger.info('Displacement after %s tries.' % ntries)
break
else:
msg = 'Could not do the displacement (%d tries).' % max_tries
raise Exception(msg)
servo_agent.set_goal_observations(obs0)
for robot_observations, boot_observations in \
run_simulation_servo(id_robot, robot,
id_servo_agent, servo_agent,
100000, max_episode_len,
id_episode=id_episode,
id_environment=episode.id_environment):
def pose_to_yaml(x):
''' Converts to yaml, or sets None. '''
if x is None:
return None
else:
return SE3.to_yaml(x)
extra = {}
sensels_list = boot_observations['observations'].tolist()
extra['servoing_base'] = dict(goal=obs0.tolist(), current=sensels_list)
current_pose = robot_observations.robot_pose
has_pose = current_pose is not None
if has_pose:
# Add extra pose information
extra['servoing_poses'] = dict(goal=pose_to_yaml(pose0),
current=pose_to_yaml(current_pose))
delta = SE2_from_SE3(SE3.multiply(SE3.inverse(current_pose),
pose0))
dist_t_m = np.linalg.norm(translation_from_SE2(delta))
dist_th_deg = np.abs(angle_from_SE2(delta))
# TODO: make it not overlapping
extra['servoing'] = dict(obs0=obs0.tolist(),
pose0=pose_to_yaml(pose0),
poseK=pose_to_yaml(current_pose),
obsK=sensels_list,
displacement=displacement,
cmd0=cmd0.tolist(),
pose1=pose_to_yaml(pose1))
if save_robot_state:
extra['robot_state'] = robot.get_state()
writer.push_observations(observations=boot_observations,
extra=extra)
if has_pose:
if ((dist_t_m <= converged_dist_t_m) and
(dist_th_deg <= converged_dist_th_deg)):
print('Converged!')
break
else:
# TODO: write convergence criterion
# without pose information
pass
