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 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