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