def __init__(self, algorithm_name: str = "socnav"):
self.joystick_params = create_joystick_params()
self.algorithm_name = algorithm_name
print("Joystick running %s algorithm" % self.algorithm_name)
# include the system dynamics for both posn & velocity commands
from params.central_params import create_system_dynamics_params
self.system_dynamics_params = create_system_dynamics_params()
if self.joystick_params.use_system_dynamics:
print("Joystick using system dynamics")
else:
print("Joystick using positional dynamics")
# episode fields
self.episode_names = []
self.current_ep = None
self.sim_state_now = None
# main fields
self.joystick_on = True # status of the joystick
# socket fields
self.robot_sender_socket = None # the socket for sending commands to the robot
self.robot_receiver_socket = None # world info receiver socket
# flipped bc joystick-recv = robot-send & vice versa
self.send_ID = create_robot_params().recv_ID
self.recv_ID = create_robot_params().send_ID
print("Initiated joystick locally (AF_UNIX) at \"%s\" & \"%s\"" %
(self.send_ID, self.recv_ID))
# potentially add more fields based off the params
self.param_based_init()
def create_params():
p = create_socnav_params()
# The camera is assumed to be mounted on a robot at fixed height
# and fixed pitch. See params/central_params.py for more information
p.camera_params.width = 1024
p.camera_params.height = 1024
p.camera_params.fov_vertical = 75.0
p.camera_params.fov_horizontal = 75.0
# Introduce the robot params
from params.central_params import create_robot_params
p.robot_params = create_robot_params()
# Introduce the episode params
from params.central_params import create_episodes_params
p.episode_params = create_episodes_params()
# Tilt the camera 10 degree down from the horizontal axis
p.robot_params.physical_params.camera_elevation_degree = -10
if p.render_3D:
# Can only render rgb and depth if the host has an available display
p.camera_params.modalities = ['rgb', 'disparity']
else:
p.camera_params.modalities = ['occupancy_grid']
return p
def create_params():
p = create_socnav_params()
# The camera is assumed to be mounted on a robot at fixed height
# and fixed pitch. See params/central_params.py for more information
p.camera_params.width = 1024
p.camera_params.height = 1024
p.camera_params.fov_vertical = 75.0
p.camera_params.fov_horizontal = 75.0
# Introduce the robot params
from params.central_params import create_robot_params
p.robot_params = create_robot_params()
# Introduce the episode params
from params.central_params import create_episodes_params, create_datasets_params
p.episode_params = create_episodes_params()
# not testing robot, only simulator + agents
p.episode_params.without_robot = False
# overwrite tests with custom basic test
p.episode_params.tests = {}
default_name = "test_socnav_univ"
p.episode_params.tests[default_name] = \
DotMap(name=default_name,
map_name='Univ',
pedestrian_datasets=create_datasets_params(["univ"]),
datasets_start_t=[0.],
ped_ranges=[(0, 100)],
# agents_start=[[8, 8, 0]], agents_end=[[17.5, 13, 0.]],
agents_start=[], agents_end=[],
robot_start_goal=[[10, 3, 0], [15.5, 8, 0.7]],
max_time=30,
write_episode_log=True
)
# Tilt the camera 10 degree down from the horizontal axis
p.robot_params.physical_params.camera_elevation_degree = -10
if p.render_3D:
# Can only render rgb and depth if the host has an available display
p.camera_params.modalities = ['rgb', 'disparity']
else:
p.camera_params.modalities = ['occupancy_grid']
return p
def simulation_init(self,
sim_map,
with_planner=False,
keep_episode_running=False):
# first initialize all the agent fields such as basic self.params
super().simulation_init(sim_map,
with_planner=with_planner,
with_system_dynamics=True,
with_objectives=True,
keep_episode_running=keep_episode_running)
# this robot agent does not have a "planner" since that is done through the joystick
self.params.robot_params = create_robot_params()
# NOTE: robot radius is not the same as regular Agents
self.radius = self.params.robot_params.physical_params.radius
# velocity bounds when teleporting to positions (if not using sys dynamics)
self.v_bounds = self.params.system_dynamics_params.v_bounds
self.w_bounds = self.params.system_dynamics_params.w_bounds
# simulation update init
self.num_executed = 0 # keeps track of the latest command that is to be executed
# number of commands the joystick sends at once
self.num_cmds_per_batch = 1
# maximum number of times that the robot will repeat the last command if in asynch-mode
self.remaining_repeats = self.params.robot_params.max_repeats
def create_params():
p = create_socnav_params()
# Angle Distance parameters
p.goal_angle_objective = DotMap(power=1, angle_cost=25.0)
p.obstacle_map_params = DotMap(obstacle_map=SBPDMap,
map_origin_2=[0., 0.],
sampling_thres=2,
plotting_grid_steps=100)
# Obstacle avoidance parameters
p.avoid_obstacle_objective = DotMap(obstacle_margin0=0.3,
obstacle_margin1=.5,
power=2,
obstacle_cost=25.0)
# Angle Distance parameters
p.goal_angle_objective = DotMap(power=1, angle_cost=25.0)
# Goal Distance parameters
p.goal_distance_objective = DotMap(power=2,
goal_cost=25.0,
goal_margin=0.0)
p.objective_fn_params = DotMap(obj_type='mean')
p.obstacle_map_params = DotMap(obstacle_map=SBPDMap,
map_origin_2=[0, 0],
sampling_thres=2,
plotting_grid_steps=100)
p.personal_space_params = DotMap(power=1, psc_scale=10)
# Introduce the robot params
from params.central_params import create_robot_params
p.robot_params = create_robot_params()
# Introduce the episode params
from params.central_params import create_episodes_params, create_datasets_params
p.episode_params = create_episodes_params()
# not testing robot, only simulator + agents
p.episode_params.without_robot = True
# overwrite tests with custom basic test
p.episode_params.tests = {}
default_name = "test_psc"
p.episode_params.tests[default_name] = \
DotMap(name=default_name,
map_name='Univ',
pedestrian_datasets=create_datasets_params([]),
datasets_start_t=[],
ped_ranges=[],
agents_start=[], agents_end=[],
robot_start_goal=[[10, 3, 0], [15.5, 8, 0.7]],
max_time=30,
write_episode_log=False
)
# definitely wont be rendering this
p.render_3D = False
# Tilt the camera 10 degree down from the horizontal axis
p.robot_params.physical_params.camera_elevation_degree = -10
if p.render_3D:
# Can only render rgb and depth if the host has an available display
p.camera_params.modalities = ['rgb', 'disparity']
else:
p.camera_params.modalities = ['occupancy_grid']
return create_test_map_params(p)
# 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
"""BEGIN socket utils"""
joystick_receiver_socket = None
joystick_sender_socket = None
recv_ID = create_robot_params().recv_ID
send_ID = create_robot_params().send_ID
# clear sockets to be used
if os.path.exists(recv_ID):
os.remove(recv_ID)
if os.path.exists(send_ID):
os.remove(send_ID)
def send_sim_state(robot):
# send the (JSON serialized) world state per joystick's request
if robot.joystick_requests_world == 0:
world_state = \
robot.world_state.to_json(robot_on=not robot.get_end_acting())
send_to_joystick(world_state)