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