def joystick_sense(self): # ping's the robot to request a sim state self.send_to_robot("sense") # store previous pos3 of the robot (x, y, theta) robot_prev = self.robot_current.copy() # copy since its just a list # listen to the robot's reply if not self.listen_once(): # occurs if the robot is unavailable or it finished self.joystick_on = False # NOTE: at this point, self.sim_state_now is updated with the # most up-to-date simulation information # Update robot current position robot = list(self.sim_state_now.get_robots().values())[0] self.robot_current = robot.get_current_config().to_3D_numpy() # Updating robot speeds (linear and angular) based off simulator data self.robot_v = euclidean_dist2(self.robot_current, robot_prev) / self.sim_delta_t self.robot_w = (self.robot_current[2] - robot_prev[2]) / self.sim_delta_t self.sim_times += [ round(self.sim_state_now.get_sim_t() / self.sim_state_now.get_delta_t()) ]
def compute_next_vel(sim_state_prev, sim_state_now, agent_name: str): old_agent = sim_state_prev.get_all_agents()[agent_name] old_pos = old_agent.get_current_config().to_3D_numpy() new_agent = sim_state_now.get_all_agents()[agent_name] new_pos = new_agent.get_current_config().to_3D_numpy() # calculate distance over time delta_t = sim_state_now.get_sim_t() - sim_state_prev.get_sim_t() return euclidean_dist2(old_pos, new_pos) / delta_t
def generate_sim_log(self, filename='episode_log.txt'): import io abs_filename = os.path.join(self.params.output_directory, filename) touch(abs_filename) # create if dosent already exist ep_params = self.episode_params data = "" data += "****************EPISODE INFO****************\n" data += "Episode name: %s\n" % ep_params.name data += "Building name: %s\n" % ep_params.map_name data += "Robot start: %s\n" % str(ep_params.robot_start_goal[0]) data += "Robot goal: %s\n" % str(ep_params.robot_start_goal[1]) data += "Time budget: %.3f\n" % ep_params.max_time # data += "Prerec start indx: %d\n" % ep_params.prerec_start_indxs[0] data += "Total agents in scene: %d\n" % self.total_agents data += "****************SIMULATOR INFO****************\n" data += "Simulator refresh rate (s): %0.3f\n" % self.dt data += "Total duration of simulation (s): %0.3f\n" % self.sim_wall_clock num_successful = self.num_completed_agents data += "Num Successful agents: %d\n" % num_successful num_collision = self.num_collided_agents data += "Num Collided agents: %d\n" % num_collision num_timeout = self.total_agents - (num_successful + num_collision) data += "Num Timeout agents: %d\n" % num_timeout if self.robot: data += "****************ROBOT INFO****************\n" data += "Robot termination cause: %s\n" % self.robot.termination_cause data += "Robot collided with %d agent(s)\n" % \ len(self.robot_collisions) if len(self.robot_collisions) != 0: data += "Collided with: %s\n" % \ iter_print(self.robot_collisions) data += "Num commands received from joystick: %d\n" % \ len(self.robot.joystick_inputs) data += "Total time blocking for joystick input (s): %0.3f\n" % \ self.robot.get_block_t_total() data += "Num commands executed by robot: %d\n" % self.robot.num_executed rob_displacement = euclidean_dist2( ep_params.robot_start_goal[0], self.robot.get_current_config().to_3D_numpy()) data += "Robot displacement (m): %0.3f\n" % rob_displacement data += "Max robot velocity (m/s): %0.3f\n" % \ absmax(self.robot.get_trajectory().speed_nk1()) data += "Max robot acceleration: %0.3f\n" % \ absmax(self.robot.get_trajectory().acceleration_nk1()) data += "Max robot angular velocity: %0.3f\n" % \ absmax(self.robot.get_trajectory().angular_speed_nk1()) data += "Max robot angular acceleration: %0.3f\n" % \ absmax(self.robot.get_trajectory().angular_acceleration_nk1()) try: with open(abs_filename, 'w') as f: f.write(data) f.close() print("%sSuccessfully wrote episode log to %s%s" % (color_green, filename, color_reset)) except: print("%sWriting episode log failed%s" % (color_red, color_reset))
def joystick_sense(self): self.send_to_robot("sense") if (not self.listen_once()): self.joystick_on = False robot_prev = self.robot.copy() agents_prev = {} for key in list(self.agents.keys()): agent = self.agents[key] agents_prev[key] = agent.copy() # NOTE: self.sim_delta_t is available self.agents = {} self.agents_radius = {} agents_info = self.sim_state_now.get_all_agents() for key in list(agents_info.keys()): agent = agents_info[key] self.agents[key] = agent.get_current_config().to_3D_numpy() self.agents_radius[key] = agent.get_radius() robot_tmp = list(self.sim_state_now.get_robots().values())[0] self.robot = robot_tmp.get_current_config().to_3D_numpy() self.robot_radius = robot_tmp.get_radius() self.robot_v = (self.robot - robot_prev) / self.sim_delta_t self.agents_v = {} for key in list(self.agents.keys()): if key in agents_prev: v = (self.agents[key] - agents_prev[key]) / self.sim_delta_t else: v = np.array([0, 0, 0], dtype=np.float32) self.agents_v[key] = v ckpt_radius = 1 goal_radius = 5 if (euclidean_dist2(self.robot, self.robot_goal) <= ckpt_radius): if (euclidean_dist2(self.robot, self.robot_goal_final) < goal_radius): self.robot_goal = self.robot_goal_final else: self.robot_goal = self.query_next_ckpt() return
def _collision_in_group(self, own_pos: np.array, group: list): for a in group: othr_pos = a.get_current_config().to_3D_numpy() is_same_agent: bool = a.get_name() is self.get_name() if(not is_same_agent and a.get_collision_cooldown() == 0 and euclidean_dist2(own_pos, othr_pos) < self.get_radius() + a.get_radius()): # instantly collide (with agent) and stop updating self.termination_cause = "Pedestrian Collision" # name of the latest agent that the agent collided with (applicable) self.latest_collider = a.get_name() if(not self.keep_episode_running): self.end_acting = True self.collision_point_k = self.trajectory.k # this instant return True # reached here means no collisions have occured, therefore there is no latest_collider self.latest_collider = "" self.termination_cause = "Timeout" # no more collisions with these group members return False
def clip_posn(sim_dt: float, old_pos3: list, new_pos3: list, v_bounds: list, epsilon: float = 0.01): # margin of error for the velocity bounds assert (sim_dt > 0) dist_to_new = euclidean_dist2(old_pos3, new_pos3) req_vel = abs(dist_to_new / sim_dt) if req_vel <= v_bounds[1] + epsilon: return new_pos3 # calculate theta of vector valid_theta = \ np.arctan2(new_pos3[1] - old_pos3[1], new_pos3[0] - old_pos3[0]) # create new position scaled off the invalid one max_vel = sim_dt * v_bounds[1] valid_x = max_vel * np.cos(valid_theta) + old_pos3[0] valid_y = max_vel * np.sin(valid_theta) + old_pos3[1] reachable_pos3 = [valid_x, valid_y, valid_theta] print("%sposn [%s] is unreachable, clipped to [%s] (%.3fm/s > %.3fm/s)%s" % (color_red, iter_print(new_pos3), iter_print(reachable_pos3), req_vel, v_bounds[1], color_reset)) return reachable_pos3