add UWB detection

crowd_nav/configs/env.config

@@ -14,11 +14,11 @@ discomfort_penalty_factor = 0.5
 
 
 [sim]
-train_val_sim = circle_crossing
-test_sim = circle_crossing
+train_val_sim = static_3_dy
+test_sim = static_3_dy
 square_width = 10
 circle_radius = 4
-human_num = 5
+human_num = 6
 
 
 [humans]

crowd_nav/configs/env.config.bak

@@ -0,0 +1,37 @@
+[env]
+time_limit = 25
+time_step = 0.25
+val_size = 100
+test_size = 500
+randomize_attributes = false
+
+
+[reward]
+success_reward = 1
+collision_penalty = -0.25
+discomfort_dist = 0.2
+discomfort_penalty_factor = 0.5
+
+
+[sim]
+train_val_sim = circle_crossing
+test_sim = circle_crossing
+square_width = 10
+circle_radius = 4
+human_num = 5
+
+
+[humans]
+visible = true
+policy = orca
+radius = 0.3
+v_pref = 1
+sensor = coordinates
+
+
+[robot]
+visible = false
+policy = none
+radius = 0.3
+v_pref = 1
+sensor = coordinates

crowd_nav/configs/policy.config

@@ -11,7 +11,7 @@ om_channel_size = 3
 
 
 [action_space]
-kinematics = holonomic
+kinematics = unicycle
 # action space size is speed_samples * rotation_samples + 1
 speed_samples = 5
 rotation_samples = 16

crowd_nav/configs/policy.config.bak

@@ -0,0 +1,50 @@
+# policy configurations for robot
+
+[rl]
+gamma = 0.9
+
+
+[om]
+cell_num = 4
+cell_size = 1
+om_channel_size = 3
+
+
+[action_space]
+kinematics = holonomic
+# action space size is speed_samples * rotation_samples + 1
+speed_samples = 5
+rotation_samples = 16
+sampling = exponential
+query_env = true
+
+
+[cadrl]
+mlp_dims = 150, 100, 100, 1
+multiagent_training = false
+
+
+[lstm_rl]
+global_state_dim = 50
+mlp1_dims = 150, 100, 100, 50
+mlp2_dims = 150, 100, 100, 1
+multiagent_training = true
+with_om = false
+with_interaction_module = false
+
+
+[srl]
+mlp1_dims = 150, 100, 100, 50
+mlp2_dims = 150, 100, 100, 1
+multiagent_training = true
+with_om = false
+
+
+[sarl]
+mlp1_dims = 150, 100
+mlp2_dims = 100, 50
+attention_dims = 100, 100, 1
+mlp3_dims = 150, 100, 100, 1
+multiagent_training = true
+with_om = false
+with_global_state = true

crowd_nav/policy/cadrl.py

@@ -88,7 +88,7 @@ def build_action_space(self, v_pref):
  if holonomic:
  rotations = np.linspace(0, 2 * np.pi, self.rotation_samples, endpoint=False)
  else:
- rotations = np.linspace(-np.pi / 4, np.pi / 4, self.rotation_samples)
+ rotations = np.linspace(-np.pi / 2, np.pi / 2, self.rotation_samples)
 
  action_space = [ActionXY(0, 0) if holonomic else ActionRot(0, 0)]
  for rotation, speed in itertools.product(rotations, speeds):

crowd_nav/train.py

@@ -54,8 +54,8 @@ def main():
  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))
+ # 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)
 

crowd_sim/envs/crowd_sim.py

@@ -8,7 +8,10 @@
 from crowd_sim.envs.utils.human import Human
 from crowd_sim.envs.utils.info import *
 from crowd_sim.envs.utils.utils import point_to_segment_dist
-
+from crowd_sim.envs.utils.state import ObservableState
+#for intersection detection
+from sympy import *
+from sympy.geometry import *
 
 class CrowdSim(gym.Env):
  metadata = {'render.modes': ['human']}
@@ -21,6 +24,9 @@ def __init__(self):
  robot is controlled by a known and learnable policy.
 
  """
+ #for block analysis
+ self.block_list = None
+
  self.time_limit = None
  self.time_step = None
  self.robot = None
@@ -149,6 +155,31 @@ def generate_random_human_position(self, human_num, rule):
  else:
  human = self.generate_square_crossing_human()
  self.humans.append(human)
+ elif rule == 'static_3_dy':
+ self.humans = []
+ for i in range(human_num):
+ if i < 3:
+ width = 4
+ height = 6
+ 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:
+ self.humans.append(self.generate_circle_crossing_human())
  else:
  raise ValueError("Rule doesn't exist")
 
@@ -291,7 +322,7 @@ def reset(self, phase='test', test_case=None):
  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
+ raise NotImplementedError 
 
  for agent in [self.robot] + self.humans:
  agent.time_step = self.time_step
@@ -308,12 +339,82 @@ def reset(self, phase='test', test_case=None):
  ob = [human.get_observable_state() for human in self.humans]
  elif self.robot.sensor == 'RGB':
  raise NotImplementedError
+ elif self.robot.sensor == 'UWB':
+ ob = self.cal_uwb_obs()
 
  return ob
 
+ def cal_uwb_obs(self):
+ obs_list = []
+ human_index = 0
+ for human in self.humans:
+ if human_index >= 3:
+ obs_list.append(human.get_observable_state())
+ uwb_ob = self.block_detection(human)
+ if(uwb_ob is not None):
+ for i in range(len(uwb_ob)):
+ obs = self.cal_obs_pos(human,uwb_ob[i])
+ obs_list.append(obs)
+ human_index += 1
+ robot_obs = self.block_detection(self.robot)
+ if robot_obs is not None:
+ for i in range(len(robot_obs)):
+ obs_list.append(self.cal_obs_pos(self.robot,robot_obs[i])) 
+ return obs_list
+
+ def cal_next_uwb_obs(self):
+ obs_list = []
+ human_index = 0
+ for human,action in zip(self.humans, human_actions):
+ ob = human.get_next_observable_state(action)
+ if human_index >= 3:
+ obs_list.append(human.get_observable_state())
+ uwb_ob = self.block_detection(human)
+ if(uwb_ob is not None):
+ for i in range(len(uwb_ob)):
+ obs = self.cal_obs_pos(human,uwb_ob[i])
+ obs_list.append(obs)
+ human_index += 1
+ robot_obs = self.block_detection(self.robot)
+ if robot_obs is not None:
+ for i in range(len(robot_obs)):
+ obs_list.append(self.cal_obs_pos(self.robot,robot_obs[i])) 
+ return obs_list
+
+ def cal_obs_pos(self, input_rob, input_anchor):
+ anch = [Point(-4.5,-4),Point(-4.5,4),Point(4.5,4),Point(4.5,-4)]
+ rob = Circle([input_rob.px,input_rob.py],1.5)#position x,y
+ line = Segment(Point(input_rob.px,input_rob.py),anch[input_anchor])
+ block = intersection(rob,line)
+ obs = ObservableState(float(block[0].coordinates[0]),float(block[0].coordinates[1]),0,0,0.25)
+ return obs
+
  def onestep_lookahead(self, action):
  return self.step(action, update=False)
 
+ def block_detection(self,robot_in):
+ anch = [Point(-4.5,-4),Point(-4.5,4),Point(4.5,4),Point(4.5,-4)]
+ rob = Point(robot_in.px,robot_in.py,evaluate = False)#position x,y 
+ obs = []
+ block = []
+ human_index = 0
+ ob = []
+ for human in self.humans:
+ if human_index < 3:
+ obs.append(Circle([human.px,human.py],0.25))
+ human_index += 1 
+ for obstacle in obs:
+ if(rob.distance(obstacle.center)<1.5):
+ for ind in range(4):
+ anchor = anch[ind]
+ line = Segment(rob,anchor)
+ block = intersection(obstacle,line)
+ if(len(block)>=2):
+ flag = ind
+ if flag not in ob:
+ ob.append(flag)#remove duplicate
+ return ob
+
  def step(self, action, update=True):
  """
  Compute actions for all agents, detect collision, update environment and return (ob, reward, done, info)
@@ -330,7 +431,7 @@ def step(self, action, update=True):
  # collision detection
  dmin = float('inf')
  collision = False
- for i, human in enumerate(self.humans):
+ for i, human in enumerate(self.humans): 
  px = human.px - self.robot.px
  py = human.py - self.robot.py
  if self.robot.kinematics == 'holonomic':
@@ -395,6 +496,7 @@ def step(self, action, update=True):
  self.action_values.append(self.robot.policy.action_values)
  if hasattr(self.robot.policy, 'get_attention_weights'):
  self.attention_weights.append(self.robot.policy.get_attention_weights())
+ # print(self.attention_weights)
 
  # update all agents
  self.robot.step(action)
@@ -409,11 +511,15 @@ def step(self, action, update=True):
  # compute the observation
  if self.robot.sensor == 'coordinates':
  ob = [human.get_observable_state() for human in self.humans]
+ elif self.robot.sensor == 'UWB':
+ ob = self.cal_uwb_obs()
  elif self.robot.sensor == 'RGB':
  raise NotImplementedError
  else:
  if self.robot.sensor == 'coordinates':
  ob = [human.get_next_observable_state(action) for human, action in zip(self.humans, human_actions)]
+ elif self.robot.sensor == 'UWB':
+ ob = self.cal_next_uwb_obs()
  elif self.robot.sensor == 'RGB':
  raise NotImplementedError
 
@@ -514,6 +620,7 @@ def render(self, mode='human', output_file=None):
 
  # compute attention scores
  if self.attention_weights is not None:
+ # print(self.attention_weights[0])
  attention_scores = [
  plt.text(-5.5, 5 - 0.5 * i, 'Human {}: {:.2f}'.format(i + 1, self.attention_weights[0][i]),
  fontsize=16) for i in range(len(self.humans))]

crowd_sim/envs/utils/human.py

@@ -15,3 +15,13 @@ def act(self, ob):
  state = JointState(self.get_full_state(), ob)
  action = self.policy.predict(state)
  return action
+
+ def act_static(self, ob):
+ """
+ The state for human is its full state and all other agents' observable states
+ :param ob:
+ :return:
+ """
+ state = JointState(self.get_full_state(), ob)
+ action = ActionXY(0, 0)
+ return action