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