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 plot_poses_vels_theta_omega(pylab, poses, vels):
thetas = np.array([angle_from_SE2(p) for p in poses])
omegas = np.array([linear_angular_from_se2(vel)[1] for vel in vels])
positive = omegas > 0
negative = np.logical_not(positive)
pylab.plot(np.rad2deg(thetas)[positive], omegas[positive], 'r.')
pylab.plot(np.rad2deg(thetas)[negative], omegas[negative], 'b.')
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 _integrate(self, state, commands, dt):
cmd1 = commands[:3]
cmd2 = np.array([commands[3]])
base_state = self.base_state_from_big_state(state)
top_state = self.top_state_from_big_state(state)
base_state2 = self.base.integrate(base_state, cmd1, dt)
top_state2 = self.top.integrate(top_state, cmd2, dt)
stateb = self.compose_state(base=base_state2, top=top_state2)
# Save angle
q, _ = self.top.joint_state(top_state2)
BaseTopDynamics.last_turret_angle = angle_from_SE2(SE2_from_SE3(q))
# print('angle: %s deg ' % np.rad2deg(Turret.last_turret_angle))
# print('new state:\n%s' % self.print_state(stateb))
return stateb
def _integrate(self, state, commands, dt):
cmd1 = commands[:3]
cmd2 = np.array([commands[3]])
base_state = self.base_state_from_big_state(state)
top_state = self.top_state_from_big_state(state)
base_state2 = self.base.integrate(base_state, cmd1, dt)
top_state2 = self.top.integrate(top_state, cmd2, dt)
stateb = self.compose_state(base=base_state2, top=top_state2)
# Save angle
q, _ = self.top.joint_state(top_state2)
BaseTopDynamics.last_turret_angle = angle_from_SE2(SE2_from_SE3(q))
# print('angle: %s deg ' % np.rad2deg(Turret.last_turret_angle))
# print('new state:\n%s' % self.print_state(stateb))
return stateb
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 step_too_big(pose1, pose2, max_theta_diff_deg):
diff = SE3.multiply(SE3.inverse(pose1), pose2)
diff_theta = np.abs(angle_from_SE2(SE2_from_SE3(diff)))
return diff_theta > np.deg2rad(max_theta_diff_deg)
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 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
