def generate_cmhall_humans(self):
"""
generates vertically moving humans
"""
for i in range(self.human_num):
human = Human(self.config, 'humans')
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
horizontal_shift = (np.random.normal(scale=0.2)) * 4
vertical_shift = (np.random.normal(scale=0.2)) * 2
px = horizontal_shift
if sign > 0:
py = sign * 4 + vertical_shift
if sign < 0:
py = -4 - 1.5 + vertical_shift
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
gx = horizontal_shift + np.random.normal()
gy = -sign * 4
human.set(px, py, gx, gy, 0, 0, 0)
self.humans.append(human)
def generate_cmhallow__humans(self, horizontal=False):
for i in range(self.human_num):
human = Human(self.config, 'humans')
while True:
horizontal_shift_sp = (np.random.rand() - 0.5) * 4.5
vertical_shift_sp = (np.random.rand() - 0.5) * 2
if horizontal:
py = horizontal_shift_sp
px = 4.5 + vertical_shift_sp
else:
px = horizontal_shift_sp
py = 4 + vertical_shift_sp
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
horizontal_shift_g = (np.random.rand() - 0.5) * 4.5
vertical_shift_g = (np.random.rand() - 0.5) * 2
if horizontal:
gy = horizontal_shift_sp
gx = -4 + vertical_shift_g
else:
gx = horizontal_shift_sp
gy = -4 + vertical_shift_g
human.set(px, py, gx, gy, 0, 0, 0)
self.humans.append(human)
def generate_circle_static_obstacle(self, position=None):
# generate a human with radius = 0.3, v_pref = 1, visible = True, and policy = orca
human = Human(self.config, 'humans')
# For fixed position
if position:
px, py = position
# For random position
else:
while True:
angle = np.random.random() * np.pi * 2
# add some noise to simulate all the possible cases robot could meet with human
v_pref = 1.0 if human.v_pref == 0 else human.v_pref
px_noise = (np.random.random() - 0.5) * v_pref
py_noise = (np.random.random() - 0.5) * v_pref
px = self.circle_radius * np.cos(angle) + px_noise
py = self.circle_radius * np.sin(angle) + py_noise
collide = False
for agent in [self.robot] + self.humans:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
break
if not collide:
break
# make it a static obstacle
# px, py, gx, gy, vx, vy, theta
human.set(px, py, px, py, 0, 0, 0, v_pref=0)
return human
def test_overlap_no_movement(self):
cell_num = 4
cell_size = 1
om_channel_size = 1
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.zeros((1, 4))
occupancy_map = build_occupancy_map(human, other_agents, cell_num,
cell_size, om_channel_size)
radius = 1
state = ObservableState(0, 0, 0, 0, radius)
action = ActionXY(0, 0)
next_state = propagate(state,
action,
time_step=.1,
kinematics='holonomic')
next_occupancy_map = propagate_occupancy_map(occupancy_map, state,
action, .1, 'holonomic',
cell_num, cell_size,
om_channel_size)
expected_next_occupancy_map = build_occupancy_map(
next_state, other_agents, cell_num, cell_size, om_channel_size)
self.assertTrue(
np.array_equal(next_occupancy_map, expected_next_occupancy_map))
def generate_human(self, human=None):
if human is None:
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
if self.human_safety_space:
human.policy.safety_space = self.human_safety_space
return human
def test_no_overlap(self):
cell_num = 4
cell_size = 1
om_channel_size = 1
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.zeros((1, 4)) + 100
result = build_occupancy_map(human, other_agents, cell_num, cell_size,
om_channel_size)
expected_result = np.zeros((cell_num**2))
self.assertTrue(np.array_equal(result, expected_result))
def test_right_upper_corner(self):
cell_num = 4
cell_size = 1
om_channel_size = 1
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.array([1.5, 1.5, 0, 0]).reshape(1, -1)
result = build_occupancy_map(human, other_agents, cell_num, cell_size,
om_channel_size)
expected_result = np.zeros((cell_num**2))
expected_result[15] = 1
self.assertTrue(np.array_equal(result, expected_result))
def generate_tri_humans(self):
human = Human(self.config, 'humans')
human.set(0, 4.1, 0, -4, 0, 0, 0)
self.humans.append(human)
vertical_shift = 0
hor_pos = 0
dist_between_humans = (2 * 0.3 + self.discomfort_dist + 0.1)
for i in range(self.human_num - 1):
human = Human(self.config, 'humans')
sign = -1 if i % 2 == 0 else 1
if i % 2 == 0:
vertical_shift += 0.4
hor_pos += 1
horizontal_shift = sign * hor_pos * dist_between_humans
px = horizontal_shift
py = 4 + vertical_shift
for agent in [self.robot] + self.humans:
assert (norm((px - agent.px, py - agent.py)) >
human.radius + agent.radius + self.discomfort_dist)
gx = horizontal_shift
gy = -4
human.set(px, py, gx, gy, 0, 0, 0)
self.humans.append(human)
def test_human_map_three_channels(self):
cell_num = 4
cell_size = 1
om_channel_size = 3
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.array([1.5, 1.5, 1, 2]).reshape(1, -1)
result = build_occupancy_map(human, other_agents, cell_num, cell_size,
om_channel_size)
expected_result = np.zeros((cell_num**2 * om_channel_size))
expected_result[15] = 1
expected_result[15 + cell_num**2] = 1
expected_result[15 + cell_num**2 * 2] = 2
self.assertTrue(np.allclose(result, expected_result, atol=1e-5))
def generate_ldl_humans(self) :
dist_between_humans = (2 * 0.3 + self.discomfort_dist + 0.1)
lane_size = self.human_num * dist_between_humans
for i in range(self.human_num) :
human = Human(self.config, 'humans')
horizontal_shift = -lane_size/2 + (i * dist_between_humans)
py = horizontal_shift
px = 4.5
collide = False
for agent in [self.robot] + self.humans:
assert(norm((px - agent.px, py - agent.py)) > human.radius + agent.radius + self.discomfort_dist)
gy = horizontal_shift
gx = -4
human.set(px, py, gx, gy, 0, 0, 0)
self.humans.append(human)
def generate_human(self, human=None):
if human is None:
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
if self.current_scenario == 'circle_crossing':
while True:
angle = np.random.random() * np.pi * 2
# add some noise to simulate all the possible cases robot could meet with human
px_noise = (np.random.random() - 0.5) * human.v_pref
py_noise = (np.random.random() - 0.5) * human.v_pref
px = self.circle_radius * np.cos(angle) + px_noise
py = self.circle_radius * np.sin(angle) + py_noise
collide = False
for agent in [self.robot] + self.humans:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
break
if not collide:
break
human.set(px, py, -px, -py, 0, 0, 0)
elif self.current_scenario == 'square_crossing':
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
px = np.random.random() * self.square_width * 0.5 * sign
py = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
while True:
gx = np.random.random() * self.square_width * 0.5 * -sign
gy = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(gx - agent.gx, gy - agent.gy)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
human.set(px, py, gx, gy, 0, 0, 0)
return human
def generate_circle_crossing_human(self, lb=False):
human = Human(self.config, 'humans')
if lb:
human = HumanLB(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
while True:
angle = np.random.random() * np.pi * 2
###angle = 0
# add some noise to simulate all the possible cases robot could meet with human
px_noise = (np.random.random() - 0.5) * human.v_pref
py_noise = (np.random.random() - 0.5) * human.v_pref
px = self.circle_radius * np.cos(angle) + px_noise
py = self.circle_radius * np.sin(angle) + py_noise
collide = False
for agent in [self.robot] + self.humans:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
break
if not collide:
break
###human.set(px, py, -px, -py, 0, 0, 0)
human.set(10, 10, 10, 10, 0, 0, 0)
return human
def test_human_rotated_counter_clockwise(self):
cell_num = 4
cell_size = 1
om_channel_size = 1
human = Human()
human.set(px=0,
py=0,
vx=np.sqrt(2),
vy=np.sqrt(2),
gx=0,
gy=0,
theta=0)
other_agents = np.array([1.5, 0, 0, 0]).reshape(1, -1)
result = build_occupancy_map(human, other_agents, cell_num, cell_size,
om_channel_size)
expected_result = np.zeros((cell_num**2))
expected_result[3] = 1
self.assertTrue(np.array_equal(result, expected_result))
def test_get_states_from_occupancy_map(self):
cell_num = 4
cell_size = 1
om_channel_size = 1
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.zeros((1, 4))
for i in np.linspace(-2, 2, 9):
for j in np.linspace(-2, 2, 9):
other_agents[0, 0] = i
other_agents[0, 1] = j
occupancy_map = build_occupancy_map(human, other_agents,
cell_num, cell_size,
om_channel_size)
states = get_states_from_occupancy_map(occupancy_map, cell_num,
cell_size,
om_channel_size)
if states.shape[0] > 0:
self.assertTrue(
np.allclose(states, other_agents, atol=cell_size / 2.))
def set_humans(self):
if self.test_case == -1:
human_settings = [
# activate, px, py, gx, gy, vx, vy
[True, 3.5, 0.0, -3.5, 0.0, 0.0, 0.0, 0.0],
[True, 4.5, 1.0, -4.0, 1.0, 0.0, 0.0, 0.0],
[True, 4.5, 2, -4.0, 2, 0.0, 0.0, 0.0],
[True, 4.5, 3.0, -4.0, 3.0, 0.0, 0.0, 0.0],
[False, 5.0, 3.0, -6.5, 3.0, 0.0, 0.0, 0.0],
]
if self.test_case == -2:
radius = 1.0
human_settings = []
for i_human in range(5):
angle = 2 * np.pi / 5 * i_human
setting = [True, radius * np.sin(angle), radius * np.cos(angle), radius * np.sin(angle), radius * np.cos(angle), 0.0, 0.0, 0.0]
human_settings.append(setting)
if self.test_case == -3:
human_settings = [
# activate, px, py, gx, gy, vx, vy
[True, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[True, -1.5, 0.0, 8.5, 0.0, 0.0, 0.0, 0.0],
[False, 4.5, 2, -4.0, 2, 0.0, 0.0, 0.0],
[False, 4.5, 3.0, -4.0, 3.0, 0.0, 0.0, 0.0],
[False, 5.0, 3.0, -6.5, 3.0, 0.0, 0.0, 0.0],
]
for human_setting in human_settings:
if human_setting[0]:
human = Human(self.config, 'humans')
px = human_setting[1]
py = human_setting[2]
gx = human_setting[3]
gy = human_setting[4]
vx = human_setting[5]
vy = human_setting[6]
theta = human_setting[7]
human.set(px=px, py=py, gx=gx, gy=gy, vx=vx, vy=vy, theta=theta)
self.humans.append(human)
def generate_circle_crossing_human(self):
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
while True:
angle = np.random.random() * np.pi * 2
# add some noise to simulate all the possible cases robot could meet with human
v_pref = 1.0 if human.v_pref == 0 else human.v_pref
px_noise = (np.random.random() - 0.5) * v_pref
py_noise = (np.random.random() - 0.5) * v_pref
px = self.circle_radius * np.cos(angle) + px_noise
py = self.circle_radius * np.sin(angle) + py_noise
collide = False
if self.group_human:
collide = self.check_collision_group((px, py), human.radius)
else:
for i, agent in enumerate([self.robot] + self.humans):
# keep human at least 3 meters away from robot
if self.robot.kinematics == 'unicycle' and i == 0:
min_dist = self.circle_radius / 2 # Todo: if circle_radius <= 4, it will get stuck here
else:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
break
if not collide:
break
human.set(px, py, -px, -py, 0, 0, 0)
return human
def generate_lanes_humans(self):
for i in range(self.human_num):
human = Human(self.config, 'humans')
sign = -1 if i % 2 == 0 else 1
if i in range(5):
px = 0
py = -4 + (i + 1) * (8 / 6)
human.set(px, py, px, py, 0, 0, 0)
self.humans.append(human)
continue
while True:
px = sign * (np.random.rand()) * 4
vertical_shift = -sign * (np.random.rand() - 0.3) * 6
py = sign * 4 + vertical_shift
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
gx = px + np.random.normal(scale=0.2)
gy = -sign * (4 - vertical_shift)
human.set(px, py, gx, gy, 0, 0, 0)
self.humans.append(human)
def generate_circle_crossing_human(self, robot_goal=None):
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
while True:
angle = np.random.random() * np.pi * 2
# add some noise to simulate all the possible cases robot could meet with human
px_noise = (np.random.random() - 0.5) * human.v_pref
py_noise = (np.random.random() - 0.5) * human.v_pref
px = self.circle_radius * np.cos(angle) + px_noise
py = self.circle_radius * np.sin(angle) + py_noise
collide = False
for agent in [self.robot] + self.humans:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
break
if not collide:
break
gx = -px
gy = -py
#ensure no agents are standing on the robot's goal
if robot_goal is not None:
if (abs(gx - robot_goal[0]) < 1.5) and (abs(gy - robot_goal[1]) <
1.5):
gx += np.sign(gx - robot_goal[0]) * 2 #push the goals apart
gy += np.sign(gy - robot_goal[0]) * 2
human.set(px, py, gx, gy, 0, 0, 0)
return human
def test_propagate_occupancy_map_agent_center_movement_left_low_channel_size_three(
self):
cell_num = 4
cell_size = 1
om_channel_size = 3
time_step = .1
human = Human()
human.set(px=0, py=0, vx=0, vy=0, gx=0, gy=0, theta=0)
other_agents = np.zeros((1, 4))
other_agents[0, 0] = -1.5
other_agents[0, 1] = 1.5
occupancy_map = build_occupancy_map(human, other_agents, cell_num,
cell_size, om_channel_size)
action = ActionXY(-1, -1)
next_state = propagate(human.get_observable_state(),
action,
time_step,
kinematics='holonomic')
next_occupancy_map = propagate_occupancy_map(
occupancy_map, human.get_observable_state(), action, time_step,
'holonomic', cell_num, cell_size, om_channel_size)
expected_next_occupancy_map = build_occupancy_map(
next_state, other_agents, cell_num, cell_size, om_channel_size)
self.assertTrue(
np.array_equal(next_occupancy_map, expected_next_occupancy_map))
def generate_square_crossing_human(self):
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
px = np.random.random() * self.square_width * 0.5 * sign
py = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
while True:
gx = np.random.random() * self.square_width * 0.5 * -sign
gy = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(gx - agent.gx, gy - agent.gy
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
human.set(px, py, gx, gy, 0, 0, 0)
return human
def generate_square_crossing_human(self, robot_goal=None):
np.random.seed()
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
px = np.random.random() * self.square_width * 0.5 * sign
py = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
while True:
gx = np.random.random() * self.square_width * 0.5 * -sign
gy = (np.random.random() - 0.5) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(gx - agent.gx, gy - agent.gy
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
#ensure no agents are standing on the robot's goal
if robot_goal is not None:
if (abs(gx - robot_goal[0]) < 1.5) and (abs(gy - robot_goal[1]) <
1.5):
gx += np.sign(gx - robot_goal[0]) * 2 #push the goals apart
gy += np.sign(gy - robot_goal[0]) * 2
human.set(px, py, gx, gy, 0, 0, 0)
return human
def generate_wall_crossing_human(self):
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
# if np.random.random() > 0.5:
# sign = -1
# else:
# sign = 1
sign = 1
while True:
px = np.random.random(
) * self.square_height * 0.01 * sign * np.cos(np.pi / 2) + 2
py = (np.random.random() - 0.01) * self.square_height * np.sin(
np.pi / 2)
# px = self.square_width * 0.1 * sign
# py = (- 0.1) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
while True:
gx = np.random.random() * self.square_height * 0.01 * -sign
gy = (np.random.random() - 0.1) * self.square_height
# gx = self.square_width * 0.1 * -sign
# gy = (- 0.1) * self.square_width
collide = False
for agent in [self.robot] + self.humans:
if norm(
(gx - agent.gx, gy - agent.gy
)) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
human.set(px, py, gx, gy, 0, 0, 0)
return human
def generate_CA_human(self):
human = Human(self.config, 'humans')
if self.randomize_attributes:
human.sample_random_attributes()
while True:
angle = np.random.random() * np.pi * 2
# add some noise to simulate all the possible cases robot could meet with human
px_noise = (np.random.random() - 0.5) * human.v_pref
py_noise = (np.random.random() - 0.5) * human.v_pref
px = px_noise
py = self.circle_radius + py_noise - 1 #(0,r-1) --> (0,-r+1)
collide = False
for agent in [self.robot] + self.humans:
min_dist = human.radius + agent.radius + self.discomfort_dist
if norm((px - agent.px, py - agent.py)) < min_dist or \
norm((px - agent.gx, py - agent.gy)) < min_dist:
collide = True
#print('collide in placing CA')
break
if not collide:
break
human.set(px, py, -px, -py, 0, 0, 0)
return human
def generate_random_human_position(self, human_num, rule):
"""
Generate human position according to certain rule
Rule square_crossing: generate start/goal position at two sides of y-axis
Rule circle_crossing: generate start position on a circle, goal position is at the opposite side
:param human_num:
:param rule:
:return:
"""
# initial min separation distance to avoid danger penalty at beginning
if rule == 'square_crossing':
self.humans = []
for i in range(human_num):
self.humans.append(self.generate_square_crossing_human())
elif rule == 'circle_crossing':
self.humans = []
for i in range(human_num):
self.humans.append(self.generate_circle_crossing_human())
elif rule == 'lb':
self.humans = []
for i in range(human_num):
self.humans.append(self.generate_circle_crossing_human(),
lb=True)
elif rule == 'mixed':
# mix different raining simulation with certain distribution
static_human_num = {
0: 0.05,
1: 0.2,
2: 0.2,
3: 0.3,
4: 0.1,
5: 0.15
}
dynamic_human_num = {1: 0.3, 2: 0.3, 3: 0.2, 4: 0.1, 5: 0.1}
static = True if np.random.random() < 0.2 else False
prob = np.random.random()
for key, value in sorted(static_human_num.items(
) if static else dynamic_human_num.items()):
if prob - value <= 0:
human_num = key
break
else:
prob -= value
self.human_num = human_num
self.humans = []
if static:
# randomly initialize static objects in a square of (width, height)
width = 4
height = 8
if human_num == 0:
human = Human(self.config, 'humans')
human.set(0, -10, 0, -10, 0, 0, 0)
self.humans.append(human)
for i in range(human_num):
human = Human(self.config, 'humans')
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
px = np.random.random() * width * 0.5 * sign
py = (np.random.random() - 0.5) * height
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
human.set(px, py, px, py, 0, 0, 0)
self.humans.append(human)
else:
# the first 2 two humans will be in the circle crossing scenarios
# the rest humans will have a random starting and end position
for i in range(human_num):
if i < 2:
human = self.generate_circle_crossing_human()
else:
human = self.generate_square_crossing_human()
self.humans.append(human)
else:
raise ValueError("Rule doesn't exist")
def generate_random_human_position(self, human_num, rule):
"""
Generate human position according to certain rule
Rule square_crossing: generate start/goal position at two sides of y-axis
Rule circle_crossing: generate start position on a circle, goal position is at the opposite side
:param human_num:
:param rule:
:return:
"""
if self.seed:
np.random.seed(self.seed)
# initial min separation distance to avoid danger penalty at beginning
if rule == 'square_crossing':
self.humans = []
for i in range(human_num):
self.humans.append(self.generate_square_crossing_human())
elif rule == 'circle_crossing':
self.humans = []
for i in range(human_num):
self.humans.append(self.generate_circle_crossing_human())
# humans move vertically, half go up ad the other half go down
elif rule == 'cm_hall':
self.humans = []
self.generate_cmhall_humans()
# humans moce vertically, all of them go down, starting positions is random in a rectangle around
# the goal
elif rule == 'cm_hall_oneway':
self.humans = []
self.generate_cmhallow__humans()
elif rule == 'cm_hall_oneway_horizontal':
self.humans = []
self.generate_cmhallow__humans(horizontal=True)
# two lanes, one goiung down the other going up, sperated by humans
elif rule == 'lanes':
self.humans = []
self.generate_lanes_humans()
#...
elif rule == 'tri-td':
self.humans = []
self.generate_tri_humans()
elif rule == 'line-td':
self.humans = []
self.generate_ld_humans()
elif rule == 'line':
self.humans = []
self.generate_ldl_humans()
elif rule == 'mixed':
# mix different raining simulation with certain distribution
static_human_num = {
0: 0.05,
1: 0.2,
2: 0.2,
3: 0.3,
4: 0.1,
5: 0.15
}
dynamic_human_num = {1: 0.3, 2: 0.3, 3: 0.2, 4: 0.1, 5: 0.1}
static = True if np.random.random() < 0.2 else False
prob = np.random.random()
for key, value in sorted(static_human_num.items(
) if static else dynamic_human_num.items()):
if prob - value <= 0:
human_num = key
break
else:
prob -= value
self.human_num = human_num
self.humans = []
if static:
# randomly initialize static objects in a square of (width, height)
width = 4
height = 8
if human_num == 0:
human = Human(self.config, 'humans')
human.set(0, -10, 0, -10, 0, 0, 0)
self.humans.append(human)
for i in range(human_num):
human = Human(self.config, 'humans')
if np.random.random() > 0.5:
sign = -1
else:
sign = 1
while True:
px = np.random.random() * width * 0.5 * sign
py = (np.random.random() - 0.5) * height
collide = False
for agent in [self.robot] + self.humans:
if norm(
(px - agent.px, py - agent.py)
) < human.radius + agent.radius + self.discomfort_dist:
collide = True
break
if not collide:
break
human.set(px, py, px, py, 0, 0, 0)
self.humans.append(human)
else:
# the first 2 two humans will be in the circle crossing scenarios
# the rest humans will have a random starting and end position
for i in range(human_num):
if i < 2:
human = self.generate_circle_crossing_human()
else:
human = self.generate_square_crossing_human()
self.humans.append(human)
else:
raise ValueError("Rule doesn't exist")
def main():
parser = argparse.ArgumentParser('Parse configuration file')
parser.add_argument('--env_config', type=str, default='configs/env.config')
parser.add_argument('--policy', type=str, default='scr')
parser.add_argument('--policy_config',
type=str,
default='configs/policy.config')
parser.add_argument('--train_config',
type=str,
default='configs/train.config')
parser.add_argument('--output_dir', type=str, default='data/output')
parser.add_argument('--weights', type=str)
parser.add_argument('--resume', default=False, action='store_true')
parser.add_argument('--gpu', default=False, action='store_true')
parser.add_argument('--debug', default=False, action='store_true')
args = parser.parse_args()
# configure paths
make_new_dir = True
if os.path.exists(args.output_dir):
key = input(
'Output directory already exists! Overwrite the folder? (y/n)')
if key == 'y' and not args.resume:
shutil.rmtree(args.output_dir)
else:
make_new_dir = False
args.env_config = os.path.join(args.output_dir,
os.path.basename(args.env_config))
args.policy_config = os.path.join(
args.output_dir, os.path.basename(args.policy_config))
args.train_config = os.path.join(
args.output_dir, os.path.basename(args.train_config))
if make_new_dir:
os.makedirs(args.output_dir)
shutil.copy(args.env_config, args.output_dir)
shutil.copy(args.policy_config, args.output_dir)
shutil.copy(args.train_config, args.output_dir)
log_file = os.path.join(args.output_dir, 'output.log')
il_weight_file = os.path.join(args.output_dir, 'il_model.pth')
rl_weight_file = os.path.join(args.output_dir, 'rl_model.pth')
# configure logging
mode = 'a' if args.resume else 'w'
file_handler = logging.FileHandler(log_file, mode=mode)
stdout_handler = logging.StreamHandler(sys.stdout)
level = logging.INFO if not args.debug else logging.DEBUG
logging.basicConfig(level=level,
handlers=[stdout_handler, file_handler],
format='%(asctime)s, %(levelname)s: %(message)s',
datefmt="%Y-%m-%d %H:%M:%S")
repo = git.Repo(search_parent_directories=True)
logging.info('Current git head hash code: %s'.format(
repo.head.object.hexsha))
device = torch.device(
"cuda:0" if torch.cuda.is_available() and args.gpu else "cpu")
logging.info('Using device: %s', device)
# configure policy
policy = policy_factory[args.policy]()
if not policy.trainable:
parser.error('Policy has to be trainable')
if args.policy_config is None:
parser.error(
'Policy config has to be specified for a trainable network')
policy_config = configparser.RawConfigParser()
policy_config.read(args.policy_config)
policy.configure(policy_config)
policy.set_device(device)
# configure environment
env = gym.make('CrowdSim-v0')
env.configure(args.env_config)
robot = Robot()
robot.configure(args.env_config, 'robot')
env.set_robot(robot)
humans = [Human() for _ in range(env.human_num)]
for human in humans:
human.configure(args.env_config, 'humans')
env.set_humans(humans)
# read training parameters
if args.train_config is None:
parser.error(
'Train config has to be specified for a trainable network')
train_config = configparser.RawConfigParser()
train_config.read(args.train_config)
rl_learning_rate = train_config.getfloat('train', 'rl_learning_rate')
train_batches = train_config.getint('train', 'train_batches')
train_episodes = train_config.getint('train', 'train_episodes')
sample_episodes = train_config.getint('train', 'sample_episodes')
target_update_interval = train_config.getint('train',
'target_update_interval')
evaluation_interval = train_config.getint('train', 'evaluation_interval')
capacity = train_config.getint('train', 'capacity')
epsilon_start = train_config.getfloat('train', 'epsilon_start')
epsilon_end = train_config.getfloat('train', 'epsilon_end')
epsilon_decay = train_config.getfloat('train', 'epsilon_decay')
checkpoint_interval = train_config.getint('train', 'checkpoint_interval')
# configure trainer and explorer
memory = ReplayMemory(capacity)
model = policy.get_model()
batch_size = train_config.getint('trainer', 'batch_size')
trainer = Trainer(policy, memory, device, batch_size)
explorer = Explorer(env,
robot,
device,
memory,
policy.gamma,
target_policy=policy)
# imitation learning
if args.resume:
if not os.path.exists(rl_weight_file):
logging.error('RL weights does not exist')
model.load_state_dict(torch.load(rl_weight_file))
rl_weight_file = os.path.join(args.output_dir, 'resumed_rl_model.pth')
logging.info(
'Load reinforcement learning trained weights. Resume training')
elif os.path.exists(il_weight_file):
model.load_state_dict(torch.load(il_weight_file))
logging.info('Load imitation learning trained weights.')
else:
il_episodes = train_config.getint('imitation_learning', 'il_episodes')
il_policy = train_config.get('imitation_learning', 'il_policy')
il_epochs = train_config.getint('imitation_learning', 'il_epochs')
il_learning_rate = train_config.getfloat('imitation_learning',
'il_learning_rate')
trainer.set_learning_rate(il_learning_rate)
if robot.visible:
safety_space = 0
else:
safety_space = train_config.getfloat('imitation_learning',
'safety_space')
il_policy = policy_factory[il_policy]()
il_policy.multiagent_training = policy.multiagent_training
il_policy.safety_space = safety_space
robot.set_policy(il_policy)
explorer.run_k_episodes(il_episodes,
'train',
update_memory=True,
imitation_learning=True)
trainer.optimize_epoch(il_epochs)
torch.save(model.state_dict(), il_weight_file)
logging.info('Finish imitation learning. Weights saved.')
logging.info('Experience set size: %d/%d', len(memory),
memory.capacity)
explorer.update_target_model(model)
# reinforcement learning
policy.set_env(env)
robot.set_policy(policy)
robot.print_info()
trainer.set_learning_rate(rl_learning_rate)
# fill the memory pool with some RL experience
if args.resume:
robot.policy.set_epsilon(epsilon_end)
explorer.run_episode('val', video_file=f'data/output/video_e{-1}.mp4')
explorer.run_k_episodes(100, 'train', update_memory=True, episode=0)
logging.info('Experience set size: %d/%d', len(memory),
memory.capacity)
episode = 0
while episode < train_episodes:
if args.resume:
epsilon = epsilon_end
else:
if episode < epsilon_decay:
epsilon = epsilon_start + (
epsilon_end - epsilon_start) / epsilon_decay * episode
else:
epsilon = epsilon_end
robot.policy.set_epsilon(epsilon)
# sample k episodes into memory and optimize over the generated memory
explorer.run_k_episodes(sample_episodes,
'train',
update_memory=True,
episode=episode)
trainer.optimize_batch(train_batches)
# evaluate the model
if episode % evaluation_interval == 0:
explorer.run_episode(
'val', video_file=f'data/output/video_e{episode}.mp4')
episode += 1
if episode % target_update_interval == 0:
explorer.update_target_model(model)
if episode != 0 and episode % checkpoint_interval == 0:
torch.save(model.state_dict(), rl_weight_file)
# final test
explorer.run_k_episodes(env.case_size['test'], 'test', episode=episode)
def main():
parser = argparse.ArgumentParser('Parse configuration file')
parser.add_argument('--env_config', type=str, default='configs/env.config')
parser.add_argument('--policy_config',
type=str,
default='configs/policy.config')
parser.add_argument('--policy', type=str, default='orca')
parser.add_argument('--model_dir', type=str, default=None)
parser.add_argument('--il', default=False, action='store_true')
parser.add_argument('--gpu', default=False, action='store_true')
parser.add_argument('--visualize', default=True, action='store_true')
parser.add_argument('--phase', type=str, default='test')
parser.add_argument('--test_case', type=int, default=0)
parser.add_argument('--square', default=False, action='store_true')
parser.add_argument('--circle', default=False, action='store_true')
parser.add_argument('--video_file', type=str, default=None)
parser.add_argument('--traj', default=False, action='store_true')
parser.add_argument('--plot', default=False, action='store_true')
args = parser.parse_args()
if args.model_dir is not None:
env_config_file = os.path.join(args.model_dir,
os.path.basename(args.env_config))
policy_config_file = os.path.join(args.model_dir,
os.path.basename(args.policy_config))
if args.il:
model_weights = os.path.join(args.model_dir, 'il_model.pth')
else:
if os.path.exists(
os.path.join(args.model_dir, 'resumed_rl_model.pth')):
model_weights = os.path.join(args.model_dir,
'resumed_rl_model.pth')
else:
model_weights = os.path.join(args.model_dir, 'rl_model.pth')
else:
env_config_file = args.env_config
policy_config_file = args.env_config
# configure logging and device
logging.basicConfig(level=logging.INFO,
format='%(asctime)s, %(levelname)s: %(message)s',
datefmt="%Y-%m-%d %H:%M:%S")
device = torch.device(
"cuda:0" if torch.cuda.is_available() and args.gpu else "cpu")
logging.info('Using device: %s', device)
# configure policy
policy = policy_factory[args.policy]()
policy_config = configparser.RawConfigParser()
policy_config.read(policy_config_file)
policy.configure(policy_config)
if policy.trainable:
if args.model_dir is None:
parser.error(
'Trainable policy must be specified with a model weights directory'
)
policy.get_model().load_state_dict(
torch.load(model_weights, map_location=torch.device('cpu')))
# configure environment
env_config = configparser.RawConfigParser()
env_config.read(env_config_file)
env = gym.make('CrowdSim-v0')
env.configure(env_config)
if args.square:
env.test_sim = 'square_crossing'
if args.circle:
env.test_sim = 'circle_crossing'
robot = Robot(env_config, 'robot')
robot.set_policy(policy)
env.set_robot(robot)
explorer = Explorer(env, robot, device, gamma=0.9)
policy.set_phase(args.phase)
policy.set_device(device)
# set safety space for ORCA in non-cooperative simulation
if isinstance(robot.policy, ORCA):
if robot.visible:
robot.policy.safety_space = 0
else:
# because invisible case breaks the reciprocal assumption
# adding some safety space improves ORCA performance. Tune this value based on your need.
robot.policy.safety_space = 0
logging.info('ORCA agent buffer: %f', robot.policy.safety_space)
policy.set_env(env)
robot.print_info()
if args.visualize:
obs = env.reset(args.phase, args.test_case)
done = False
last_pos = np.array(robot.get_position())
policy_time = 0.0
numFrame = 0
dist = 20.0
obs_flg = 0
sim_time = False
while sim_time == False:
samples, robot_state, sim_time = pc2obs.pc2obs(
voxel_size=voxel_size)
t1 = float(sim_time)
while (dist > 0.8):
#t1 = time.time()
env.humans = []
samples, robot_state, sim_time = pc2obs.pc2obs(
voxel_size=voxel_size)
if type(samples) == type(False):
print("Not Connect")
continue
dist = math.sqrt((GOAL_X - robot_state[0])**2 +
(GOAL_Y - robot_state[1])**2)
if obs_flg == 0 and dist < 10:
os.system('sh ./init.sh')
obs_flg = 1
print("dist: {}".format(dist))
# rotate and shift obs position
t2 = time.time()
yaw = robot_state[2]
rot_matrix = np.array([[math.cos(yaw),
math.sin(yaw)],
[-math.sin(yaw),
math.cos(yaw)]])
#samples = np.array([sample + robot_state[0:2] for sample in samples[:,0:2]])
if len(samples) == 1:
samples = np.array(
[np.dot(rot_matrix, samples[0][0:2]) + robot_state[0:2]])
print(samples)
elif len(samples) > 1:
samples = np.array([
np.dot(rot_matrix, sample) + robot_state[0:2]
for sample in samples[:, 0:2]
])
obs_position_list = samples
obs = []
for ob in obs_position_list:
human = Human(env.config, 'humans')
# px, py, gx, gy, vx, vy, theta
human.set(ob[0], ob[1], ob[0], ob[1], 0, 0, 0)
env.humans.append(human)
# px, py, vx, vy, radius
obs.append(ObservableState(ob[0], ob[1], 0, 0, voxel_size / 2))
if len(obs_position_list) == 0:
human = Human(env.config, 'humans')
# SARL, CADRL
human.set(0, -10, 0, -10, 0, 0, 0)
# LSTM
#human.set(robot_state[0]+10, robot_state[1]+10, robot_state[0]+10, robot_state[1]+10 , 0, 0, 0)
env.humans.append(human)
# SARL, CADRL
obs.append(ObservableState(0, -10, 0, 0, voxel_size / 2))
# LSTM
#obs.append(ObservableState(robot_state[0]+10, robot_state[1]+10, 0, 0, voxel_size/2))
robot.set_position(robot_state)
robot.set_velocity([math.sin(yaw), math.cos(yaw)])
#print(obs)
action = robot.act(obs)
obs, _, done, info = env.step(action)
current_pos = np.array(robot.get_position())
current_vel = np.array(robot.get_velocity())
#print("Velocity: {}, {}".format(current_vel[0], current_vel[1]))
logging.debug(
'Speed: %.2f',
np.linalg.norm(current_pos - last_pos) / robot.time_step)
last_pos = current_pos
policy_time += time.time() - t1
numFrame += 1
#print(t2-t1, time.time() - t2)
diff_angle = (-yaw + math.atan2(current_vel[0], current_vel[1]))
if diff_angle > math.pi:
diff_angle = diff_angle - 2 * math.pi
elif diff_angle < -math.pi:
diff_angle = diff_angle + 2 * math.pi
#print("diff_angle: {}, {}, {}".format(diff_angle, yaw ,-math.atan2(current_vel[0], current_vel[1])))
if diff_angle < -0.7:
direc = 2 # turn left
elif diff_angle > 0.7:
direc = 3 # turn right
else:
direc = 1 # go straight
# GoEasy(direc)
vx = SPEED * math.sqrt(current_vel[0]**2 + current_vel[1]**2)
if diff_angle > 0:
v_ang = ANGULAR_SPEED * min(diff_angle / (math.pi / 2), 1)
else:
v_ang = ANGULAR_SPEED * max(diff_angle / (math.pi / 2), -1)
print(vx, -v_ang)
easyGo.mvCurve(vx, -v_ang)
if args.plot:
plt.scatter(current_pos[0], current_pos[1], label='robot')
plt.arrow(current_pos[0],
current_pos[1],
current_vel[0],
current_vel[1],
width=0.05,
fc='g',
ec='g')
plt.arrow(current_pos[0],
current_pos[1],
math.sin(yaw),
math.cos(yaw),
width=0.05,
fc='r',
ec='r')
if len(samples) == 1:
plt.scatter(samples[0][0],
samples[0][1],
label='obstacles')
elif len(samples) > 1:
plt.scatter(samples[:, 0],
samples[:, 1],
label='obstacles')
plt.xlim(-6.5, 6.5)
plt.ylim(-9, 4)
plt.legend()
plt.title("gazebo test")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.pause(0.001)
plt.cla()
plt.clf()
print("NAV TIME {}".format(float(sim_time) - t1))
time.sleep(0.5)
print("NAV TIME {}".format(float(sim_time) - t1))
easyGo.stop()
plt.close()
print("Average took {} sec per iteration, {} Frame".format(
policy_time / numFrame, numFrame))
else:
explorer.run_k_episodes(env.case_size[args.phase],
args.phase,
print_failure=True)
def reset(self, phase='test', test_case=None):
"""
Set px, py, gx, gy, vx, vy, theta for robot and humans
:return:
"""
if self.robot is None:
raise AttributeError('robot has to be set!')
assert phase in ['train', 'val', 'test']
if test_case is not None:
self.case_counter[phase] = test_case
self.global_time = 0
if phase == 'test':
self.human_times = [0] * self.human_num
else:
self.human_times = [0] * (
self.human_num if self.robot.policy.multiagent_training else 1)
#if not self.robot.policy.multiagent_training:
# self.train_val_sim = 'circle_crossing'
if self.config.get('humans', 'policy') == 'trajnet':
raise NotImplementedError
else:
counter_offset = {
'train':
self.case_capacity['val'] + self.case_capacity['test'],
'val': 0,
'test': self.case_capacity['val']
}
self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0,
np.pi / 2)
if self.case_counter[phase] >= 0:
#np.random.seed(counter_offset[phase] + self.case_counter[phase])
if phase in ['train', 'val']:
human_num = self.human_num if self.robot.policy.multiagent_training else 1
self.generate_random_human_position(
human_num=human_num, rule=self.train_val_sim)
else:
self.generate_random_human_position(
human_num=self.human_num, rule=self.test_sim)
# case_counter is always between 0 and case_size[phase]
self.case_counter[phase] = (self.case_counter[phase] +
1) % self.case_size[phase]
else:
assert phase == 'test'
if self.case_counter[phase] == -1:
# for debugging purposes
self.human_num = 3
self.humans = [
Human(self.config, 'humans')
for _ in range(self.human_num)
]
self.humans[0].set(0, -6, 0, 5, 0, 0, np.pi / 2)
self.humans[1].set(-5, -5, -5, 5, 0, 0, np.pi / 2)
self.humans[2].set(5, -5, 5, 5, 0, 0, np.pi / 2)
else:
raise NotImplementedError
###
if self.domain_settings is not None:
for human in self.humans:
human.modify_policy(self.domain_settings)
for agent in [self.robot] + self.humans:
agent.time_step = self.time_step
agent.policy.time_step = self.time_step
self.states = list()
if hasattr(self.robot.policy, 'action_values'):
self.action_values = list()
if hasattr(self.robot.policy, 'get_attention_weights'):
self.attention_weights = list()
# get current observation
if self.robot.sensor == 'coordinates':
ob = [human.get_observable_state() for human in self.humans]
elif self.robot.sensor == 'RGB':
raise NotImplementedError
self.simulator = socialforce.Simulator(
self.humans_to_sf_state(self.humans, init=True),
ped_ped=socialforce.PedPedPotential(self.delta_t, self.v0,
self.sigma),
delta_t=self.delta_t,
tau=self.tau)
return ob
def reset(self, phase='train'):
"""
Set px, py, gx, gy, vx, vy, theta for robot and humans
:return:
"""
if self.robot is None:
raise AttributeError('robot has to be set!')
assert phase in ['train', 'val', 'test']
self.global_time = 0
if phase == 'test':
self.human_times = [0] * self.human_num
else:
self.human_times = [0] * (
self.human_num if self.robot.policy.multiagent_training else 1)
if not self.robot.policy.multiagent_training:
self.train_val_sim = 'circle_crossing'
if self.config.get('humans', 'policy') == 'trajnet':
raise NotImplementedError
else:
counter_offset = {
'train':
self.case_capacity['val'] + self.case_capacity['test'],
'val': 0,
'test': self.case_capacity['val']
}
###self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0, np.pi / 2)
self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0,
0)
if self.case_counter[phase] >= 0:
np.random.seed()
if phase in ['train', 'val']:
human_num = self.human_num if self.robot.policy.multiagent_training else 1
self.generate_random_human_position(
human_num=human_num, rule=self.train_val_sim)
else:
self.generate_random_human_position(
human_num=self.human_num, rule=self.test_sim)
# case_counter is always between 0 and case_size[phase]
self.case_counter[phase] = (self.case_counter[phase] +
1) % self.case_size[phase]
else:
assert phase == 'test'
if self.case_counter[phase] == -1:
# for debugging purposes
self.human_num = 3
self.humans = [
Human(self.config, 'humans')
for _ in range(self.human_num)
]
self.humans[0].set(0, -6, 0, 5, 0, 0, np.pi / 2)
self.humans[1].set(-5, -5, -5, 5, 0, 0, np.pi / 2)
self.humans[2].set(5, -5, 5, 5, 0, 0, np.pi / 2)
else:
raise NotImplementedError
for agent in [self.robot] + self.humans:
agent.time_step = self.time_step
agent.policy.time_step = self.time_step
self.states = list()
if hasattr(self.robot.policy, 'action_values'):
self.action_values = list()
if hasattr(self.robot.policy, 'get_attention_weights'):
self.attention_weights = list()
# get current observation
if self.robot.sensor == 'coordinates':
ob = [
self.robot.get_full_state() + human.get_observable_state()
for human in self.humans
]
elif self.robot.sensor == 'RGB':
raise NotImplementedError
return ob
def state_human(state):
human = Human(self.config, 'humans')
human.set(state[0], state[1], state[4], state[5], state[2],
state[3], 0)
return human