├── .gitignore ├── APF.py ├── README.md ├── marinenav_env ├── __init__.py ├── envs │ ├── __init__.py │ ├── marinenav_env.py │ └── utils │ │ └── robot.py └── setup.py ├── nav ├── BSP.py ├── EM.py ├── exp_max.py ├── frontier.py ├── navigation.py ├── utils.py └── virtualmap.py ├── real_pipeline.jpeg ├── scripts └── test │ └── test_multi_SLAM.py └── system_requirements /.gitignore: -------------------------------------------------------------------------------- 1 | training* 2 | experiment* 3 | __pycache__ 4 | marine_env/marine_env.egg-info/ 5 | config/ 6 | *.png 7 | *.json 8 | *.txt 9 | scripts/* 10 | -------------------------------------------------------------------------------- /APF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import copy 3 | from nav.utils import from_cos_sin, theta_0_to_2pi 4 | 5 | class APF_agent: 6 | 7 | def __init__(self, a, w): 8 | self.k_att = 50.0 # attractive force constant 9 | self.k_rep = 50.0 # repulsive force constant 10 | self.m = 500 # robot weight (kg) 11 | self.d0 = 10.0 # obstacle distance threshold (m) 12 | self.n = 2 # power constant of repulsive force 13 | self.min_vel = 0.5 # if velocity is lower than the threshold, mandate acceleration 14 | 15 | self.a = a # available linear acceleration (action 1) 16 | self.w = w # available angular velocity (action 2) 17 | 18 | def act1(self, observation): 19 | if observation is None: 20 | return 0 21 | self_state,static_states,dynamic_states = observation 22 | # print(self_state,static_states,dynamic_states,idx_array) 23 | velocity = np.array(self_state[2:4]) 24 | goal = np.array(self_state[:2]) 25 | dd = np.sqrt(goal[0] ** 2 + goal[1] ** 2) 26 | angle_w = from_cos_sin(goal[0]/dd, goal[1]/dd) 27 | return angle_w 28 | 29 | def act(self, observation): 30 | if observation is None: 31 | return 0 32 | self_state,static_states,dynamic_states = observation 33 | # print(self_state,static_states,dynamic_states,idx_array) 34 | velocity = np.array(self_state[2:4]) 35 | goal = np.array(self_state[:2]) 36 | dd = np.sqrt(goal[0] ** 2 + goal[1] ** 2) 37 | angle_w = from_cos_sin(goal[0]/dd, goal[1]/dd) 38 | if angle_w > np.pi: 39 | angle_w = angle_w - 2 * np.pi 40 | 41 | # if angle_w > (180-30)/180 * np.pi: 42 | w_idx = np.argmin(abs(angle_w-self.w)) 43 | a = copy.deepcopy(self.a) 44 | a[a<=0.0] = -np.inf 45 | a_idx = np.argmin(np.abs(a)) 46 | return a_idx * len(self.w) + w_idx 47 | 48 | # sonar_points = observation[4:] 49 | 50 | # compute attractive force 51 | F_att = self.k_att * goal 52 | 53 | # compute total repulsive force from sonar reflections 54 | F_rep = np.zeros(2) 55 | d_goal = np.linalg.norm(goal) 56 | # for i in range(0,len(sonar_points),2): 57 | # x = sonar_points[i] 58 | # y = sonar_points[i+1] 59 | # if x == 0 and y == 0: 60 | # continue 61 | for obs in static_states: 62 | obs = np.array(obs) 63 | d_obs = np.linalg.norm(obs[:2]) 64 | # d_obs = np.linalg.norm(sonar_points[i:i+2]) 65 | 66 | # repulsive force component to move away from the obstacle 67 | mag_1 = self.k_rep * ((1/d_obs)-(1/self.d0)) * (d_goal ** self.n)/(d_obs ** 2) 68 | # dir_1 = -1.0 * sonar_points[i:i+2] / d_obs 69 | dir_1 = -1.0 * obs[:2] / d_obs 70 | F_rep_1 = mag_1 * dir_1 71 | 72 | # repulsive force component to move towards the goal 73 | mag_2 = (self.n / 2) * self.k_rep * (((1/d_obs)-(1/self.d0))**2) * (d_goal ** (self.n-1)) 74 | dir_2 = -1.0 * goal / d_goal 75 | F_rep_2 = mag_2 * dir_2 76 | 77 | F_rep += (F_rep_1 + F_rep_2) 78 | 79 | # select angular velocity action 80 | F_total = F_att + F_rep 81 | V_angle = 0.0 82 | if np.linalg.norm(velocity) > 1e-03: 83 | V_angle = np.arctan2(velocity[1],velocity[0]) 84 | F_angle = np.arctan2(F_total[1],F_total[0]) 85 | 86 | diff_angle = F_angle - V_angle 87 | while diff_angle < -np.pi: 88 | diff_angle += 2 * np.pi 89 | while diff_angle >= np.pi: 90 | diff_angle -= 2 * np.pi 91 | 92 | w_idx = np.argmin(np.abs(self.w-diff_angle)) 93 | 94 | # select linear acceleration action 95 | a_total = F_total / self.m 96 | V_dir = np.array([1.0,0.0]) 97 | if np.linalg.norm(velocity) > 1e-03: 98 | V_dir = velocity / np.linalg.norm(velocity) 99 | a_proj = np.dot(a_total,V_dir) 100 | 101 | a = copy.deepcopy(self.a) 102 | if np.linalg.norm(velocity) < self.min_vel: 103 | # if the velocity is small, mandate acceleration 104 | a[a<=0.0] = -np.inf 105 | a_diff = a-a_proj 106 | a_idx = np.argmin(np.abs(a_diff)) 107 | 108 | return a_idx * len(self.w) + w_idx 109 | 110 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi Robot EM Exploration 2 | **This is the implementation of our autonomous exploration algorithm designed for decentralized multi-robot teams, which takes into account map and localization uncertainties of range-sensing 3 | mobile robots. Virtual landmarks are used to quantify the combined impact of process noise and sensor noise on map uncertainty. Additionally, we employ an iterative expectation-maximization inspired algorithm to assess the potential outcomes of both a local robot’s and its neighbors’ next-step actions.** 4 |

5 | 6 | ## 7 | - Here we provide a 2D example. 8 | - The Environment and local motion planner used in this repo is developed from our IROS 2023 paper [here](https://github.com/RobustFieldAutonomyLab/Distributional_RL_Navigation). 9 | ``` 10 | ├── Multi-Robot-EM-Exploration 11 | ├── marinenav_env # IROS 2023 Environment 12 | ├── nav # Exploration and SLAM code 13 | ├── scripts/test 14 | │ └── test_multi_SALM.py # 2D example 15 | └── ... 16 | ``` 17 | # Dependencies 18 | - numpy 19 | - scipy 20 | - gtsam 21 | - tqdm 22 | - matplotlib 23 | 24 | ## How to use 25 | ```bash 26 | python3 scripts/test/test_multi_SALM.py 27 | ``` 28 | -------------------------------------------------------------------------------- /marinenav_env/__init__.py: -------------------------------------------------------------------------------- 1 | # from gym.envs.registration import register 2 | # 3 | # register( 4 | # id='marinenav_env-v0', 5 | # entry_point='marinenav_env.envs:MarineNavEnv2', 6 | # ) -------------------------------------------------------------------------------- /marinenav_env/envs/__init__.py: -------------------------------------------------------------------------------- 1 | from marinenav_env.envs.marinenav_env import MarineNavEnv2 -------------------------------------------------------------------------------- /marinenav_env/envs/marinenav_env.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.spatial 3 | import marinenav_env.envs.utils.robot as robot 4 | # import gym 5 | import json 6 | import copy 7 | 8 | 9 | class Core: 10 | 11 | def __init__(self, x: float, y: float, clockwise: bool, Gamma: float): 12 | self.x = x # x coordinate of the vortex core 13 | self.y = y # y coordinate of the vortex core 14 | self.clockwise = clockwise # if the vortex direction is clockwise 15 | self.Gamma = Gamma # circulation strength of the vortex core 16 | 17 | 18 | class Obstacle: 19 | 20 | def __init__(self, x: float, y: float, r: float): 21 | self.x = x # x coordinate of the obstacle center 22 | self.y = y # y coordinate of the obstacle center 23 | self.r = r # radius of the obstacle 24 | 25 | 26 | # class MarineNavEnv2(gym.Env): 27 | class MarineNavEnv2: 28 | def __init__(self, seed: int = 0, schedule: dict = None, params: dict = None): 29 | 30 | self.sd = seed 31 | self.rd = np.random.RandomState(seed) # PRNG 32 | 33 | # Define action space and observation space for gym 34 | # self.action_space = gym.spaces.Discrete(self.robot.compute_actions_dimension()) 35 | 36 | # parameter initialization 37 | self.width = params["width"] # x coordinate dimension of the map 38 | self.height = params["height"] # y coordinate dimension of the map 39 | self.r = 0.5 # radius of vortex core 40 | self.v_rel_max = 1.0 # max allowable speed when two currents flowing towards each other 41 | self.p = 0.8 # max allowable relative speed at another vortex core 42 | self.v_range = [5, 10] # speed range of the vortex (at the edge of core) 43 | self.obs_r_range = [1, 1] # radius range of the obstacle 44 | self.clear_r = 2.0 # radius of area centered at the start(goal) of each robot, 45 | # where no vortex cores, static obstacles, or the start(goal) of other robots exist 46 | self.reset_start_and_goal = True # if the start and goal position be set randomly in reset() 47 | self.start = np.array([5.0, 5.0]) # robot start position 48 | self.goal = np.array([45.0, 45.0]) # goal position 49 | self.goal_dis = 2.0 # max distance to goal considered as reached 50 | self.timestep_penalty = -1.0 51 | self.collision_penalty = -50.0 52 | self.goal_reward = 100.0 53 | self.discount = 0.99 54 | self.num_cores = 0 # number of vortices 55 | self.num_obs = params["num_obs"] # number of static obstacles 56 | self.min_start_goal_dis = 5.0 57 | self.num_cooperative = params["num_cooperative"] # number of cooperative robots 58 | self.num_non_cooperative = 0 # number of non-cooperative robots 59 | self.sensor_range = params["sensor_range"] 60 | self.obs_radius = min(self.width, self.height) / 10 61 | self.robots = [] # list of robots 62 | for _ in range(self.num_cooperative): 63 | self.robots.append(robot.Robot(sensor_range=params["sensor_range"], cooperative=True)) 64 | for _ in range(self.num_non_cooperative): 65 | self.robots.append(robot.Robot(sensor_range=params["sensor_range"], cooperative=False)) 66 | 67 | self.cores = [] # list of vortex cores 68 | self.obstacles = [] # list of static obstacles 69 | 70 | self.schedule = schedule # schedule for curriculum learning 71 | self.episode_timesteps = 0 # current episode timesteps 72 | self.total_timesteps = 0 # learning timesteps 73 | 74 | self.set_boundary = False # set boundary of environment 75 | 76 | def load_map(self, path): 77 | data = np.loadtxt(path, delimiter=',') 78 | self.num_obs = data.shape[0] 79 | self.obstacles = [] 80 | for i in range(data.shape[0]): 81 | self.obstacles.append(Obstacle(data[i][0], data[i][1], 2)) 82 | 83 | def get_action_space_dimension(self): 84 | return self.robot.compute_actions_dimension() 85 | 86 | def reset(self, start_center): 87 | # reset the environment 88 | 89 | if self.schedule is not None: 90 | steps = self.schedule["timesteps"] 91 | diffs = np.array(steps) - self.total_timesteps 92 | 93 | # find the interval the current timestep falls into 94 | idx = len(diffs[diffs <= 0]) - 1 95 | 96 | self.num_cores = self.schedule["num_cores"][idx] 97 | self.num_obs = self.schedule["num_obstacles"][idx] 98 | self.min_start_goal_dis = self.schedule["min_start_goal_dis"][idx] 99 | 100 | print("======== training env setup ========") 101 | print("num of cores: ", self.num_cores) 102 | print("num of obstacles: ", self.num_obs) 103 | print("min start goal dis: ", self.min_start_goal_dis) 104 | print("======== training env setup ========\n") 105 | 106 | self.episode_timesteps = 0 107 | 108 | self.cores.clear() 109 | self.obstacles.clear() 110 | self.robots.clear() 111 | 112 | num_cores = self.num_cores 113 | num_obs = self.num_obs 114 | robot_types = [True] * self.num_cooperative + [False] * self.num_non_cooperative 115 | assert len(robot_types) > 0, "Number of robots is 0!" 116 | 117 | ##### generate robots with randomly generated start and goal 118 | num_robots = 0 119 | iteration = 1000 120 | # start_center = np.array([self.width-10, self.height/2]) 121 | # start_center = np.array([20, self.height/2]) 122 | # start_center = self.rd.uniform(low=5.0 * np.ones(2), high=np.array([self.width - 5.0, self.height - 5.0])) 123 | # goal_center = self.rd.uniform(low=5.0 * np.ones(2), high=np.array([self.width - 5.0, self.height - 5.0])) 124 | # start_centers = [[10,10], [self.width-10, 10], [self.width-10, self.height-10], [10, self.height-10]] 125 | while True: 126 | start = self.rd.uniform(low=start_center - np.array([5.0, 5.0]), 127 | high=start_center + np.array([5.0, 5.0])) 128 | # goal = self.rd.uniform(low=goal_center - np.array([5.0, 5.0]), high=goal_center + np.array([5.0, 5.0])) 129 | # start = self.rd.uniform(low=5.0 * np.ones(2), high=np.array([self.width - 5.0, self.height - 5.0])) 130 | goal = self.rd.uniform(low=start - np.array([4.0, 4.0]), high=start) 131 | iteration -= 1 132 | if self.check_start_and_goal(start, goal): 133 | rob = robot.Robot(self.sensor_range, robot_types[num_robots]) 134 | rob.start = start 135 | rob.goal = goal 136 | self.reset_robot(rob) 137 | self.robots.append(rob) 138 | num_robots += 1 139 | if iteration == 0 or num_robots == len(robot_types): 140 | break 141 | 142 | ##### generate vortex with random position, spinning direction and strength 143 | if num_cores > 0: 144 | iteration = 500 145 | while True: 146 | center = self.rd.uniform(low=np.zeros(2), high=np.array([self.width, self.height])) 147 | direction = self.rd.binomial(1, 0.5) 148 | v_edge = self.rd.uniform(low=self.v_range[0], high=self.v_range[1]) 149 | Gamma = 2 * np.pi * self.r * v_edge 150 | core = Core(center[0], center[1], direction, Gamma) 151 | iteration -= 1 152 | if self.check_core(core): 153 | self.cores.append(core) 154 | num_cores -= 1 155 | if iteration == 0 or num_cores == 0: 156 | break 157 | 158 | centers = None 159 | for core in self.cores: 160 | if centers is None: 161 | centers = np.array([[core.x, core.y]]) 162 | else: 163 | c = np.array([[core.x, core.y]]) 164 | centers = np.vstack((centers, c)) 165 | 166 | # KDTree storing vortex core center positions 167 | if centers is not None: 168 | self.core_centers = scipy.spatial.KDTree(centers) 169 | 170 | ##### generate static obstacles with random position and size 171 | if num_obs > 0: 172 | iteration = 500 173 | while True: 174 | center = self.rd.uniform(low=0.0 * np.ones(2), high=np.array([self.width, self.height])) 175 | r = self.rd.uniform(low=self.obs_r_range[0], high=self.obs_r_range[1]) 176 | obs = Obstacle(center[0], center[1], r) 177 | iteration -= 1 178 | if self.check_obstacle(obs): 179 | self.obstacles.append(obs) 180 | num_obs -= 1 181 | if iteration == 0 or num_obs == 0: 182 | break 183 | 184 | # TODO: check get_observations 185 | # return self.get_observations() 186 | 187 | def reset_robot(self, rob): 188 | # reset robot state 189 | rob.init_theta = self.rd.uniform(low=0.0, high=2 * np.pi) 190 | # rob.init_speed = self.rd.uniform(low=0.0, high=rob.max_speed) 191 | rob.init_speed = rob.max_speed 192 | current_v = self.get_velocity(rob.start[0], rob.start[1]) 193 | rob.reset_state(current_velocity=current_v) 194 | 195 | def reset_goal(self, goal_list): 196 | for i, robot in enumerate(self.robots): 197 | robot.reset_goal(goal_list[i]) 198 | 199 | def step(self, actions): 200 | # TODO: rewrite step function to update state of all robots, generate corresponding observations and rewards 201 | # execute action, update the environment, and return (obs, reward, done) 202 | 203 | rewards = [0] * len(self.robots) 204 | 205 | assert len(actions) == len(self.robots), "Number of actions not equal number of robots!" 206 | # Execute actions for all robots 207 | for i, action in enumerate(actions): 208 | rob = self.robots[i] 209 | # save action to history 210 | rob.action_history.append(action) 211 | 212 | dis_before = rob.dist_to_goal() 213 | 214 | # update robot state after executing the action 215 | for _ in range(rob.N): 216 | current_velocity = self.get_velocity(rob.x, rob.y) 217 | rob.update_state(action, current_velocity) 218 | # save trajectory 219 | rob.trajectory.append([rob.x, rob.y]) 220 | 221 | dis_after = rob.dist_to_goal() 222 | 223 | # constant penalty applied at every time step 224 | rewards[i] += self.timestep_penalty 225 | 226 | # reward agent for getting closer to the goal 227 | rewards[i] += dis_before - dis_after 228 | 229 | # Get observation for all robots 230 | observations = self.get_observations() 231 | 232 | dones = [True] * len(self.robots) 233 | infos = [{"state": "normal"}] * len(self.robots) 234 | 235 | # TODO: rewrite the reward and function: 236 | # (1) if any collision happens, end the current episode 237 | # (2) if all robots reach goals, end the current episode 238 | # (3) when a robot reach the goal and the episode does not end, 239 | # remove it from the map 240 | # if self.set_boundary and self.out_of_boundary(): 241 | # # No used in training 242 | # done = True 243 | # info = {"state":"out of boundary"} 244 | # elif self.episode_timesteps >= 1000: 245 | # done = True 246 | # info = {"state":"too long episode"} 247 | # elif self.check_collision(): 248 | # reward += self.collision_penalty 249 | # done = True 250 | # info = {"state":"collision"} 251 | # elif self.check_reach_goal(): 252 | # reward += self.goal_reward 253 | # done = True 254 | # info = {"state":"reach goal"} 255 | # else: 256 | # done = False 257 | # info = {"state":"normal"} 258 | 259 | self.episode_timesteps += 1 260 | self.total_timesteps += 1 261 | 262 | return observations, rewards, dones, infos 263 | 264 | def out_of_boundary(self): 265 | # only used when boundary is set 266 | x_out = self.robot.x < 0.0 or self.robot.x > self.width 267 | y_out = self.robot.y < 0.0 or self.robot.y > self.height 268 | return x_out or y_out 269 | 270 | def get_observations(self): 271 | observations = [] 272 | for robot in self.robots: 273 | observations.append(robot.perception_output(self.obstacles, self.robots)) 274 | return observations 275 | 276 | def check_collision(self): 277 | if len(self.obstacles) == 0: 278 | return False 279 | 280 | for obs in self.obstacles: 281 | d = np.sqrt((self.robot.x - obs.x) ** 2 + (self.robot.y - obs.y) ** 2) 282 | if d <= obs.r + self.robot.r: 283 | return True 284 | return False 285 | 286 | def check_reach_goal(self): 287 | dis = np.array([self.robot.x, self.robot.y]) - self.goal 288 | if np.linalg.norm(dis) <= self.goal_dis: 289 | return True 290 | return False 291 | 292 | def check_start_and_goal(self, start, goal): 293 | 294 | # The start and goal point is far enough 295 | if np.linalg.norm(goal - start) < self.min_start_goal_dis: 296 | return False 297 | 298 | for robot in self.robots: 299 | 300 | dis_s = robot.start - start 301 | # Start point not too close to that of existing robots 302 | if np.linalg.norm(dis_s) <= self.clear_r: 303 | return False 304 | 305 | dis_g = robot.goal - goal 306 | # Goal point not too close to that of existing robots 307 | if np.linalg.norm(dis_g) <= self.clear_r: 308 | return False 309 | 310 | return True 311 | 312 | def check_core(self, core_j): 313 | 314 | # Within the range of the map 315 | if core_j.x - self.r < 0.0 or core_j.x + self.r > self.width: 316 | return False 317 | if core_j.y - self.r < 0.0 or core_j.y + self.r > self.width: 318 | return False 319 | 320 | for robot in self.robots: 321 | # Not too close to start and goal point of each robot 322 | core_pos = np.array([core_j.x, core_j.y]) 323 | dis_s = core_pos - robot.start 324 | if np.linalg.norm(dis_s) < self.r + self.clear_r: 325 | return False 326 | dis_g = core_pos - robot.goal 327 | if np.linalg.norm(dis_g) < self.r + self.clear_r: 328 | return False 329 | 330 | for core_i in self.cores: 331 | dx = core_i.x - core_j.x 332 | dy = core_i.y - core_j.y 333 | dis = np.sqrt(dx * dx + dy * dy) 334 | 335 | if core_i.clockwise == core_j.clockwise: 336 | # i and j rotate in the same direction, their currents run towards each other at boundary 337 | # The currents speed at boundary need to be lower than threshold 338 | boundary_i = core_i.Gamma / (2 * np.pi * self.v_rel_max) 339 | boundary_j = core_j.Gamma / (2 * np.pi * self.v_rel_max) 340 | if dis < boundary_i + boundary_j: 341 | return False 342 | else: 343 | # i and j rotate in the opposite direction, their currents join at boundary 344 | # The relative current speed of the stronger vortex at boundary need to be lower than threshold 345 | Gamma_l = max(core_i.Gamma, core_j.Gamma) 346 | Gamma_s = min(core_i.Gamma, core_j.Gamma) 347 | v_1 = Gamma_l / (2 * np.pi * (dis - 2 * self.r)) 348 | v_2 = Gamma_s / (2 * np.pi * self.r) 349 | if v_1 > self.p * v_2: 350 | return False 351 | 352 | return True 353 | 354 | def check_obstacle(self, obs): 355 | 356 | # Within the range of the map 357 | if obs.x - obs.r < 0.0 or obs.x + obs.r > self.width: 358 | return False 359 | if obs.y - obs.r < 0.0 or obs.y + obs.r > self.height: 360 | return False 361 | 362 | for robot in self.robots: 363 | # Not too close to start and goal point 364 | obs_pos = np.array([obs.x, obs.y]) 365 | dis_s = obs_pos - robot.start 366 | if np.linalg.norm(dis_s) < obs.r + self.clear_r: 367 | return False 368 | dis_g = obs_pos - robot.goal 369 | if np.linalg.norm(dis_g) < obs.r + self.clear_r: 370 | return False 371 | 372 | # Not collide with vortex cores 373 | for core in self.cores: 374 | dx = core.x - obs.x 375 | dy = core.y - obs.y 376 | dis = np.sqrt(dx * dx + dy * dy) 377 | 378 | if dis <= self.r + obs.r: 379 | return False 380 | 381 | # Not collide with other obstacles 382 | for obstacle in self.obstacles: 383 | dx = obstacle.x - obs.x 384 | dy = obstacle.y - obs.y 385 | dis = np.sqrt(dx * dx + dy * dy) 386 | 387 | if dis <= obstacle.r + obs.r + self.obs_radius: 388 | return False 389 | 390 | return True 391 | 392 | def get_velocity(self, x: float, y: float): 393 | if len(self.cores) == 0: 394 | return np.zeros(2) 395 | 396 | # sort the vortices according to their distance to the query point 397 | d, idx = self.core_centers.query(np.array([x, y]), k=len(self.cores)) 398 | if isinstance(idx, np.int64): 399 | idx = [idx] 400 | 401 | v_radial_set = [] 402 | v_velocity = np.zeros((2, 1)) 403 | for i in list(idx): 404 | core = self.cores[i] 405 | v_radial = np.matrix([[core.x - x], [core.y - y]]) 406 | 407 | for v in v_radial_set: 408 | project = np.transpose(v) * v_radial 409 | if project[0, 0] > 0: 410 | # if the core is in the outter area of a checked core (wrt the query position), 411 | # assume that it has no influence the velocity of the query position 412 | continue 413 | 414 | v_radial_set.append(v_radial) 415 | dis = np.linalg.norm(v_radial) 416 | v_radial /= dis 417 | if core.clockwise: 418 | rotation = np.matrix([[0., -1.], [1., 0]]) 419 | else: 420 | rotation = np.matrix([[0., 1.], [-1., 0]]) 421 | v_tangent = rotation * v_radial 422 | speed = self.compute_speed(core.Gamma, dis) 423 | v_velocity += v_tangent * speed 424 | 425 | return np.array([v_velocity[0, 0], v_velocity[1, 0]]) 426 | 427 | def get_velocity_test(self, x: float, y: float): 428 | v = np.ones(2) 429 | return v / np.linalg.norm(v) 430 | 431 | def compute_speed(self, Gamma: float, d: float): 432 | if d <= self.r: 433 | return Gamma / (2 * np.pi * self.r * self.r) * d 434 | else: 435 | return Gamma / (2 * np.pi * d) 436 | 437 | def reset_with_eval_config(self, eval_config): 438 | self.episode_timesteps = 0 439 | 440 | # load env config 441 | self.sd = eval_config["env"]["seed"] 442 | self.width = eval_config["env"]["width"] 443 | self.height = eval_config["env"]["height"] 444 | self.r = eval_config["env"]["r"] 445 | self.v_rel_max = eval_config["env"]["v_rel_max"] 446 | self.p = eval_config["env"]["p"] 447 | self.v_range = copy.deepcopy(eval_config["env"]["v_range"]) 448 | self.obs_r_range = copy.deepcopy(eval_config["env"]["obs_r_range"]) 449 | self.clear_r = eval_config["env"]["clear_r"] 450 | self.start = np.array(eval_config["env"]["start"]) 451 | self.goal = np.array(eval_config["env"]["goal"]) 452 | self.goal_dis = eval_config["env"]["goal_dis"] 453 | self.timestep_penalty = eval_config["env"]["timestep_penalty"] 454 | self.collision_penalty = eval_config["env"]["collision_penalty"] 455 | self.goal_reward = eval_config["env"]["goal_reward"] 456 | self.discount = eval_config["env"]["discount"] 457 | 458 | # load vortex cores 459 | self.cores.clear() 460 | centers = None 461 | for i in range(len(eval_config["env"]["cores"]["positions"])): 462 | center = eval_config["env"]["cores"]["positions"][i] 463 | clockwise = eval_config["env"]["cores"]["clockwise"][i] 464 | Gamma = eval_config["env"]["cores"]["Gamma"][i] 465 | core = Core(center[0], center[1], clockwise, Gamma) 466 | self.cores.append(core) 467 | if centers is None: 468 | centers = np.array([[core.x, core.y]]) 469 | else: 470 | c = np.array([[core.x, core.y]]) 471 | centers = np.vstack((centers, c)) 472 | 473 | if centers is not None: 474 | self.core_centers = scipy.spatial.KDTree(centers) 475 | 476 | # load obstacles 477 | self.obstacles.clear() 478 | for i in range(len(eval_config["env"]["obstacles"]["positions"])): 479 | center = eval_config["env"]["obstacles"]["positions"][i] 480 | r = eval_config["env"]["obstacles"]["r"][i] 481 | obs = Obstacle(center[0], center[1], r) 482 | self.obstacles.append(obs) 483 | 484 | # load robot config 485 | self.robot.dt = eval_config["robot"]["dt"] 486 | self.robot.N = eval_config["robot"]["N"] 487 | self.robot.length = eval_config["robot"]["length"] 488 | self.robot.width = eval_config["robot"]["width"] 489 | self.robot.r = eval_config["robot"]["r"] 490 | self.robot.max_speed = eval_config["robot"]["max_speed"] 491 | self.robot.a = np.array(eval_config["robot"]["a"]) 492 | self.robot.w = np.array(eval_config["robot"]["w"]) 493 | self.robot.compute_k() 494 | self.robot.compute_actions() 495 | 496 | # load perception config 497 | self.robot.perception.range = eval_config["robot"]["perception"]["range"] 498 | self.robot.perception.angle = eval_config["robot"]["perception"]["angle"] 499 | self.robot.perception.num_beams = eval_config["robot"]["perception"]["num_beams"] 500 | self.robot.perception.compute_phi() 501 | self.robot.perception.compute_beam_angles() 502 | 503 | # update env action and observation space 504 | # self.action_space = gym.spaces.Discrete(self.robot.compute_actions_dimension()) 505 | obs_len = 2 + 2 + 2 * self.robot.perception.num_beams 506 | # self.observation_space = gym.spaces.Box(low = -np.inf * np.ones(obs_len), \ 507 | # high = np.inf * np.ones(obs_len), \ 508 | # dtype = np.float32) 509 | 510 | # reset robot state 511 | current_v = self.get_velocity(self.start[0], self.start[1]) 512 | self.robot.reset_state(self.start[0], self.start[1], current_velocity=current_v) 513 | 514 | return self.get_observation() 515 | 516 | def episode_data(self): 517 | episode = {} 518 | 519 | # save environment config 520 | episode["env"] = {} 521 | episode["env"]["seed"] = self.sd 522 | episode["env"]["width"] = self.width 523 | episode["env"]["height"] = self.height 524 | episode["env"]["r"] = self.r 525 | episode["env"]["v_rel_max"] = self.v_rel_max 526 | episode["env"]["p"] = self.p 527 | episode["env"]["v_range"] = copy.deepcopy(self.v_range) 528 | episode["env"]["obs_r_range"] = copy.deepcopy(self.obs_r_range) 529 | episode["env"]["clear_r"] = self.clear_r 530 | episode["env"]["start"] = list(self.start) 531 | episode["env"]["goal"] = list(self.goal) 532 | episode["env"]["goal_dis"] = self.goal_dis 533 | episode["env"]["timestep_penalty"] = self.timestep_penalty 534 | # episode["env"]["energy_penalty"] = self.energy_penalty.tolist() 535 | # episode["env"]["angle_penalty"] = self.angle_penalty 536 | episode["env"]["collision_penalty"] = self.collision_penalty 537 | episode["env"]["goal_reward"] = self.goal_reward 538 | episode["env"]["discount"] = self.discount 539 | 540 | # save vortex cores information 541 | episode["env"]["cores"] = {} 542 | episode["env"]["cores"]["positions"] = [] 543 | episode["env"]["cores"]["clockwise"] = [] 544 | episode["env"]["cores"]["Gamma"] = [] 545 | for core in self.cores: 546 | episode["env"]["cores"]["positions"].append([core.x, core.y]) 547 | episode["env"]["cores"]["clockwise"].append(core.clockwise) 548 | episode["env"]["cores"]["Gamma"].append(core.Gamma) 549 | 550 | # save obstacles information 551 | episode["env"]["obstacles"] = {} 552 | episode["env"]["obstacles"]["positions"] = [] 553 | episode["env"]["obstacles"]["r"] = [] 554 | for obs in self.obstacles: 555 | episode["env"]["obstacles"]["positions"].append([obs.x, obs.y]) 556 | episode["env"]["obstacles"]["r"].append(obs.r) 557 | 558 | # save robot config 559 | episode["robot"] = {} 560 | episode["robot"]["dt"] = self.robot.dt 561 | episode["robot"]["N"] = self.robot.N 562 | episode["robot"]["length"] = self.robot.length 563 | episode["robot"]["width"] = self.robot.width 564 | episode["robot"]["r"] = self.robot.r 565 | episode["robot"]["max_speed"] = self.robot.max_speed 566 | episode["robot"]["a"] = list(self.robot.a) 567 | episode["robot"]["w"] = list(self.robot.w) 568 | 569 | # save perception config 570 | episode["robot"]["perception"] = {} 571 | episode["robot"]["perception"]["range"] = self.robot.perception.range 572 | episode["robot"]["perception"]["angle"] = self.robot.perception.angle 573 | episode["robot"]["perception"]["num_beams"] = self.robot.perception.num_beams 574 | 575 | # save action history 576 | episode["robot"]["action_history"] = copy.deepcopy(self.robot.action_history) 577 | episode["robot"]["trajectory"] = copy.deepcopy(self.robot.trajectory) 578 | 579 | return episode 580 | 581 | def save_episode(self, filename): 582 | episode = self.episode_data() 583 | with open(filename, "w") as file: 584 | json.dump(episode, file) 585 | -------------------------------------------------------------------------------- /marinenav_env/envs/utils/robot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import copy 3 | from nav.utils import theta_0_to_2pi, pose_vector_to_matrix, world_to_local 4 | 5 | 6 | class Perception: 7 | 8 | def __init__(self, sensor_range=10, cooperative: bool = False): 9 | # 2D LiDAR model with detection area as a sector 10 | self.observation = None 11 | self.range = sensor_range # range of beams (meter) 12 | self.angle = 2 * np.pi # detection angle range 13 | # self.len_obs_history = 0 # the window size of observation history 14 | self.observation_format(cooperative) 15 | self.observed_obs = [] # indices of observed static obstacles 16 | self.observed_objs = [] # indiced of observed dynamic objects 17 | 18 | def observation_format(self, cooperative: bool = False): 19 | # format: {"self": [velocity,goal, odom], 20 | # "static":[[obs_1.x,obs_1.y,obs_1.r, obs_1.id],...,[obs_n.x,obs_n.y,obs_n.r, obs_n.id]], 21 | # "dynamic":{id_1:[[robot_1.x,robot_1.y,robot_1.vx,robot_1.vy,robot_1.theta]_(t-m),...,[]_t]...} 22 | if cooperative: 23 | self.observation = dict(self=[], static=[], dynamic=[]) 24 | else: 25 | self.observation = dict(self=[], static=[]) 26 | 27 | 28 | class Odometry: 29 | def __init__(self, use_noise=True): 30 | self.use_noise = use_noise 31 | if use_noise: 32 | self.max_g_error = .5 / 180 * np.pi # max gyro error 33 | self.max_a_error = 0.05 # max acceleration error 34 | self.max_a_p_error = 0.05 # max acceleration error percentage 35 | z_score = 1.96 # 95% confidence interval 36 | 37 | self.sigma_g = self.max_g_error / z_score 38 | self.sigma_a = self.max_a_error / z_score # accel noise 39 | self.sigma_a_p = self.max_a_p_error / z_score # accel noise percentage 40 | else: 41 | self.max_g_error = 0 42 | self.max_a_error = 0 43 | self.max_a_p_error = 0 44 | self.sigma_g = 0 45 | self.sigma_a = 0 46 | self.sigma_a_p = 0 47 | 48 | self.x_old = None 49 | self.y_old = None 50 | self.theta_old = None 51 | self.observation = [0, 0, 0] 52 | 53 | def reset(self, x0, y0=None, theta0=None): 54 | if y0 is None and theta0 is None: 55 | self.x_old = x0[0] 56 | self.y_old = x0[1] 57 | self.theta_old = x0[2] 58 | 59 | else: 60 | self.x_old = x0 61 | self.y_old = y0 62 | self.theta_old = theta0 63 | 64 | def add_noise(self, x_new, y_new, theta_new): 65 | if self.x_old is None: 66 | raise ValueError("Odometry is not initialized!") 67 | 68 | dx, dy, dtheta = world_to_local(x_new, y_new, theta_new, 69 | [self.x_old, self.y_old, self.theta_old]) 70 | 71 | self.x_old = copy.deepcopy(x_new) 72 | self.y_old = copy.deepcopy(y_new) 73 | self.theta_old = copy.deepcopy(theta_new) 74 | 75 | if self.use_noise: 76 | # max acceleration 5 cm / 5% of measurement 77 | sigma_a_p = self.sigma_a_p * np.linalg.norm([dx, dy]) 78 | sigma_a = np.min([self.sigma_a, sigma_a_p]) 79 | 80 | max_a_p_error = self.max_a_p_error * np.linalg.norm([dx, dy]) 81 | max_a_error = np.min([max_a_p_error, self.max_a_error]) 82 | 83 | w_a = np.random.normal(0, sigma_a) 84 | w_g = np.random.normal(0, self.sigma_g) 85 | 86 | dtheta_noisy = theta_0_to_2pi(dtheta + np.clip(w_g, None, self.max_g_error)) 87 | dx_noisy = dx + np.clip(w_a, None, max_a_error) * np.cos(dtheta_noisy) 88 | dy_noisy = dy + np.clip(w_a, None, max_a_error) * np.sin(dtheta_noisy) 89 | else: 90 | dx_noisy = dx 91 | dy_noisy = dy 92 | dtheta_noisy = dtheta 93 | 94 | self.observation = [dx_noisy, dy_noisy, dtheta_noisy] 95 | 96 | def get_odom(self): 97 | return self.observation 98 | 99 | 100 | class RangeBearingMeasurement: 101 | def __init__(self, use_noise=True): 102 | self.use_noise = use_noise 103 | if use_noise: 104 | self.max_b_error = 0.5 / 180 * np.pi # max bearing error 105 | self.max_r_error = 0.002 # max range error 106 | z_score = 1.96 # 95% confidence interval 107 | 108 | self.sigma_r = self.max_r_error / z_score 109 | self.sigma_b = self.max_b_error / z_score 110 | else: 111 | self.max_r_error = 0 112 | self.max_b_error = 0 113 | self.sigma_r = 0 114 | self.sigma_b = 0 115 | 116 | def add_noise(self, x_obs, y_obs): 117 | r = np.linalg.norm([x_obs, y_obs]) 118 | b = np.arccos(x_obs / r) # [0, PI] 119 | if y_obs < 0: 120 | b = 2 * np.pi - b 121 | if self.use_noise: 122 | r_noise = np.random.normal(0, self.sigma_r) 123 | r_noise = np.clip(r_noise, None, self.max_r_error) 124 | 125 | b_noise = np.random.normal(0, self.sigma_b) 126 | b_noise = np.clip(b_noise, None, self.max_b_error) 127 | else: 128 | r_noise = 0 129 | b_noise = 0 130 | 131 | return [r + r_noise, b + b_noise] 132 | 133 | def add_noise_point_based(self, robot_pose, landmark_position): 134 | x_r = np.reshape(np.array(landmark_position), (2, 1)) 135 | 136 | R_wr, t_wr = pose_vector_to_matrix(robot_pose[0], robot_pose[1], robot_pose[2]) 137 | 138 | R_rw = np.transpose(R_wr) 139 | t_rw = -R_rw * t_wr 140 | 141 | x_r = R_rw * x_r + t_rw 142 | x_r.resize((2,)) 143 | 144 | return self.add_noise(x_r[0], x_r[1]) 145 | 146 | 147 | class RobotNeighborMeasurement: 148 | def __init__(self, use_noise=True): 149 | self.use_noise = use_noise 150 | if use_noise: 151 | self.max_b_error = 0.2 / 180 * np.pi # max bearing error 152 | self.max_r_error = 0.002 # max range error 153 | z_score = 1.96 # 95% confidence interval 154 | 155 | self.sigma_r = self.max_r_error / z_score 156 | self.sigma_b = self.max_b_error / z_score 157 | else: 158 | self.max_b_error = 0 159 | self.max_r_error = 0 160 | self.sigma_r = 0 161 | self.sigma_b = 0 162 | 163 | def add_noise(self, x_obs, y_obs, theta_obs): 164 | if self.use_noise: 165 | x_noise = np.random.normal(0, self.sigma_r) 166 | y_noise = np.random.normal(0, self.sigma_r) 167 | 168 | b_noise = np.random.normal(0, self.sigma_b) 169 | 170 | x_noisy = x_obs + np.clip(x_noise, None, self.max_r_error) 171 | y_noisy = y_obs + np.clip(y_noise, None, self.max_r_error) 172 | theta_noisy = theta_0_to_2pi(theta_obs + np.clip(b_noise, None, self.max_b_error)) 173 | 174 | # x_noisy = x_obs + np.clip(x_noise, None, self.max_r_error) * np.cos(theta_noisy) 175 | # y_noisy = y_obs + np.clip(x_noise, None, self.max_r_error) * np.sin(theta_noisy) 176 | 177 | else: 178 | x_noisy = x_obs 179 | y_noisy = y_obs 180 | theta_noisy = theta_obs 181 | 182 | return [x_noisy, y_noisy, theta_noisy] 183 | 184 | def add_noise_pose_based(self, pose_local, pose_observed): 185 | x_r = np.reshape(np.array([pose_observed[0], pose_observed[1]]), (2, 1)) 186 | 187 | R_wr, t_wr = pose_vector_to_matrix(pose_local[0], pose_local[1], pose_local[2]) 188 | 189 | R_rw = np.transpose(R_wr) 190 | t_rw = -R_rw * t_wr 191 | 192 | x_r = R_rw * x_r + t_rw 193 | x_r.resize((2,)) 194 | 195 | return self.add_noise(x_r[0], x_r[1], theta_0_to_2pi(pose_observed[2] - pose_local[2])) 196 | 197 | 198 | class Robot: 199 | 200 | def __init__(self, sensor_range=10, cooperative: bool = False): 201 | 202 | # parameter initialization 203 | self.cooperative = cooperative # if the robot is cooperative or not 204 | self.dt = .2 # discretized time step (second) 205 | self.N = 5 # number of time step per action 206 | self.perception = Perception(sensor_range=sensor_range, cooperative=cooperative) 207 | self.length = 1.0 208 | self.width = 0.5 209 | self.r = 0.8 # collision range 210 | self.detect_r = 0.5 * np.sqrt(self.length ** 2 + self.width ** 2) # detection range 211 | self.goal_dis = 1 # max distance to goal considered as reached 212 | self.max_speed = 1.0 213 | self.a = np.array([-0.4, 0.0, 0.4]) # linear accelerations (m/s^2) 214 | self.w = np.zeros(12) # angular velocities (rad/s) 215 | for i in range(12): 216 | self.w[i] = -np.pi + i * np.pi / 6 217 | # self.w = np.array([-np.pi / 3, -np.pi / 4, -np.pi / 6, -np.pi / 12, 218 | # 0.0, np.pi / 12, np.pi / 6, np.pi / 4, np.pi / 3]) # angular velocities (rad/s) 219 | self.compute_k() # cofficient of water resistance 220 | self.compute_actions() # list of actions 221 | 222 | self.x = None # x coordinate 223 | self.y = None # y coordinate 224 | self.theta = None # steering heading angle 225 | self.speed = None # steering foward speed 226 | self.velocity = None # velocity wrt sea floor 227 | 228 | self.start = None # start position 229 | self.goal = None # goal position 230 | self.reach_goal = False 231 | self.signal_init = True 232 | 233 | self.init_theta = 0.0 # theta at initial position 234 | self.init_speed = 0.0 # speed at initial position 235 | 236 | self.action_history = [] # history of action commands in one episode 237 | self.trajectory = [] # trajectory in one episode 238 | 239 | self.odometry = Odometry() # odometry 240 | # add noisy to landmark observation 241 | self.landmark_observation = RangeBearingMeasurement() 242 | self.robot_observation = RobotNeighborMeasurement() 243 | 244 | self.waiting = False 245 | self.waiting_for_robot = None 246 | 247 | def reset_waiting(self, id=None): 248 | if id is not None: 249 | self.waiting = True 250 | self.waiting_for_robot = id 251 | else: 252 | self.waiting = False 253 | self.waiting_for_robot = None 254 | 255 | def compute_k(self): 256 | self.k = np.max(self.a) / self.max_speed 257 | 258 | def compute_actions(self): 259 | self.actions = [(acc, ang_v) for acc in self.a for ang_v in self.w] 260 | 261 | def compute_actions_dimension(self): 262 | return len(self.actions) 263 | 264 | def compute_dist_reward_scale(self): 265 | # scale the distance reward 266 | return 1 / (self.max_speed * self.N * self.dt) 267 | 268 | def compute_penalty_matrix(self): 269 | # scale the penalty value to [-1,0] 270 | scale_a = 1 / (np.max(self.a) * np.max(self.a)) 271 | scale_w = 1 / (np.max(self.w) * np.max(self.w)) 272 | p = -0.5 * np.matrix([[scale_a, 0.0], [0.0, scale_w]]) 273 | return p 274 | 275 | def compute_action_energy_cost(self, action): 276 | # scale the a and w to [0,1] 277 | a, w = self.actions[action] 278 | a /= np.max(self.a) 279 | w /= np.max(self.w) 280 | return np.abs(a) + np.abs(w) 281 | 282 | def dist_to_goal(self): 283 | return np.linalg.norm(self.goal - np.array([self.x, self.y])) 284 | 285 | def check_reach_goal(self): 286 | if self.dist_to_goal() <= self.goal_dis: 287 | self.reach_goal = True 288 | 289 | def reset_goal(self, goal_this): 290 | self.goal = np.array(goal_this) 291 | self.reach_goal = False 292 | 293 | def reset_state(self, current_velocity=np.zeros(2)): 294 | # only called when resetting the environment 295 | self.action_history.clear() 296 | self.trajectory.clear() 297 | self.x = self.start[0] 298 | self.y = self.start[1] 299 | self.theta = self.init_theta 300 | self.speed = self.init_speed 301 | self.update_velocity(current_velocity) 302 | self.trajectory.append([self.x, self.y, self.theta, self.speed, self.velocity[0], self.velocity[1]]) 303 | 304 | self.odometry.reset(self.x, self.y, self.theta) 305 | 306 | def get_robot_transform(self): 307 | # compute transformation from world frame to robot frame 308 | R_wr = np.matrix([[np.cos(self.theta), -np.sin(self.theta)], [np.sin(self.theta), np.cos(self.theta)]]) 309 | t_wr = np.matrix([[self.x], [self.y]]) 310 | return R_wr, t_wr 311 | 312 | def get_steer_velocity(self, speed=None, theta=None): 313 | if speed is None and theta is None: 314 | # Case when no parameters are provided 315 | return self.speed * np.array([np.cos(self.theta), np.sin(self.theta)]) 316 | else: 317 | # Case when speed and theta are provided 318 | return speed * np.array([np.cos(theta), np.sin(theta)]) 319 | 320 | def update_velocity(self, current_velocity=np.zeros(2), velocity=None, theta=None): 321 | if velocity is None and theta is None: 322 | steer_velocity = self.get_steer_velocity() 323 | self.velocity = steer_velocity + current_velocity 324 | else: 325 | steer_velocity = self.get_steer_velocity(velocity, theta) 326 | return steer_velocity + current_velocity 327 | 328 | def update_state(self, action, current_velocity): 329 | # update robot position in one time step 330 | self.update_velocity(current_velocity) 331 | dis = self.velocity * self.dt 332 | self.x += dis[0] 333 | self.y += dis[1] 334 | 335 | # update robot speed in one time step 336 | a, w = self.actions[action] 337 | 338 | # self.theta += action 339 | # self.update_velocity(current_velocity) 340 | # dis = self.velocity * self.dt 341 | # self.x += dis[0] 342 | # self.y += dis[1] 343 | # a = 1 344 | # assume that water resistance force is proportion to the speed 345 | self.speed += (a-self.k*self.speed) * self.dt 346 | self.speed = np.clip(self.speed,0.0,self.max_speed) 347 | 348 | # update robot heading angle in one time step 349 | self.theta += w * self.dt 350 | 351 | 352 | # warp theta to [0,2*pi) 353 | while self.theta < 0.0: 354 | self.theta += 2 * np.pi 355 | while self.theta >= 2 * np.pi: 356 | self.theta -= 2 * np.pi 357 | # print(self.x, self.y, self.theta/np.pi * 180, self.velocity) 358 | # if add_noise: 359 | # self.odometry.add_noise(self.x, self.y, self.theta) 360 | 361 | def check_collision(self, obj_x, obj_y, obj_r): 362 | d = np.sqrt((self.x - obj_x) ** 2 + (self.y - obj_y) ** 2) 363 | if d <= obj_r + self.r: 364 | return True 365 | else: 366 | return False 367 | 368 | def check_detection(self, obj_x, obj_y, obj_r): 369 | proj_pos = self.project_to_robot_frame(np.array([obj_x, obj_y]), False) 370 | 371 | if np.linalg.norm(proj_pos) > self.perception.range + obj_r: 372 | return False 373 | 374 | # angle = np.arctan2(proj_pos[1], proj_pos[0]) 375 | # if angle < -0.5 * self.perception.angle or angle > 0.5 * self.perception.angle: 376 | # return False 377 | 378 | return True 379 | 380 | def project_to_robot_frame(self, x, is_vector=True): 381 | assert isinstance(x, np.ndarray), "the input needs to be an numpy array" 382 | assert np.shape(x) == (2,) 383 | 384 | x_r = np.reshape(x, (2, 1)) 385 | 386 | R_wr, t_wr = self.get_robot_transform() 387 | 388 | R_rw = np.transpose(R_wr) 389 | t_rw = -R_rw * t_wr 390 | 391 | if is_vector: 392 | x_r = R_rw * x_r 393 | else: 394 | x_r = R_rw * x_r + t_rw 395 | 396 | x_r.resize((2,)) 397 | return np.array(x_r) 398 | 399 | def perception_output(self, obstacles, robots): 400 | # TODO: remove LiDAR reflection computations and check dynamic obstacle observation error 401 | if self.reach_goal: 402 | return None, False, True 403 | 404 | self.perception.observation["static"].clear() 405 | self.perception.observation["dynamic"].clear() 406 | 407 | ##### self observation (velocity and goal in self frame) ##### 408 | # vehicle velocity wrt seafloor in self frame 409 | abs_velocity_r = self.project_to_robot_frame(self.velocity) 410 | 411 | # goal position in self frame 412 | goal_r = self.project_to_robot_frame(self.goal, False) 413 | self.perception.observation["self"] = [goal_r[0], goal_r[1], 414 | abs_velocity_r[0], abs_velocity_r[1], 415 | self.x, self.y, self.theta] 416 | 417 | ##### observation of other objects ##### 418 | self.perception.observed_obs.clear() 419 | if self.cooperative: 420 | self.perception.observed_objs.clear() 421 | 422 | collision = False 423 | self.check_reach_goal() 424 | 425 | # static obstacles observation 426 | for i, obs in enumerate(obstacles): 427 | if not self.check_detection(obs.x, obs.y, obs.r): 428 | continue 429 | 430 | self.perception.observed_obs.append(i) 431 | 432 | if not collision: 433 | collision = self.check_collision(obs.x, obs.y, obs.r) 434 | 435 | pos_r = self.project_to_robot_frame(np.array([obs.x, obs.y]), False) 436 | self.perception.observation["static"].append([pos_r[0], pos_r[1], obs.r, i]) 437 | 438 | if self.cooperative: 439 | # dynamic objects observation 440 | for j, robot in enumerate(robots): 441 | if robot is self: 442 | continue 443 | # if robot.reach_goal and not self.signal_init: 444 | # This robot is in the deactivate state, and abscent from the current map 445 | # continue 446 | if not self.check_detection(robot.x, robot.y, robot.detect_r) and not self.signal_init: 447 | continue 448 | 449 | if self.signal_init: 450 | self.signal_init = False 451 | 452 | self.perception.observed_objs.append(j) 453 | 454 | if not collision: 455 | collision = self.check_collision(robot.x, robot.y, robot.r) 456 | 457 | pos_r = self.project_to_robot_frame(np.array([robot.x, robot.y]), False) 458 | v_r = self.project_to_robot_frame(robot.velocity) 459 | theta_r = theta_0_to_2pi(robot.theta - self.theta) 460 | new_obs = list([pos_r[0], pos_r[1], v_r[0], v_r[1], theta_r, j]) 461 | self.perception.observation["dynamic"].append(new_obs) 462 | self_state = copy.deepcopy(self.perception.observation["self"]) 463 | static_states = copy.deepcopy(self.perception.observation["static"]) 464 | if self.cooperative: 465 | dynamic_states = copy.deepcopy(self.perception.observation["dynamic"]) 466 | obs = (self_state, static_states, dynamic_states) 467 | return obs, collision, self.reach_goal 468 | else: 469 | return (self_state, static_states), collision, self.reach_goal 470 | -------------------------------------------------------------------------------- /marinenav_env/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup(name='marinenav_env', 4 | version='0.0.1', 5 | install_requires=['numpy', 'scipy', 'matplotlib', 'gtsam'] 6 | ) -------------------------------------------------------------------------------- /nav/BSP.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gtsam 3 | 4 | from marinenav_env.envs.utils.robot import Odometry, RangeBearingMeasurement, RobotNeighborMeasurement 5 | from nav.utils import get_symbol, local_to_world_values, generate_virtual_waypoints 6 | 7 | DEBUG_BSP = False 8 | 9 | 10 | class BeliefSpacePlanning: 11 | def __init__(self, radius, robot_state_idx_position_goal, landmarks): 12 | self.prior_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.001, 0.001, 0.001]) 13 | # for odometry measurement 14 | self.odom_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.025, 0.025, 0.008]) 15 | 16 | # for landmark measurement 17 | self.range_bearing_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.004, 0.05]) 18 | 19 | # for inter-robot measurement 20 | self.robot_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.1, 0.1, 0.004]) 21 | 22 | self.slam_speed = 2 23 | 24 | # radius for generate virtual landmark and virtual inter-robot observation 25 | self.radius = radius 26 | 27 | self.landmarks_position = [[item[1], item[2]] for item in landmarks] 28 | self.landmarks_id = [item[0] for item in landmarks] 29 | 30 | self.robot_state_idx = robot_state_idx_position_goal[0] 31 | self.robot_state_position = robot_state_idx_position_goal[1] 32 | self.robot_state_goal = robot_state_idx_position_goal[2] 33 | 34 | def odometry_measurement_gtsam_format(self, measurement, robot_id=None, id0=None, id1=None): 35 | return gtsam.BetweenFactorPose2(get_symbol(robot_id, id0), get_symbol(robot_id, id1), 36 | gtsam.Pose2(measurement[0], measurement[1], measurement[2]), 37 | self.odom_noise_model) 38 | 39 | def landmark_measurement_gtsam_format(self, bearing=None, range=None, robot_id=None, id0=None, idl=None): 40 | return gtsam.BearingRangeFactor2D(get_symbol(robot_id, id0), idl, 41 | gtsam.Rot2(bearing), range, self.range_bearing_noise_model) 42 | 43 | def robot_measurement_gtsam_format(self, measurement, robot_id: tuple, id: tuple): 44 | return gtsam.BetweenFactorPose2(get_symbol(robot_id[0], id[0]), get_symbol(robot_id[1], id[1]), 45 | gtsam.Pose2(measurement[0], measurement[1], measurement[2]), 46 | self.robot_noise_model) 47 | 48 | def prior_gtsam_format(self, measurement, robot_id=None, id0=None): 49 | return gtsam.PriorFactorPose2(get_symbol(robot_id, id0), 50 | gtsam.Pose2(measurement[0], measurement[1], measurement[2]), 51 | self.prior_noise_model) 52 | 53 | def waypoints2landmark_observations(self, waypoints, robot_id=None, graph=None, initial_estimate=None): 54 | # waypoints: [[x, y, theta], ...] 55 | # landmarks: [id, x, y], ...] 56 | # robot_id: robot id 57 | # id_initial: index of the first virtual waypoint in the sequence 58 | if graph is None: 59 | graph = gtsam.NonlinearFactorGraph() 60 | if initial_estimate is None: 61 | initial_estimate = gtsam.Values() 62 | 63 | odometry_factory = Odometry(use_noise=False) 64 | range_bearing_factory = RangeBearingMeasurement(use_noise=False) 65 | id_initial = self.robot_state_idx[robot_id] 66 | for i, waypoint in enumerate(waypoints): 67 | # calculate virtual odometry between neighbor waypoints 68 | if i == 0: 69 | # the first waypoint is same as present robot position, so no need to add into graph 70 | odometry_factory.reset(waypoint) 71 | initial_estimate.insert(get_symbol(robot_id, id_initial), 72 | gtsam.Pose2(waypoint[0], waypoint[1], waypoint[2])) 73 | graph.add(self.prior_gtsam_format(waypoint, robot_id=robot_id, id0=id_initial)) 74 | continue 75 | 76 | odometry_factory.add_noise(waypoint[0], waypoint[1], waypoint[2]) 77 | odometry_this = odometry_factory.get_odom() 78 | # add odometry 79 | graph.add(self.odometry_measurement_gtsam_format( 80 | odometry_this, robot_id, id_initial + i - 1, id_initial + i)) 81 | initial_estimate.insert(get_symbol(robot_id, id_initial + i), 82 | gtsam.Pose2(waypoint[0], waypoint[1], waypoint[2])) 83 | 84 | if len(self.landmarks_position) != 0: 85 | # calculate Euclidean distance between waypoint and landmarks 86 | distances = np.linalg.norm(self.landmarks_position - np.array(waypoint[0:2]), axis=1) 87 | landmark_indices = np.argwhere(distances < self.radius) 88 | else: 89 | landmark_indices = [] 90 | if len(landmark_indices) != 0: 91 | # add landmark observation factor 92 | for landmark_index in landmark_indices: 93 | landmark_this = self.landmarks_position[landmark_index[0]] 94 | # calculate range and bearing 95 | rb = range_bearing_factory.add_noise_point_based(waypoint, landmark_this) 96 | if not initial_estimate.exists(self.landmarks_id[landmark_index[0]]): 97 | initial_estimate.insert(self.landmarks_id[landmark_index[0]], gtsam.Point2(landmark_this[0], landmark_this[1])) 98 | graph.add(self.landmark_measurement_gtsam_format(bearing=rb[1], range=rb[0], 99 | robot_id=robot_id, 100 | id0=id_initial + i, 101 | idl=self.landmarks_id[landmark_index[0]])) 102 | return graph, initial_estimate 103 | 104 | def waypoints2robot_observation(self, waypoints: tuple, robot_id: tuple, graph=None): 105 | # waypoints: ([[x, y, theta], ...], [[x, y, theta], ...]) 106 | # robot_id: (robot id0, robot id1) 107 | # id_initial: (index robot0, index robot1) 108 | if graph is None: 109 | graph = gtsam.NonlinearFactorGraph() 110 | id_initial = (self.robot_state_idx[robot_id[0]], self.robot_state_idx[robot_id[1]]) 111 | robot_neighbor_factory = RobotNeighborMeasurement(use_noise=False) 112 | 113 | waypoints0 = np.array(waypoints[0]) 114 | waypoints1 = np.array(waypoints[1]) 115 | 116 | length0 = len(waypoints0) 117 | length1 = len(waypoints1) 118 | 119 | if length0 < length1: 120 | repeat_times = length1 - length0 121 | repeated_rows = np.repeat(waypoints0[-1:], repeat_times, axis=0) 122 | waypoints0 = np.concatenate((waypoints0, repeated_rows), axis=0) 123 | elif length0 > length1: 124 | repeat_times = length0 - length1 125 | repeated_rows = np.repeat(waypoints1[-1:], repeat_times, axis=0) 126 | waypoints1 = np.concatenate((waypoints1, repeated_rows), axis=0) 127 | try: 128 | distances = np.linalg.norm(waypoints0[:, 0:2] - waypoints1[:, 0:2], axis=1) 129 | except IndexError: 130 | print("waypoint0: ", length0, waypoints0) 131 | print("waypoint1: ", length1, waypoints1) 132 | # find out the index of waypoints where robot could observe each other 133 | robot_indices = np.argwhere(distances < self.radius) 134 | for robot_index in robot_indices: 135 | # add robot observation factor 136 | measurement = robot_neighbor_factory.add_noise_pose_based(waypoints0[robot_index, :].flatten(), 137 | waypoints1[robot_index, :].flatten()) 138 | if robot_index >= length0: 139 | idx0 = id_initial[0] + length0 - 1 140 | else: 141 | idx0 = id_initial[0] + robot_index 142 | 143 | if robot_index >= length1: 144 | idx1 = id_initial[1] + length1 - 1 145 | else: 146 | idx1 = id_initial[1] + robot_index 147 | graph.add(self.robot_measurement_gtsam_format( 148 | measurement, robot_id, (idx0, idx1))) 149 | return graph 150 | 151 | def generate_virtual_observation_graph(self, frontier_position, robot_id): 152 | graph = gtsam.NonlinearFactorGraph() 153 | initial_estimate = gtsam.Values() 154 | virtual_waypoints = [[] for _ in range(len(self.robot_state_idx))] 155 | for i in range(len(self.robot_state_idx)): 156 | # set the target robot's virtual goal as the frontier position, other robots just reach existing goal 157 | if i == robot_id: 158 | virtual_goal = frontier_position 159 | else: 160 | virtual_goal = self.robot_state_goal[i] 161 | virtual_waypoints[i] = generate_virtual_waypoints(self.robot_state_position[i], 162 | virtual_goal, 163 | speed=self.slam_speed) 164 | self.waypoints2landmark_observations(virtual_waypoints[i], i, 165 | graph=graph, 166 | initial_estimate=initial_estimate) 167 | for i in range(len(self.robot_state_idx)): 168 | if i == robot_id or len(virtual_waypoints[i]) == 0 or len(virtual_waypoints[robot_id]) == 0: 169 | continue 170 | self.waypoints2robot_observation(tuple([virtual_waypoints[robot_id], virtual_waypoints[i]]), 171 | tuple([robot_id, i]), graph=graph) 172 | return graph, initial_estimate 173 | 174 | def optimize_virtual_observation_graph(self, graph: gtsam.NonlinearFactorGraph, initial_estimate: gtsam.Values): 175 | # optimize the graph 176 | params = gtsam.ISAM2Params() 177 | params.setFactorization("QR") 178 | isam = gtsam.ISAM2(params) 179 | # with open('log.txt', 'a') as file: 180 | # print("initial_estimate: ", initial_estimate, file=file) 181 | # print("graph: ", graph, file=file) 182 | isam.update(graph, initial_estimate) 183 | result = isam.calculateEstimate() 184 | marginals = gtsam.Marginals(isam.getFactorsUnsafe(), result) 185 | 186 | return result, marginals 187 | 188 | def do(self, frontier_position, robot_id, origin, axis): 189 | # return the optimized trajectories of the robots and the marginals for calculation of information matrix 190 | graph, initial_estimate = self.generate_virtual_observation_graph(frontier_position=frontier_position, 191 | robot_id=robot_id) 192 | result, marginals = self.optimize_virtual_observation_graph(graph, initial_estimate) 193 | result = local_to_world_values(result, origin, robot_id) 194 | 195 | # draw the optimized result 196 | if DEBUG_BSP: 197 | scatters_x = [] 198 | scatters_y = [] 199 | for key in result.keys(): 200 | if key < gtsam.symbol('a', 0): 201 | axis.scatter(result.atPoint2(key)[0], result.atPoint2(key)[1], c='r', marker='o') 202 | else: 203 | scatters_x.append(result.atPose2(key).x()) 204 | scatters_y.append(result.atPose2(key).y()) 205 | axis.scatter(scatters_x, scatters_y, c='b', marker='.', alpha=0.1) 206 | 207 | return result, marginals 208 | -------------------------------------------------------------------------------- /nav/EM.py: -------------------------------------------------------------------------------- 1 | # Everything in SLAM frame in ExpectationMaximizationTrajectory 2 | import gtsam 3 | import numpy as np 4 | from nav.utils import get_symbol, generate_virtual_waypoints, local_to_world_values 5 | from marinenav_env.envs.utils.robot import Odometry, RangeBearingMeasurement, RobotNeighborMeasurement 6 | 7 | DEBUG_EM = False 8 | 9 | 10 | class ExpectationMaximizationTrajectory: 11 | def __init__(self, radius, robot_state_idx_position_goal, landmarks, isam: tuple): 12 | # for odometry measurement 13 | self.odom_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.03, 0.03, 0.004]) 14 | 15 | # for landmark measurement 16 | self.range_bearing_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.004, 0.001]) 17 | 18 | # for inter-robot measurement 19 | self.robot_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.01, 0.01, 0.004]) 20 | 21 | self.slam_speed = 2 22 | 23 | # radius for generate virtual landmark and virtual inter-robot observation 24 | self.radius = radius 25 | 26 | self.landmarks_position = [[item[1], item[2]] for item in landmarks] 27 | self.landmarks_id = [item[0] for item in landmarks] 28 | 29 | params = gtsam.ISAM2Params() 30 | params.setFactorization("QR") 31 | self.isam = (isam[0], isam[1]) 32 | self.params = params 33 | 34 | self.robot_state_idx = robot_state_idx_position_goal[0] 35 | self.robot_state_position = robot_state_idx_position_goal[1] 36 | self.robot_state_goal = robot_state_idx_position_goal[2] 37 | 38 | def odometry_measurement_gtsam_format(self, measurement, robot_id=None, id0=None, id1=None): 39 | return gtsam.BetweenFactorPose2(get_symbol(robot_id, id0), get_symbol(robot_id, id1), 40 | gtsam.Pose2(measurement[0], measurement[1], measurement[2]), 41 | self.odom_noise_model) 42 | 43 | def landmark_measurement_gtsam_format(self, bearing=None, range=None, robot_id=None, id0=None, idl=None): 44 | return gtsam.BearingRangeFactor2D(get_symbol(robot_id, id0), idl, 45 | gtsam.Rot2(bearing), range, self.range_bearing_noise_model) 46 | 47 | def robot_measurement_gtsam_format(self, measurement, robot_id: tuple, id: tuple): 48 | return gtsam.BetweenFactorPose2(get_symbol(robot_id[0], id[0]), get_symbol(robot_id[1], id[1]), 49 | gtsam.Pose2(measurement[0], measurement[1], measurement[2]), 50 | self.robot_noise_model) 51 | 52 | def waypoints2landmark_observations(self, waypoints, robot_id=None, graph=None, initial_estimate=None): 53 | # waypoints: [[x, y, theta], ...] 54 | # landmarks: [id, x, y], ...] 55 | # robot_id: robot id 56 | # id_initial: index of the first virtual waypoint in the sequence 57 | if graph is None: 58 | graph = gtsam.NonlinearFactorGraph() 59 | if initial_estimate is None: 60 | initial_estimate = gtsam.Values() 61 | 62 | odometry_factory = Odometry(use_noise=False) 63 | range_bearing_factory = RangeBearingMeasurement(use_noise=False) 64 | id_initial = self.robot_state_idx[robot_id] 65 | for i, waypoint in enumerate(waypoints): 66 | # calculate virtual odometry between neighbor waypoints 67 | if i == 0: 68 | # the first waypoint is same as present robot position, so no need to add into graph 69 | odometry_factory.reset(waypoint) 70 | continue 71 | 72 | odometry_factory.add_noise(waypoint[0], waypoint[1], waypoint[2]) 73 | odometry_this = odometry_factory.get_odom() 74 | # add odometry 75 | graph.add(self.odometry_measurement_gtsam_format( 76 | odometry_this, robot_id, id_initial + i - 1, id_initial + i)) 77 | initial_estimate.insert(get_symbol(robot_id, id_initial + i), 78 | gtsam.Pose2(waypoint[0], waypoint[1], waypoint[2])) 79 | 80 | if len(self.landmarks_position) != 0: 81 | # calculate Euclidean distance between waypoint and landmarks 82 | distances = np.linalg.norm(self.landmarks_position - np.array(waypoint[0:2]), axis=1) 83 | landmark_indices = np.argwhere(distances < self.radius) 84 | else: 85 | landmark_indices = [] 86 | if len(landmark_indices) != 0: 87 | # add landmark observation factor 88 | for landmark_index in landmark_indices: 89 | landmark_this = self.landmarks_position[landmark_index[0]] 90 | # calculate range and bearing 91 | rb = range_bearing_factory.add_noise_point_based(waypoint, landmark_this) 92 | graph.add(self.landmark_measurement_gtsam_format(bearing=rb[1], range=rb[0], 93 | robot_id=robot_id, 94 | id0=id_initial + i, 95 | idl=self.landmarks_id[landmark_index[0]])) 96 | # print(graph) 97 | return graph, initial_estimate 98 | 99 | def waypoints2robot_observation(self, waypoints: tuple, robot_id: tuple, graph=None): 100 | # waypoints: ([[x, y, theta], ...], [[x, y, theta], ...]) 101 | # robot_id: (robot id0, robot id1) 102 | # id_initial: (index robot0, index robot1) 103 | if graph is None: 104 | graph = gtsam.NonlinearFactorGraph() 105 | id_initial = (self.robot_state_idx[robot_id[0]], self.robot_state_idx[robot_id[1]]) 106 | robot_neighbor_factory = RobotNeighborMeasurement(use_noise=False) 107 | 108 | waypoints0 = np.array(waypoints[0]) 109 | waypoints1 = np.array(waypoints[1]) 110 | 111 | length0 = len(waypoints0) 112 | length1 = len(waypoints1) 113 | 114 | if length0 < length1: 115 | repeat_times = length1 - length0 116 | repeated_rows = np.repeat(waypoints0[-1:], repeat_times, axis=0) 117 | waypoints0 = np.concatenate((waypoints0, repeated_rows), axis=0) 118 | elif length0 > length1: 119 | repeat_times = length0 - length1 120 | repeated_rows = np.repeat(waypoints1[-1:], repeat_times, axis=0) 121 | waypoints1 = np.concatenate((waypoints1, repeated_rows), axis=0) 122 | try: 123 | distances = np.linalg.norm(waypoints0[:, 0:2] - waypoints1[:, 0:2], axis=1) 124 | except IndexError: 125 | print("waypoint0: ", length0, waypoints0) 126 | print("waypoint1: ", length1, waypoints1) 127 | # find out the index of waypoints where robot could observe each other 128 | robot_indices = np.argwhere(distances < self.radius) 129 | for robot_index in robot_indices: 130 | # add robot observation factor 131 | measurement = robot_neighbor_factory.add_noise_pose_based(waypoints0[robot_index, :].flatten(), 132 | waypoints1[robot_index, :].flatten()) 133 | if robot_index >= length0: 134 | idx0 = id_initial[0] + length0 - 1 135 | else: 136 | idx0 = id_initial[0] + robot_index 137 | 138 | if robot_index >= length1: 139 | idx1 = id_initial[1] + length1 - 1 140 | else: 141 | idx1 = id_initial[1] + robot_index 142 | graph.add(self.robot_measurement_gtsam_format( 143 | measurement, robot_id, (idx0, idx1))) 144 | return graph 145 | 146 | def generate_virtual_observation_graph(self, frontier_position, robot_id): 147 | graph = gtsam.NonlinearFactorGraph() 148 | initial_estimate = gtsam.Values() 149 | virtual_waypoints = [[] for _ in range(len(self.robot_state_idx))] 150 | for i in range(len(self.robot_state_idx)): 151 | # set the target robot's virtual goal as the frontier position, other robots just reach existing goal 152 | if i == robot_id: 153 | virtual_goal = frontier_position 154 | else: 155 | virtual_goal = self.robot_state_goal[i] 156 | virtual_waypoints[i] = generate_virtual_waypoints(self.robot_state_position[i], 157 | virtual_goal, 158 | speed=self.slam_speed) 159 | self.waypoints2landmark_observations(virtual_waypoints[i], i, 160 | graph=graph, 161 | initial_estimate=initial_estimate) 162 | for i in range(len(self.robot_state_idx)): 163 | if i == robot_id or len(virtual_waypoints[i]) == 0 or len(virtual_waypoints[robot_id]) == 0: 164 | continue 165 | self.waypoints2robot_observation(tuple([virtual_waypoints[robot_id], virtual_waypoints[i]]), 166 | tuple([robot_id, i]), graph=graph) 167 | return graph, initial_estimate 168 | 169 | def optimize_virtual_observation_graph(self, graph: gtsam.NonlinearFactorGraph, initial_estimate: gtsam.Values): 170 | # optimize the graph 171 | # helps nothing but remind me to delete everything after using 172 | isam_copy = gtsam.ISAM2(self.params) 173 | isam_copy.update(self.isam[0], self.isam[1]) 174 | isam_copy.update(graph, initial_estimate) 175 | try: 176 | result = isam_copy.calculateEstimate() 177 | marginals = gtsam.Marginals(isam_copy.getFactorsUnsafe(), result) 178 | except: 179 | print(graph) 180 | print(initial_estimate) 181 | # with open('log.txt', 'a') as file: 182 | # # print(graph, file=file) 183 | # # print(initial_estimate, file=file) 184 | # a = 0 185 | # b = 0 186 | # for key in result.keys(): 187 | # try: 188 | # a+=marginals.marginalCovariance(key).trace() 189 | # b+=np.linalg.det(marginals.marginalCovariance(key)) 190 | # except: 191 | # print(result) 192 | # print(a,b, file=file) 193 | # print(key, marginals.marginalCovariance(key).trace(), 194 | # np.linalg.det(marginals.marginalCovariance(key)), file=file) 195 | 196 | return result, marginals 197 | 198 | def do(self, frontier_position, robot_id, origin, axis): 199 | # return the optimized trajectories of the robots and the marginals for calculation of information matrixc 200 | graph, initial_estimate = self.generate_virtual_observation_graph(frontier_position=frontier_position, 201 | robot_id=robot_id) 202 | result, marginals = self.optimize_virtual_observation_graph(graph, initial_estimate) 203 | result = local_to_world_values(result, origin, robot_id) 204 | 205 | # draw the optimized result 206 | if DEBUG_EM and axis is not None: 207 | scatters_x = [] 208 | scatters_y = [] 209 | for key in result.keys(): 210 | if key < gtsam.symbol('a', 0): 211 | axis.scatter(result.atPoint2(key)[0], result.atPoint2(key)[1], c='r', marker='o') 212 | else: 213 | scatters_x.append(result.atPose2(key).x()) 214 | scatters_y.append(result.atPose2(key).y()) 215 | axis.scatter(scatters_x, scatters_y, c='b', marker='.', alpha=0.1) 216 | return result, marginals 217 | -------------------------------------------------------------------------------- /nav/exp_max.py: -------------------------------------------------------------------------------- 1 | import marinenav_env.envs.marinenav_env as marinenav_env 2 | import numpy as np 3 | import gtsam 4 | import matplotlib as mpl 5 | import matplotlib.pyplot as plt 6 | import matplotlib.cm as cm 7 | from matplotlib.patches import Ellipse 8 | import copy 9 | from APF import APF_agent 10 | from nav.navigation import LandmarkSLAM 11 | from nav.virtualmap import VirtualMap 12 | from nav.frontier import FrontierGenerator, DEBUG_EM, DEBUG_FRONTIER, PLOT_VIRTUAL_MAP 13 | from nav.utils import point_to_local, point_to_world, A_Star 14 | from matplotlib.colors import LinearSegmentedColormap, to_rgba 15 | import time 16 | 17 | DEBUG_EXP_MAX = False 18 | 19 | 20 | def local_goal_to_world_goal(local_goal_p, local_robot_p, world_robot_p): 21 | if isinstance(local_robot_p, gtsam.Pose2): 22 | p_local = [local_robot_p.x(), local_robot_p.y(), local_robot_p.theta()] 23 | else: 24 | p_local = local_robot_p 25 | if isinstance(world_robot_p, gtsam.Pose2): 26 | p_world = [world_robot_p.x(), world_robot_p.y(), world_robot_p.theta()] 27 | else: 28 | p_world = world_robot_p 29 | trans_p = point_to_local(local_goal_p[0], local_goal_p[1], p_local) 30 | world_goal_p = point_to_world(trans_p[0], trans_p[1], p_world) 31 | return world_goal_p 32 | 33 | 34 | def eigsorted(info): 35 | vals, vecs = np.linalg.eigh(info) 36 | vals = 1.0 / vals 37 | order = vals.argsort()[::-1] 38 | return vals[order], vecs[:, order] 39 | 40 | 41 | def plot_info_ellipse(position, info, axis, nstd=.2, **kwargs): 42 | vals, vecs = eigsorted(info) 43 | theta = np.degrees(np.arctan2(*vecs[:, 0][::-1])) 44 | 45 | width, height = 2 * nstd * np.sqrt(vals) 46 | ellip = Ellipse(xy=position, width=width, height=height, angle=theta, **kwargs, color='#444444', alpha=0.3) 47 | 48 | axis.add_artist(ellip) 49 | return ellip 50 | 51 | 52 | class ExpParams: 53 | def __init__(self, cell_size=4, env_width=160, 54 | env_height=200, num_obs=100, 55 | num_cooperative=3, boundary_dist=8, 56 | start_center=np.array([25, 80]), sensor_range=10, 57 | map_path=None): 58 | self.cell_size = cell_size 59 | self.env_width = env_width 60 | self.env_height = env_height 61 | self.num_obs = num_obs 62 | self.num_cooperative = num_cooperative 63 | self.boundary_dist = boundary_dist 64 | self.dpi = 96 65 | self.map_path = map_path 66 | self.start_center = start_center 67 | self.sensor_range = sensor_range 68 | 69 | 70 | class ExpVisualizer: 71 | 72 | def __init__(self, 73 | seed: int = 0, 74 | method: str = "NF", 75 | num: int = 0, 76 | folder_path: str = None, 77 | params: ExpParams = ExpParams() 78 | ): 79 | self.repeat_num = num 80 | params_env = {"width": params.env_width, "height": params.env_height, 81 | "num_obs": params.num_obs,"num_cooperative": params.num_cooperative, 82 | "sensor_range": params.sensor_range} 83 | self.env = marinenav_env.MarineNavEnv2(seed, params = params_env) 84 | self.env.reset(params.start_center) 85 | 86 | if params.map_path is not None: 87 | self.env.load_map(params.map_path) 88 | 89 | self.fig = None # figure for visualization 90 | self.axis_graph = None # sub figure for the map 91 | self.axis_grid = None # sub figure for exploration 92 | 93 | self.robots_plot = [] 94 | self.robots_last_pos = [] 95 | self.robots_traj_plot = [] 96 | 97 | self.dpi = params.dpi # monitor DPI 98 | 99 | self.APF_agents = None 100 | self.a_star = None 101 | 102 | self.initialize_apf_agents() 103 | 104 | self.slam_origin = None 105 | self.landmark_slam = LandmarkSLAM() 106 | self.landmark_slam.reset_graph(len(self.env.robots)) 107 | self.slam_frequency = 10 108 | self.slam_ground_truth = [] 109 | self.exploration_terminate_ratio = 0.85 110 | 111 | param_virtual_map = {"maxX": self.env.width, "maxY": self.env.height, "minX": 0, "minY": 0, 112 | "radius": self.env.robots[0].perception.range, "cell_size": params.cell_size} 113 | self.virtual_map = VirtualMap(param_virtual_map) 114 | self.a_star = A_Star(self.virtual_map.num_rows, self.virtual_map.num_cols, self.virtual_map.cell_size) 115 | 116 | self.color_list = ['#C28AB1', '#70A7D2', '#4B9694', '#2B43A6', '#825AB0', '#F4B940', '#E45F2B'] 117 | 118 | param_frontier = self.virtual_map.get_parameters() 119 | param_frontier["num_robot"] = self.env.num_cooperative 120 | param_frontier["origin"] = [self.env.robots[0].start[0], 121 | self.env.robots[0].start[1], 122 | self.env.robots[0].init_theta] 123 | param_frontier["boundary_dist"] = params.boundary_dist 124 | self.frontier_generator = FrontierGenerator(param_frontier) 125 | 126 | self.slam_result = gtsam.Values() 127 | self.landmark_list = [] 128 | 129 | self.max_exploration_ratio = 0.85 130 | self.max_dist = 1400 131 | 132 | self.cnt = 0 133 | 134 | self.method = method 135 | self.folder_path = folder_path 136 | 137 | self.history = [] 138 | 139 | def init_visualize(self, draw_ground_truth=True): 140 | # Mode 1 (default): Display an episode 141 | self.fig = plt.figure(figsize=(32, 16)) 142 | spec = self.fig.add_gridspec(5, 4) 143 | if draw_ground_truth: 144 | self.axis_graph = self.fig.add_subplot(spec[:, :2]) 145 | self.axis_grid = self.fig.add_subplot(spec[:, 2:4]) 146 | if self.axis_graph is not None: 147 | self.plot_graph(self.axis_graph) 148 | 149 | def plot_grid(self, probability=True, information=True): 150 | if probability: 151 | data = self.virtual_map.get_probability_matrix() 152 | custom_colors = ['#ffffff', '#444444'] 153 | a = to_rgba(custom_colors[0]) 154 | b = to_rgba(custom_colors[1]) 155 | cmap_segments = {'red': [(0.0, a[0], a[0]), 156 | (1.0, b[0], b[0])], 157 | 158 | 'green': [(0.0, a[1], a[1]), 159 | (1.0, b[1], b[1])], 160 | 161 | 'blue': [(0.0, a[2], a[2]), 162 | (1.0, b[2], b[2])]} 163 | custom_cmap = LinearSegmentedColormap('CustomColormap', cmap_segments) 164 | self.axis_grid.imshow(data, origin='lower', alpha=0.5, cmap=custom_cmap, vmin=0.0, vmax=1.0, 165 | extent=[self.virtual_map.minX, self.virtual_map.maxX, 166 | self.virtual_map.minY, self.virtual_map.maxY]) 167 | self.axis_grid.set_xticks([]) 168 | self.axis_grid.set_yticks([]) 169 | self.axis_grid.set_xlim([0, self.env.width]) 170 | self.axis_grid.set_ylim([0, self.env.height]) 171 | if information: 172 | self.virtual_map.update(self.slam_result, self.landmark_slam.marginals) 173 | virtual_map = self.virtual_map.get_virtual_map() 174 | for i, map_row in enumerate(virtual_map): 175 | for j, virtual_landmark in enumerate(map_row): 176 | if virtual_landmark.covariance().trace() == 2.88: 177 | continue 178 | plot_info_ellipse(np.array([virtual_landmark.x, 179 | virtual_landmark.y]), 180 | virtual_landmark.information, self.axis_grid, 181 | nstd=self.virtual_map.cell_size * 0.3) 182 | 183 | def plot_graph(self, axis): 184 | # plot current velocity in the mapf 185 | x_pos = list(np.linspace(-2.5, self.env.width + 2.5, 110)) 186 | y_pos = list(np.linspace(-2.5, self.env.height + 2.5, 110)) 187 | 188 | pos_x = [] 189 | pos_y = [] 190 | arrow_x = [] 191 | arrow_y = [] 192 | speeds = np.zeros((len(x_pos), len(y_pos))) 193 | for m, x in enumerate(x_pos): 194 | for n, y in enumerate(y_pos): 195 | v = self.env.get_velocity(x, y) 196 | speed = np.clip(np.linalg.norm(v), 0.1, 10) 197 | pos_x.append(x) 198 | pos_y.append(y) 199 | arrow_x.append(v[0]) 200 | arrow_y.append(v[1]) 201 | speeds[n, m] = np.log(speed) 202 | 203 | cmap = cm.Blues(np.linspace(0, 1, 20)) 204 | cmap = mpl.colors.ListedColormap(cmap[10:, :-1]) 205 | 206 | axis.contourf(x_pos, y_pos, speeds, cmap=cmap) 207 | axis.quiver(pos_x, pos_y, arrow_x, arrow_y, width=0.001) 208 | 209 | # plot the evaluation boundary 210 | boundary = np.array([[0.0, 0.0], 211 | [self.env.width, 0.0], 212 | [self.env.width, self.env.height], 213 | [0.0, self.env.height], 214 | [0.0, 0.0]]) 215 | axis.plot(boundary[:, 0], boundary[:, 1], color='r', linestyle="-.", linewidth=3) 216 | 217 | # plot obstacles in the map 218 | l = True 219 | for obs in self.env.obstacles: 220 | if l: 221 | axis.add_patch(mpl.patches.Circle((obs.x, obs.y), radius=obs.r, color='m')) 222 | l = False 223 | else: 224 | axis.add_patch(mpl.patches.Circle((obs.x, obs.y), radius=obs.r, color='m')) 225 | 226 | axis.set_aspect('equal') 227 | axis.set_xlim([-2.5, self.env.width + 2.5]) 228 | axis.set_ylim([-2.5, self.env.height + 2.5]) 229 | axis.set_xticks([]) 230 | axis.set_yticks([]) 231 | 232 | # plot start and goal state of each robot 233 | for idx, robot in enumerate(self.env.robots): 234 | axis.scatter(robot.start[0], robot.start[1], marker="o", color="yellow", s=160, zorder=5) 235 | # axis.scatter(robot.goal[0], robot.goal[1], marker="*", color="yellow", s=500, zorder=5) 236 | axis.text(robot.start[0] - 1, robot.start[1] + 1, str(idx), color="yellow", fontsize=15) 237 | # axis.text(robot.goal[0] - 1, robot.goal[1] + 1, str(idx), color="yellow", fontsize=15) 238 | self.robots_last_pos.append([]) 239 | self.robots_traj_plot.append([]) 240 | 241 | self.plot_robots() 242 | 243 | def reset_one_goal(self, goal, idx): 244 | self.env.robots[idx].reset_goal(goal) 245 | 246 | def reset_goal(self, goal_list): 247 | self.env.reset_goal(goal_list) 248 | for idx, goal in enumerate(goal_list): 249 | try: 250 | self.axis_graph.scatter(goal[0], goal[1], marker=".", color="yellow", s=500, zorder=5) 251 | # self.axis_grid.scatter(goal[0], goal[1], marker=".", color="yellow", s=300, zorder=4, alpha=0.5) 252 | except: 253 | print("idx: ", idx) 254 | print("goal: ", goal) 255 | # self.axis_graph.text(goal[0] - 1, goal[1] + 1, str(idx), color="yellow", fontsize=15) 256 | 257 | def plot_robots(self): 258 | for robot_plot in self.robots_plot: 259 | robot_plot.remove() 260 | self.robots_plot.clear() 261 | 262 | for i, robot in enumerate(self.env.robots): 263 | d = np.matrix([[0.5 * robot.length], [0.5 * robot.width]]) 264 | rot = np.matrix([[np.cos(robot.theta), -np.sin(robot.theta)], [np.sin(robot.theta), np.cos(robot.theta)]]) 265 | d_r = rot * d 266 | xy = (robot.x - d_r[0, 0], robot.y - d_r[1, 0]) 267 | 268 | angle_d = robot.theta / np.pi * 180 269 | c = 'g' if robot.cooperative else 'r' 270 | if self.axis_graph is not None: 271 | self.robots_plot.append(self.axis_graph.add_patch(mpl.patches.Rectangle(xy, robot.length, 272 | robot.width, color=c, 273 | angle=angle_d, zorder=7))) 274 | self.robots_plot.append(self.axis_graph.add_patch(mpl.patches.Circle((robot.x, robot.y), 275 | robot.perception.range, color=c, 276 | alpha=0.2))) 277 | self.robots_plot.append( 278 | self.axis_graph.text(robot.x - 1, robot.y + 1, str(i), color="yellow", fontsize=15)) 279 | 280 | if len(self.robots_last_pos[i]) != 0: 281 | h = self.axis_graph.plot((self.robots_last_pos[i][0], robot.x), 282 | (self.robots_last_pos[i][1], robot.y), 283 | color='tab:orange', linestyle='--') 284 | self.robots_traj_plot[i].append(h) 285 | 286 | self.robots_last_pos[i] = [robot.x, robot.y] 287 | 288 | def one_step(self, action, robot_idx=None): 289 | if robot_idx is not None: 290 | rob = self.env.robots[robot_idx] 291 | if not rob.reach_goal: 292 | current_velocity = self.env.get_velocity(rob.x, rob.y) 293 | rob.update_state(action, current_velocity) 294 | 295 | # assert len(action) == len(self.env.robots), "Number of actions not equal number of robots!" 296 | # for i, action in enumerate(action): 297 | # rob = self.env.robots[i] 298 | # if rob.reach_goal: 299 | # continue 300 | # current_velocity = self.env.get_velocity(rob.x, rob.y) 301 | # rob.update_state(action, current_velocity) 302 | # 303 | # self.plot_robots() 304 | # 305 | # self.step += 1 306 | 307 | def initialize_apf_agents(self): 308 | self.APF_agents = [] 309 | for robot in self.env.robots: 310 | self.APF_agents.append(APF_agent(robot.a, robot.w)) 311 | 312 | def slam_one_step(self, observations, video=False, path=None): 313 | obs_list = self.generate_SLAM_observations(observations) 314 | self.landmark_slam.add_one_step(obs_list) 315 | self.slam_result = self.landmark_slam.get_result([self.env.robots[0].start[0], 316 | self.env.robots[0].start[1], 317 | self.env.robots[0].init_theta]) 318 | # self.virtual_map.update(self.slam_result, self.landmark_slam.get_marginal()) 319 | if video: 320 | self.plot_grid() 321 | self.visualize_SLAM() 322 | self.fig.savefig(path + str(self.cnt) + ".png", bbox_inches="tight") 323 | self.cnt += 1 324 | 325 | def navigate_one_step(self, max_ite, path, video=False): 326 | stop_signal = False 327 | 328 | speed = 15 329 | direction_list = [[0, 1], [-1, 1], [-1, 0], [-1, -1], 330 | [0, -1], [1, -1], [1, 0], [1, 1]] 331 | plot_cnt = 0 332 | direction_cnt = [0] * self.env.num_cooperative 333 | while plot_cnt < max_ite and not stop_signal: 334 | plot_signal = False 335 | observations = self.env.get_observations() 336 | if self.cnt % self.slam_frequency == 0: 337 | self.slam_one_step(observations) 338 | self.cnt += 1 339 | actions = [] 340 | for i, apf in enumerate(self.APF_agents): 341 | robot = self.env.robots[i] 342 | if robot.reach_goal: 343 | # if reach a goal, design a new goal 344 | direction_this = direction_list[(direction_cnt[i] + i) % len(direction_list)] 345 | direction_cnt[i] += 1 346 | new_goal = [robot.x + speed * direction_this[0], 347 | robot.y + speed * direction_this[1]] 348 | robot.reset_goal(new_goal) 349 | plot_signal = True 350 | 351 | obstacles = self.landmark_slam.get_landmark_list(self.slam_origin) 352 | # if len(obstacles) != 0: 353 | # vec, vec0, vec1 = observations[i][0] 354 | # goal_new = self.a_star.a_star(vec[:2], vec[4:6], obstacles) 355 | # vec[:2] = goal_new 356 | # observations[i][0] = (vec, vec0, vec1) 357 | actions.append(apf.act(observations[i][0])) 358 | # moving 359 | for i, action in enumerate(actions): 360 | self.one_step(action, robot_idx=i) 361 | if plot_signal: 362 | plot_cnt += 1 363 | if self.plot_grid is not None: 364 | self.axis_grid.cla() 365 | self.virtual_map.update(self.slam_result, self.landmark_slam.get_marginal()) 366 | self.plot_grid() 367 | self.plot_robots() 368 | self.visualize_SLAM() 369 | 370 | self.fig.savefig(path + str(plot_cnt) + ".png", bbox_inches="tight") 371 | return True 372 | 373 | def explore_one_step(self, max_ite, path=None, video=False): 374 | self.slam_origin = [self.env.robots[0].start[0], 375 | self.env.robots[0].start[1], 376 | self.env.robots[0].init_theta] 377 | stop_signal = False 378 | plot_cnt = 0 379 | while plot_cnt < max_ite and not stop_signal: 380 | plot_signal = False 381 | observations = self.env.get_observations() 382 | if self.cnt % self.slam_frequency == 0: 383 | self.slam_one_step(observations) 384 | self.cnt += 1 385 | actions = [] 386 | for i, apf in enumerate(self.APF_agents): 387 | robot = self.env.robots[i] 388 | if robot.reach_goal: 389 | if DEBUG_EM: 390 | with open('log.txt', 'a') as file: 391 | print("No: ", plot_cnt, file=file) 392 | if robot.waiting: 393 | id_that = robot.waiting_for_robot 394 | robot_that = self.env.robots[id_that] 395 | if robot_that.reach_goal: 396 | robot.reset_waiting() 397 | robot_that.reset_waiting() 398 | if not robot.waiting: 399 | # if reach a goal, design a new goal 400 | stop_signal, new_goal = self.generate_frontier(i) 401 | robot.reset_goal(new_goal) 402 | # if we find a rendezvous frontier, we notify the neighbor to wait for us 403 | plot_signal = True 404 | actions.append(apf.act(observations[i][0])) 405 | # moving 406 | for i, action in enumerate(actions): 407 | self.one_step(action, robot_idx=i) 408 | if plot_signal: 409 | if self.axis_grid is not None: 410 | self.plot_grid() 411 | self.plot_robots() 412 | self.visualize_SLAM() 413 | self.fig.savefig(path + str(plot_cnt) + ".png", bbox_inches="tight") 414 | plot_cnt += 1 415 | self.save() 416 | if stop_signal: 417 | return True 418 | else: 419 | return False 420 | 421 | # update robot state and make animation when executing action sequence 422 | def generate_SLAM_observations(self, observations): 423 | # SLAM obs_list 424 | # obs: obs_odom, obs_landmark, obs_robot 425 | # obs_odom: [dx, dy, dtheta] 426 | # obs_landmark: [landmark0:range, bearing, id], [landmark1], ...] 427 | # obs_robot: [obs0: dx, dy, dtheta, id], [obs1], ...] 428 | # ground truth robot: [x_r, y_r, theta_r] 429 | 430 | # env obs_list 431 | # format: {"self": [velocity,goal,pose], 432 | # "static":[[obs_1.x,obs_1.y,obs_1.r, obs_1.id],...,[obs_n.x,obs_n.y,obs_n.r, obs_n.id]], 433 | # "dynamic":{id_1:[[robot_1.x,robot_1.y,robot_1.vx,robot_1.vy]_(t-m),...,[]_t]...} 434 | slam_obs_list = np.empty((len(observations),), dtype=object) 435 | for i, obs in enumerate(observations): 436 | if obs[2]: 437 | continue 438 | self_state, static_states, dynamic_states = obs[0] 439 | self.env.robots[i].odometry.add_noise(self_state[4], self_state[5], self_state[6]) 440 | slam_obs_odom = np.array(self.env.robots[i].odometry.get_odom()) 441 | if len(static_states) != 0: 442 | slam_obs_landmark = np.zeros([len(static_states), 3]) 443 | for j, static_state in enumerate(static_states): 444 | rb = self.env.robots[i].landmark_observation.add_noise(static_state[0], 445 | static_state[1]) 446 | slam_obs_landmark[j, 0] = copy.deepcopy(rb[0]) 447 | slam_obs_landmark[j, 1] = copy.deepcopy(rb[1]) 448 | slam_obs_landmark[j, 2] = copy.deepcopy(static_state[3]) 449 | else: 450 | slam_obs_landmark = [] 451 | if len(dynamic_states) != 0: 452 | slam_obs_robot = np.zeros([len(dynamic_states), 4]) 453 | for j, dynamic_state in enumerate(dynamic_states): 454 | xyt = self.env.robots[i].robot_observation.add_noise(dynamic_state[0], 455 | dynamic_state[1], 456 | dynamic_state[4]) 457 | slam_obs_robot[j, 0] = copy.deepcopy(xyt[0]) 458 | slam_obs_robot[j, 1] = copy.deepcopy(xyt[1]) 459 | slam_obs_robot[j, 2] = copy.deepcopy(xyt[2]) 460 | slam_obs_robot[j, 3] = copy.deepcopy(dynamic_state[5]) 461 | else: 462 | slam_obs_robot = [] 463 | slam_obs_list[i] = [slam_obs_odom, slam_obs_landmark, slam_obs_robot, self_state[4:7]] 464 | return slam_obs_list 465 | 466 | def visualize_SLAM(self, start_idx=0): 467 | # color_list = ['tab:pink', 'tab:green', 'tab:red', 'tab:purple', 'tab:orange', 'tab:gray', 'tab:olive'] 468 | init_x = self.env.robots[0].start[0] 469 | init_y = self.env.robots[0].start[1] 470 | for i in range(self.env.num_cooperative): 471 | pose = self.landmark_slam.get_robot_trajectory(i, [init_x, init_y, 472 | self.env.robots[0].init_theta]) 473 | self.axis_grid.plot(pose[:, 0], pose[:, 1], color=self.color_list[i], linewidth=2, zorder=7) 474 | # self.axis_graph.scatter(pose[:,0],pose[:,1], marker="*", color="pink", s=500, zorder=5) 475 | 476 | for landmark_obs in self.landmark_list: 477 | self.axis_grid.plot(landmark_obs[1], landmark_obs[2], 'x', color='black', zorder=10) 478 | 479 | def draw_present_position(self): 480 | for robot in self.env.robots: 481 | self.axis_graph.scatter(robot.x, robot.y, marker="*", color="yellow", s=500, zorder=5) 482 | 483 | def generate_frontier(self, idx): 484 | self.virtual_map.update(self.slam_result, self.landmark_slam.get_marginal()) 485 | probability_map = self.virtual_map.get_probability_matrix() 486 | latest_state = self.landmark_slam.get_latest_state(self.slam_origin) 487 | self.landmark_list = self.landmark_slam.get_landmark_list(self.slam_origin) 488 | explored_ratio, frontiers_generated = self.frontier_generator.generate(idx, probability_map, 489 | latest_state, 490 | self.landmark_list, self.axis_grid) 491 | 492 | if not frontiers_generated: 493 | self.save() 494 | assert "No more frontiers." 495 | 496 | time0 = time.time() 497 | if self.method == "EM_2": 498 | self.frontier_generator.EMParam["w_t"] = 0 499 | goal, robot_waiting = self.frontier_generator.choose_EM(idx, self.landmark_slam.get_landmark_list(), 500 | self.landmark_slam.get_isam(), 501 | self.landmark_slam.get_last_key_state_pair(), 502 | self.virtual_map, 503 | self.axis_grid) 504 | elif self.method == "EM_3": 505 | goal, robot_waiting = self.frontier_generator.choose_EM(idx, self.landmark_slam.get_landmark_list(), 506 | self.landmark_slam.get_isam(), 507 | self.landmark_slam.get_last_key_state_pair(), 508 | self.virtual_map, 509 | self.axis_grid) 510 | elif self.method == "NF": 511 | goal, robot_waiting = self.frontier_generator.choose_NF(idx) 512 | elif self.method == "CE": 513 | goal, robot_waiting = self.frontier_generator.choose_CE(idx, self.landmark_slam.get_last_key_state_pair()) 514 | elif self.method == "BSP": 515 | goal, robot_waiting = self.frontier_generator.choose_BSP(idx, self.landmark_slam.get_landmark_list(), 516 | self.landmark_slam.get_last_key_state_pair(), 517 | self.axis_grid) 518 | time1 = time.time() 519 | time_this = time1 - time0 520 | dist = self.record_history(explored_ratio, time_this) 521 | if robot_waiting is not None: 522 | # let them wait for each other 523 | self.env.robots[robot_waiting].reset_waiting(idx) 524 | self.env.robots[idx].reset_waiting(robot_waiting) 525 | 526 | slam_poses = self.landmark_slam.get_last_key_state_pair(self.slam_origin) 527 | if self.axis_grid is not None: 528 | if not DEBUG_EXP_MAX and not DEBUG_FRONTIER and not PLOT_VIRTUAL_MAP: 529 | self.axis_grid.cla() 530 | self.axis_grid.scatter(goal[0], goal[1], marker="*", color="#BB421F", s=300, zorder=6) # , alpha=0.5) 531 | self.axis_grid.scatter(latest_state[idx].x(), 532 | latest_state[idx].y(), 533 | marker='*', s=300, c='black', zorder=9) 534 | goal = local_goal_to_world_goal(goal, slam_poses[1][idx], [self.env.robots[idx].x, 535 | self.env.robots[idx].y, 536 | self.env.robots[idx].theta]) 537 | if explored_ratio < self.max_exploration_ratio or dist < self.max_dist: 538 | return False, goal 539 | else: 540 | return True, goal 541 | 542 | def save(self): 543 | filename = self.folder_path + "/" + self.method + "/" + str(self.env.num_obs) + "_" + str( 544 | self.repeat_num) + ".txt" 545 | np.savetxt(filename, np.array(self.history)) 546 | 547 | def record_history(self, exploration_ratio, time_this): 548 | # localization error 549 | err_localization = 0 550 | err_angle = 0 551 | cnt = 0 552 | ground_truth = self.landmark_slam.get_ground_truth() 553 | result = self.landmark_slam.get_result(self.slam_origin) 554 | dist = 0 555 | for key in ground_truth.keys(): 556 | pose_true = ground_truth.atPose2(key) 557 | pose_estimated = result.atPose2(key) 558 | err_localization += np.linalg.norm([pose_true.x() - pose_estimated.x(), 559 | pose_true.y() - pose_estimated.y()]) 560 | err_angle += np.abs(pose_true.theta() - pose_estimated.theta()) 561 | if ground_truth.exists(key + 1): 562 | pose_true_next = ground_truth.atPose2(key + 1) 563 | dist += np.linalg.norm([pose_true.x() - pose_true_next.x(), 564 | pose_true.y() - pose_true_next.y()]) 565 | cnt += 1 566 | err_localization /= len(ground_truth.keys()) 567 | err_angle /= len(ground_truth.keys()) 568 | # landmark error 569 | err_landmark = 0 570 | landmarks_list = self.landmark_slam.get_landmark_list(self.slam_origin) 571 | for landmark in landmarks_list: 572 | landmark_id = landmark[0] 573 | landmark_real = np.array([self.env.obstacles[landmark_id].x, self.env.obstacles[landmark_id].y]) 574 | landmark_estimated = landmark[1:3] 575 | err_landmark += np.linalg.norm(landmark_real - landmark_estimated) 576 | if len(landmarks_list) != 0: 577 | err_landmark /= len(landmarks_list) 578 | self.history.append([dist, err_localization, err_angle, err_landmark, exploration_ratio, time_this]) 579 | if self.axis_grid is not None: 580 | print(len(self.history) - 1 , "dist, err_localization, err_angle, err_landmark, exploration_ratio, time: ", 581 | dist, err_localization, err_angle, err_landmark, exploration_ratio, time_this) 582 | return dist 583 | 584 | def visualize_frontier(self): 585 | # color_list = ['tab:pink', 'tab:green', 'tab:red', 'tab:purple', 'tab:orange', 'tab:gray', 'tab:olive'] 586 | for i in range(0, self.env.num_cooperative): 587 | positions = self.frontier_generator.return_frontiers_position(i) 588 | for position in positions: 589 | self.axis_grid.plot(position[0], position[1], '.', color=self.color_list[i]) 590 | 591 | meeting_position = self.frontier_generator.return_frontiers_rendezvous() 592 | for position in meeting_position: 593 | self.axis_grid.scatter(position[0], position[1], marker='o', facecolors='none', edgecolors='black') 594 | 595 | def draw_virtual_waypoints(self, waypoints, robot_id): 596 | self.axis_grid.plot(waypoints[:, 0], waypoints[:, 1], 597 | marker='.', linestyle='-', color=self.color_list[robot_id], 598 | alpha=0.3, linewidth=.5) 599 | -------------------------------------------------------------------------------- /nav/frontier.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gtsam 3 | import math 4 | from scipy.ndimage import convolve 5 | from matplotlib.patches import Rectangle 6 | 7 | from nav.utils import point_to_local, point_to_world, generate_virtual_waypoints 8 | from nav.BSP import BeliefSpacePlanning, DEBUG_BSP 9 | from nav.EM import ExpectationMaximizationTrajectory, DEBUG_EM 10 | 11 | DEBUG_FRONTIER = False 12 | PLOT_VIRTUAL_MAP = False 13 | 14 | 15 | class Frontier: 16 | def __init__(self, position, origin=None, relative=None, rendezvous=False, nearest_frontier=False): 17 | self.position = position 18 | # relative position in SLAM framework 19 | self.position_local = None 20 | self.nearest_frontier = nearest_frontier 21 | if origin is not None: 22 | self.position_local = point_to_local(position[0], position[1], origin) 23 | 24 | # case rendezvous 25 | self.rendezvous = rendezvous 26 | self.waiting_punishment = None 27 | self.connected_robot = [] 28 | 29 | # case landmark 30 | self.relatives = [] 31 | 32 | # if meet_robot_id is not None: 33 | # self.connected_robot.append(meet_robot_id) 34 | if relative is not None: 35 | self.relatives.append(relative) 36 | 37 | def add_relative(self, relative): 38 | self.relatives.append(relative) 39 | 40 | def add_waiting_punishment(self, robot_id, punishment): 41 | self.connected_robot.append(robot_id) 42 | self.waiting_punishment = punishment 43 | 44 | # def add_robot_connection(self, robot_id): 45 | # self.connected_robot.append(robot_id) 46 | 47 | 48 | def compute_distance(frontier, robot_p): 49 | frontier_p = frontier.position_local 50 | dist_cost = np.sqrt((robot_p.x() - frontier_p[0]) ** 2 + (robot_p.y() - frontier_p[1]) ** 2) 51 | if frontier.rendezvous: 52 | dist_cost += frontier.waiting_punishment 53 | return dist_cost 54 | 55 | 56 | def line_parameters(start_point, end_point): 57 | x1, y1 = start_point 58 | x2, y2 = end_point 59 | A = y2 - y1 60 | B = x1 - x2 61 | C = (x2 * y1) - (x1 * y2) 62 | return A, B, C 63 | 64 | 65 | def distance_to_line_segment(points, line_params, start_point, end_point): 66 | A, B, C = line_params 67 | distances = np.zeros(points.shape[0]) 68 | for i, point in enumerate(points): 69 | x, y = point 70 | dot_product = (x - start_point[0]) * (end_point[0] - start_point[0]) + \ 71 | (y - start_point[1]) * (end_point[1] - start_point[1]) 72 | length_squared = (end_point[0] - start_point[0]) ** 2 + (end_point[1] - start_point[1]) ** 2 73 | 74 | if dot_product < 0: 75 | distance = np.sqrt((x - start_point[0]) ** 2 + (y - start_point[1]) ** 2) 76 | elif dot_product > length_squared: 77 | distance = np.sqrt((x - end_point[0]) ** 2 + (y - end_point[1]) ** 2) 78 | else: 79 | distance = np.abs(A * x + B * y + C) / np.sqrt(A ** 2 + B ** 2) 80 | distances[i] = distance 81 | 82 | return distances 83 | 84 | 85 | class FrontierGenerator: 86 | def __init__(self, parameters): 87 | self.max_x = parameters["maxX"] 88 | self.max_y = parameters["maxY"] 89 | self.min_x = parameters["minX"] 90 | self.min_y = parameters['minY'] 91 | self.cell_size = parameters["cell_size"] 92 | self.num_robot = parameters["num_robot"] 93 | self.radius = parameters["radius"] 94 | self.origin = parameters["origin"] 95 | 96 | self.max_dist_robot = 10 97 | self.min_dist_landmark = 10 98 | self.allocation_max_distance = 30 99 | 100 | self.min_landmark_frontier_cnt = 5 101 | 102 | self.max_dist_robot_scaled = float(self.max_dist_robot) / float(self.cell_size) 103 | self.max_dist_landmark_scaled = float(self.min_dist_landmark) / float(self.cell_size) 104 | 105 | self.free_threshold = 0.45 106 | self.obstacle_threshold = 0.55 107 | self.neighbor_kernel = np.array([[0, 1, 0], 108 | [1, 0, 1], 109 | [0, 1, 0]]) 110 | self.max_visited_neighbor = 2 111 | self.explored_ratio = 0 112 | 113 | self.more_frontiers = False 114 | 115 | self.virtual_move_length = 0.5 116 | 117 | self.EMParam = {"w_d": 10, "w_t": 100, "w_t_degenerate": 1} 118 | self.BSPParam = {"w_d": 5} 119 | self.CEParam = {"w_d": 1, "w_t": 10} 120 | 121 | self.u_t_speed = 10 122 | self.num_history_goal = 5 123 | 124 | self.boundary_dist = parameters[ 125 | "boundary_dist"] # Avoid frontiers near boundaries since our environment actually do not have boundary 126 | self.boundary_value_j = int(self.boundary_dist / self.cell_size) 127 | self.boundary_value_i = int(self.boundary_dist / self.cell_size) 128 | if DEBUG_FRONTIER: 129 | print("boundary value: ", self.boundary_value_i, self.boundary_value_j) 130 | 131 | self.frontiers = None 132 | self.goal_history = [[] for _ in range(self.num_robot)] 133 | 134 | def position_2_index(self, position): 135 | if not isinstance(position, np.ndarray): 136 | index_j = int(math.floor((position[0] - self.min_x) / self.cell_size)) 137 | index_i = int(math.floor((position[1] - self.min_y) / self.cell_size)) 138 | return [index_i, index_j] 139 | elif len(position.shape) == 1: 140 | index_j = int(math.floor((position[0] - self.min_x) / self.cell_size)) 141 | index_i = int(math.floor((position[1] - self.min_y) / self.cell_size)) 142 | return [index_i, index_j] 143 | else: 144 | indices = np.zeros_like(position) 145 | indices[:, 1] = np.floor((position[:, 0] - self.min_x) / self.cell_size) 146 | indices[:, 0] = np.floor((position[:, 1] - self.min_y) / self.cell_size) 147 | return indices 148 | 149 | def index_2_position(self, index): 150 | if not isinstance(index, np.ndarray): 151 | x = index[1] * self.cell_size + self.cell_size * .5 + self.min_x 152 | y = index[0] * self.cell_size + self.cell_size * .5 + self.min_y 153 | return [x, y] 154 | elif len(index.shape) == 1: 155 | x = index[1] * self.cell_size + self.cell_size * .5 + self.min_x 156 | y = index[0] * self.cell_size + self.cell_size * .5 + self.min_y 157 | return [x, y] 158 | else: 159 | positions = np.zeros_like(index) 160 | positions[:, 0] = index[:, 1] * self.cell_size + self.cell_size * .5 + self.min_x 161 | positions[:, 1] = index[:, 0] * self.cell_size + self.cell_size * .5 + self.min_y 162 | return positions 163 | 164 | def generate_potential_frontier_indices(self, probability_map, max_visited_neighbor): 165 | # generate frontier candidates 166 | data_free = probability_map < self.free_threshold 167 | data_occupied = probability_map > self.obstacle_threshold 168 | data = data_free | data_occupied 169 | 170 | explored_ratio = np.mean(data) 171 | self.explored_ratio = explored_ratio 172 | # print("Present exploration ratio: ", explored_ratio) 173 | 174 | neighbor_sum = convolve(data.astype(int), self.neighbor_kernel, mode='constant', cval=0) - data.astype(int) 175 | data[0:self.boundary_value_i, :] = False # avoid the boundary being selected 176 | data[-self.boundary_value_i:, :] = False 177 | data[:, 0:self.boundary_value_j] = False 178 | data[:, -self.boundary_value_j:] = False 179 | 180 | frontier_candidate_pool_data = data & (neighbor_sum < max_visited_neighbor) 181 | indices = np.argwhere(frontier_candidate_pool_data) 182 | if DEBUG_FRONTIER: 183 | print("indices: ", indices) 184 | return explored_ratio, indices 185 | 186 | def generate(self, robot_id, probability_map, state_list, landmark_list, axis=None): 187 | # probability_map: 2d-array, value: [0,1] 188 | # robot_id: int, id of the robot to generate frontiers 189 | # state_list: [gtsam.Pose2, ...] 190 | # landmarks_list: [[id, x, y], ...] 191 | if len(state_list) != self.num_robot: 192 | print("state_list: ", state_list) 193 | print("num_robot: ", self.num_robot) 194 | raise ValueError("len(state_list) not equal to num of robots!") 195 | explored_ratio, indices = self.generate_potential_frontier_indices(probability_map, self.max_visited_neighbor) 196 | if len(indices) == 0: 197 | return explored_ratio, False 198 | 199 | # clear history 200 | self.frontiers = {} 201 | 202 | # state and index of the robot who needs a new goal (target robot) 203 | state = state_list[robot_id] 204 | state_index = np.array(self.position_2_index([state.x(), state.y()])) 205 | 206 | # find the frontiers close enough to the target robot 207 | 208 | # Type 1 frontier, the nearest frontier to current position 209 | # _, indices_ = self.generate_potential_frontier_indices(probability_map, max_visited_neighbor=3) 210 | # distance between the target robot abd frontier candidates 211 | distances = np.linalg.norm(indices - state_index, axis=1) 212 | indices_distances_within_list = np.argwhere(distances < self.max_dist_robot_scaled) 213 | index_this = np.argmin(distances) 214 | position_this = self.index_2_position(indices[index_this]) 215 | 216 | if index_this in self.frontiers: 217 | self.frontiers[index_this].nearest_frontier = True 218 | else: 219 | self.frontiers[index_this] = Frontier(position_this, 220 | origin=self.origin, 221 | nearest_frontier=True) 222 | if DEBUG_FRONTIER: 223 | print("robot ", robot_id) 224 | print("distances: ", distances) 225 | print("index: ", index_this, position_this) 226 | if self.explored_ratio < 0.2: 227 | for index_in_range in indices_distances_within_list: 228 | if index_in_range[0] not in self.frontiers: 229 | position_this = self.index_2_position(indices[index_in_range[0]]) 230 | self.frontiers[index_in_range[0]] = Frontier(position_this, origin=self.origin) 231 | 232 | # Type 2 frontier, re-visitation of existing landmarks 233 | landmark_frontier_cnt = 0 234 | # frontiers near landmarks 235 | for landmark in landmark_list: 236 | landmark_index = np.array(self.position_2_index([landmark[1], landmark[2]])) 237 | distances = np.linalg.norm(indices - landmark_index, axis=1) 238 | # find the candidates close enough to the landmark 239 | indices_distances_within_list = np.argwhere((distances >self.max_dist_robot_scaled) & 240 | (distances < self.max_dist_landmark_scaled)) 241 | 242 | for index_this in indices_distances_within_list: 243 | if index_this[0] in self.frontiers: 244 | self.frontiers[index_this[0]].add_relative(landmark[0]) 245 | else: 246 | position_this = self.index_2_position(indices[index_this[0]]) 247 | self.frontiers[index_this[0]] = Frontier(position_this, origin=self.origin, 248 | relative=landmark[0]) 249 | landmark_frontier_cnt += 1 250 | 251 | if (self.more_frontiers or len(self.frontiers) < self.min_landmark_frontier_cnt) and len(landmark_list) != 0: 252 | # frontiers with landmarks on the way there 253 | landmark_array = np.array(landmark_list)[:, 1:] 254 | landmark_array_index = self.position_2_index(landmark_array) 255 | distances = np.linalg.norm(landmark_array_index - state_index, axis=1) 256 | num_landmarks_close = np.count_nonzero(distances < self.max_dist_robot_scaled) 257 | for i, index_this in enumerate(indices): 258 | line_params = line_parameters(state_index, index_this) 259 | distances = distance_to_line_segment(landmark_array_index, line_params, state_index, index_this) 260 | # if there are landmarks on the way there 261 | if np.count_nonzero(distances < self.max_dist_landmark_scaled) > num_landmarks_close: 262 | landmark_min = np.argmin(distances) 263 | if i in self.frontiers: 264 | self.frontiers[i].add_relative(landmark_list[landmark_min][0]) 265 | else: 266 | position_this = self.index_2_position(index_this) 267 | self.frontiers[i] = Frontier(position_this, origin=self.origin, 268 | relative=landmark_list[landmark_min][0]) 269 | # landmark_frontier_cnt += 1 270 | 271 | # if (self.more_frontiers or len(self.frontiers) < self.min_landmark_frontier_cnt) and len(landmark_list) != 0: 272 | # landmark_array = np.array(landmark_list)[:, :] 273 | # landmark_array= sorted(landmark_array, 274 | # key=lambda elem: np.linalg.norm([state.x(), state.y()] - elem[1:])) 275 | # distances = [np.linalg.norm([state.x(), state.y()] - elem[1:]) for elem in landmark_array] 276 | # dt = int(len(landmark_array) / self.min_landmark_frontier_cnt) 277 | # for i, landmark_this in enumerate(landmark_array): 278 | # if len(self.frontiers) > self.min_landmark_frontier_cnt: 279 | # break 280 | # # if there are landmarks on the way there 281 | # if i % dt == 0 and distances[i] > self.radius: 282 | # # print("landmark revisit: ", landmark_this, ) 283 | # self.frontiers[i+len(indices)] = Frontier(landmark_this[1:] + [self.radius/2, self.radius/2], 284 | # origin=self.origin, 285 | # relative=int(landmark_this[0])) 286 | # landmark_frontier_cnt += 1 287 | 288 | # Type 3 frontier,meet with other robots 289 | if landmark_frontier_cnt < self.min_landmark_frontier_cnt: 290 | for robot_index in range(self.num_robot): 291 | if robot_index == robot_id: 292 | continue # you don't need to meet yourself 293 | if len(self.goal_history[robot_index]) == 0: 294 | continue # the robot has no goal 295 | goal_this = self.goal_history[robot_index][-1] 296 | # my time to the goal 297 | dist_this = np.sqrt((state.x() - goal_this[0]) ** 2 + (state.y() - goal_this[1]) ** 2) 298 | if dist_this < self.max_dist_robot: 299 | continue # the robot is too close to the goal 300 | self.frontiers[len(indices) + robot_index] = Frontier(goal_this, 301 | origin=self.origin, 302 | rendezvous=True) 303 | 304 | # neighbor's time to the goal 305 | dist_that = np.sqrt((state_list[robot_index].x() - goal_this[0]) ** 2 + 306 | (state_list[robot_index].y() - goal_this[1]) ** 2) 307 | self.frontiers[len(indices) + robot_index]. \ 308 | add_waiting_punishment(robot_id=robot_index, 309 | punishment=abs(dist_this - dist_that)) 310 | 311 | if DEBUG_FRONTIER: 312 | for frontier in self.frontiers.values(): 313 | print("frontier: ", frontier.position) 314 | 315 | return explored_ratio, True 316 | 317 | def find_frontier_nearest_neighbor(self): 318 | for value in self.frontiers.values(): 319 | if value.nearest_frontier: 320 | return value.position 321 | 322 | def draw_frontiers(self, axis): 323 | if DEBUG_FRONTIER: 324 | axis.cla() 325 | scatters_x = [] 326 | scatters_y = [] 327 | for frontier in self.frontiers.values(): 328 | scatters_x.append(frontier.position[0]) 329 | scatters_y.append(frontier.position[1]) 330 | axis.scatter(scatters_x, scatters_y, c='y', marker='.') 331 | if PLOT_VIRTUAL_MAP and axis is not None: 332 | axis.cla() 333 | for frontier in self.frontiers.values(): 334 | if frontier.nearest_frontier: 335 | axis.scatter(frontier.position[0], frontier.position[1], 336 | c='#59ABA9', marker='o', s=300, zorder=5) 337 | elif frontier.rendezvous: 338 | axis.scatter(frontier.position[0], frontier.position[1], 339 | c='#865eb3', marker='o', s=300, zorder=5) 340 | elif len(frontier.relatives) == 0: 341 | axis.scatter(frontier.position[0], frontier.position[1], 342 | c='#4D9CD7', marker='o', s=300, zorder=5) 343 | # else: 344 | # axis.scatter(frontier.position[0], frontier.position[1], 345 | # c='#59ABA9', marker='o', s=300, zorder=5) 346 | 347 | def choose_NF(self, robot_id): 348 | goal = self.find_frontier_nearest_neighbor() 349 | self.goal_history[robot_id].append(np.array(goal)) 350 | return goal, None 351 | 352 | def choose_CE(self, robot_id, robot_state_idx_position_local): 353 | robot_p = robot_state_idx_position_local[1][robot_id] 354 | 355 | num_history_goal = min(self.num_history_goal, len(self.goal_history[robot_id])) 356 | goals_task_allocation = self.goal_history[robot_id][-num_history_goal:] 357 | goals_task_allocation.append(point_to_world(robot_p.x(), robot_p.y(), self.origin)) 358 | cost_list = [] 359 | for key, frontier in self.frontiers.items(): 360 | u_t = 0 361 | for goal in goals_task_allocation: 362 | pts = generate_virtual_waypoints(goal, frontier.position, speed=self.u_t_speed) 363 | if len(pts) == 0: 364 | pts = [frontier.position] 365 | u_t += self.compute_utility_task_allocation(pts, robot_id, True) 366 | # calculate the landmark visitation and new exploration case first 367 | u_d = compute_distance(frontier, robot_p) 368 | cost_list.append((key, self.CEParam["w_t"] * u_t + self.CEParam["w_d"] * u_d)) 369 | if len(cost_list) == 0: 370 | # if no frontier is available, return the nearest frontier 371 | goal = self.find_frontier_nearest_neighbor() 372 | else: 373 | min_cost = min(cost_list, key=lambda tuple_item: tuple_item[1]) 374 | goal = self.frontiers[min_cost[0]].position 375 | return goal, None 376 | 377 | def choose_BSP(self, robot_id, landmark_list_local, robot_state_idx_position_local, axis=None): 378 | if any(not sublist for sublist in self.goal_history): 379 | goal = self.find_frontier_nearest_neighbor() 380 | self.goal_history[robot_id].append(goal) 381 | return goal, None 382 | newest_goal_local = [] 383 | for goals in self.goal_history: 384 | newest_goal_local.append(point_to_local(goals[-1][0], goals[-1][1], self.origin)) 385 | robot_state_idx_position_goal_local = robot_state_idx_position_local + (newest_goal_local,) 386 | bsp = BeliefSpacePlanning(radius=self.radius, 387 | robot_state_idx_position_goal=robot_state_idx_position_goal_local, 388 | landmarks=landmark_list_local) 389 | robot_waiting = None 390 | cost_list = [] 391 | for key, frontier in self.frontiers.items(): 392 | result, marginals = bsp.do(robot_id=robot_id, 393 | frontier_position=frontier.position_local, 394 | origin=self.origin, 395 | axis=axis) 396 | cost_this = self.compute_utility_BSP(result, marginals, 397 | robot_state_idx_position_local[1][robot_id], 398 | frontier) 399 | cost_list.append((key, cost_this)) 400 | if len(cost_list) == 0: 401 | goal = self.find_frontier_nearest_neighbor() 402 | else: 403 | min_cost = min(cost_list, key=lambda tuple_item: tuple_item[1]) 404 | if self.frontiers[min_cost[0]].rendezvous: 405 | robot_waiting = self.frontiers[min_cost[0]].connected_robot[0] 406 | goal = self.frontiers[min_cost[0]].position 407 | self.goal_history[robot_id].append(goal) 408 | if DEBUG_BSP: 409 | with open('log.txt', 'a') as file: 410 | print("goal: ", goal, file=file) 411 | return goal, robot_waiting 412 | 413 | def choose_EM(self, robot_id, landmark_list_local, isam, robot_state_idx_position_local, virtual_map, 414 | axis=None): 415 | if any(not sublist for sublist in self.goal_history): 416 | goal = self.find_frontier_nearest_neighbor() 417 | self.goal_history[robot_id].append(goal) 418 | return goal, None 419 | 420 | newest_goal_local = [] 421 | for goals in self.goal_history: 422 | newest_goal_local.append(point_to_local(goals[-1][0], goals[-1][1], self.origin)) 423 | robot_state_idx_position_goal_local = robot_state_idx_position_local + (newest_goal_local,) 424 | emt = ExpectationMaximizationTrajectory(radius=self.radius, 425 | robot_state_idx_position_goal=robot_state_idx_position_goal_local, 426 | landmarks=landmark_list_local, 427 | isam=isam) 428 | self.draw_frontiers(axis) 429 | robot_waiting = None 430 | cost_list = [] 431 | for key, frontier in self.frontiers.items(): 432 | # return the transformed virtual SLAM result for the calculation of the information of virtual map 433 | result, marginals = emt.do(robot_id=robot_id, 434 | frontier_position=frontier.position_local, 435 | origin=self.origin, 436 | axis=axis) 437 | 438 | # no need to reset, since the update_information will reset the virtual map 439 | cost_this = self.compute_utility_EM(virtual_map, result, marginals, 440 | robot_state_idx_position_local[1][robot_id], 441 | frontier, robot_id) 442 | cost_list.append((key, cost_this)) 443 | if len(cost_list) == 0: 444 | # if no frontier is available, return the nearest frontier 445 | goal = self.find_frontier_nearest_neighbor() 446 | else: 447 | min_cost = min(cost_list, key=lambda tuple_item: tuple_item[1]) 448 | if self.frontiers[min_cost[0]].rendezvous: 449 | robot_waiting = self.frontiers[min_cost[0]].connected_robot[0] 450 | goal = self.frontiers[min_cost[0]].position 451 | self.goal_history[robot_id].append(goal) 452 | if DEBUG_EM: 453 | with open('log.txt', 'a') as file: 454 | print("goal: ", goal, file=file) 455 | return goal, robot_waiting 456 | 457 | def compute_utility_BSP(self, result: gtsam.Values, marginals: gtsam.Marginals, 458 | robot_p, frontier): 459 | u_d = compute_distance(frontier, robot_p) 460 | u_m = 0 461 | for key in result.keys(): 462 | if key < gtsam.symbol('a', 0): 463 | pass 464 | else: 465 | pose = result.atPose2(key) 466 | try: 467 | marginal = marginals.marginalInformation(key) 468 | u_m += np.sqrt(marginals.marginalCovariance(key).trace()) 469 | except RuntimeError: 470 | marginal = np.zeros((3, 3)) 471 | u_m += 1000 472 | if DEBUG_BSP: 473 | with open('log.txt', 'a') as file: 474 | print("u_m, u_d: ", u_m, u_d, file=file) 475 | return u_m + self.BSPParam["w_d"] * u_d 476 | 477 | def compute_utility_EM(self, virtual_map, result: gtsam.Values, marginals: gtsam.Marginals, 478 | robot_p, frontier, robot_id): 479 | # robot_position could be a tuple of two robots 480 | # calculate the cost of the frontier for a specific robot locally 481 | virtual_map.reset_information() 482 | virtual_map.update_information(result, marginals) 483 | u_m = virtual_map.get_sum_uncertainty() 484 | num_history_goal = min(self.num_history_goal, len(self.goal_history[robot_id])) 485 | goals_task_allocation = self.goal_history[robot_id][-num_history_goal:] 486 | goals_task_allocation.append(point_to_world(robot_p.x(), robot_p.y(), self.origin)) 487 | u_t = 0 488 | for goal in goals_task_allocation: 489 | pts = generate_virtual_waypoints(np.array(goal), frontier.position, speed=self.u_t_speed) 490 | if len(pts) == 0: 491 | pts = [frontier.position] 492 | u_t += self.compute_utility_task_allocation(pts, robot_id) 493 | # calculate the landmark visitation and new exploration case first 494 | u_d = compute_distance(frontier, robot_p) 495 | w_t = max(1 - self.explored_ratio * self.EMParam["w_t_degenerate"], 0) 496 | if DEBUG_EM: 497 | with open('log.txt', 'a') as file: 498 | print("robot id, robot position, frontier position: ", robot_id, robot_p, 499 | frontier.position_local, frontier.position, file=file) 500 | print("uncertainty & task allocation & distance cost & sum: ", u_m, u_t, u_d, 501 | u_m + u_t * self.EMParam["w_t"] * w_t + u_d * self.EMParam["w_d"], file=file) 502 | return u_m + u_t * self.EMParam["w_t"] * w_t + u_d * self.EMParam["w_d"] * self.explored_ratio 503 | 504 | def compute_utility_task_allocation(self, frontier_w_list, robot_id, ce_flag=False): 505 | # local frame to global frame 506 | u_t = 0 507 | for frontier_w in frontier_w_list: 508 | for i, goal_list in enumerate(self.goal_history): 509 | if i == robot_id: 510 | continue 511 | for goal in goal_list: 512 | dist = np.sqrt((goal[0] - frontier_w[0]) ** 2 + (goal[1] - frontier_w[1]) ** 2) 513 | u_t += self.compute_P_d(dist, ce_flag) 514 | return u_t 515 | 516 | def compute_P_d(self, dist, ce_flag=False): 517 | if ce_flag: 518 | allocation_max_distance = self.allocation_max_distance 519 | else: 520 | allocation_max_distance = self.allocation_max_distance * \ 521 | (1 - self.explored_ratio * self.EMParam["w_t_degenerate"]) 522 | if dist < allocation_max_distance: 523 | P_d = 1 - dist / allocation_max_distance 524 | else: 525 | P_d = 0 526 | return P_d 527 | 528 | def return_frontiers_position(self, robot_id): 529 | frontiers = [] 530 | for value in self.frontiers.values(): 531 | if robot_id in value.connected_robot: 532 | frontiers.append(value.position) 533 | for pair in value.connected_robot_pair: 534 | if robot_id in pair: 535 | frontiers.append(value.position) 536 | return frontiers 537 | 538 | def return_frontiers_rendezvous(self): 539 | frontiers = [] 540 | for value in self.frontiers.values(): 541 | if len(value.connected_robot_pair) != 0: 542 | frontiers.append(value.position) 543 | return frontiers 544 | -------------------------------------------------------------------------------- /nav/navigation.py: -------------------------------------------------------------------------------- 1 | import copy 2 | 3 | import numpy as np 4 | import gtsam 5 | from nav.utils import get_symbol, local_to_world_values 6 | 7 | DEBUG_NAV = False 8 | 9 | 10 | class LandmarkSLAM: 11 | def __init__(self): 12 | # Create a factor graph to hold the constraints 13 | self.graph = gtsam.NonlinearFactorGraph() 14 | self.initial_recorded = gtsam.Values() 15 | self.initial = gtsam.Values() 16 | self.result = gtsam.Values() 17 | self.marginals = [] 18 | params = gtsam.ISAM2Params() 19 | params.setFactorization("QR") 20 | self.isam = gtsam.ISAM2(params) 21 | 22 | # GROUND TRUTH is in world frame, only used for evaluation 23 | self.ground_truth = gtsam.Values() 24 | 25 | self.idx = [] 26 | # Noise models for the prior 27 | self.prior_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.0001, 0.0001, 0.000001]) 28 | # Noise models for the odometry 29 | self.odom_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.03, 0.03, 0.004]) 30 | # Noise models for the range bearing measurements 31 | self.range_bearing_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.004, 0.001]) 32 | # self.range_bearing_noise_model = gtsam.noiseModel.Robust.Create( 33 | # gtsam.noiseModel.mEstimator.Cauchy.Create(0.1), gtsam.noiseModel.Diagonal.Sigmas([0.004, 0.001])) 34 | # Noise models for the robot observations 35 | self.robot_noise_model = gtsam.noiseModel.Diagonal.Sigmas([0.1, 0.1, 0.004]) 36 | # self.robot_noise_model = gtsam.noiseModel.Robust.Create( 37 | # gtsam.noiseModel.mEstimator.Cauchy.Create(0.1), gtsam.noiseModel.Diagonal.Sigmas([0.01, 0.01, 0.004])) 38 | 39 | self.parameters = gtsam.LevenbergMarquardtParams() 40 | 41 | # for deugging 42 | # self.landmark_list = [[] for _ in range(28)] 43 | 44 | def reset_graph(self, num_robot): 45 | self.graph.resize(0) 46 | self.initial.clear() 47 | self.idx = [] 48 | self.idl = 0 49 | 50 | for i in range(0, num_robot): 51 | self.idx.append(-1) 52 | 53 | def add_prior(self, idx: gtsam.symbol, pose: gtsam.Pose2): 54 | self.graph.add(gtsam.PriorFactorPose2(idx, pose, self.prior_noise_model)) 55 | 56 | def add_odom(self, idx0: gtsam.symbol, idx1: gtsam.symbol, pose: gtsam.Pose2): 57 | self.graph.add(gtsam.BetweenFactorPose2(idx0, idx1, pose, self.odom_noise_model)) 58 | 59 | # bearing: radius, range: meter 60 | def add_bearing_range(self, idx: gtsam.symbol, idl: int, range: float, bearing: float): 61 | self.graph.add(gtsam.BearingRangeFactor2D(idx, idl, 62 | gtsam.Rot2(bearing), range, 63 | self.range_bearing_noise_model)) 64 | 65 | def add_robot_observation(self, idx0: gtsam.symbol, idx1: gtsam.symbol, pose: gtsam.Pose2): 66 | self.graph.add(gtsam.BetweenFactorPose2(idx0, idx1, pose, self.robot_noise_model)) 67 | # if self.initial.exists(idx0): 68 | # pose_1 = self.initial.atPose2(idx0).compose(pose) 69 | # if self.initial.exists(idx1): 70 | # dpose = self.initial.atPose2(idx1).between(pose_1) 71 | # print("Residual calculated pose: ", idx1, " from ", idx0, ":",dpose) 72 | 73 | def add_initial_pose(self, idx: gtsam.symbol, pose: gtsam.Pose2): 74 | self.initial.insert(idx, pose) 75 | 76 | def add_initial_landmark(self, idl, landmark: gtsam.Point2): 77 | if not self.initial.exists(idl): 78 | self.initial.insert(idl, landmark) 79 | 80 | def optimize(self): 81 | # optimizer = gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, self.parameters) 82 | # result = optimizer.optimize() 83 | self.initial_recorded.insert(self.initial) 84 | self.isam.update(self.graph, self.initial) 85 | self.graph.resize(0) 86 | self.initial.clear() 87 | self.result = self.isam.calculateEstimate() 88 | self.marginals = gtsam.Marginals(self.isam.getFactorsUnsafe(), self.result) 89 | if DEBUG_NAV: 90 | print("SLAM result:", self.result) 91 | print("SLAM graph: ", self.isam.getFactorsUnsafe()) 92 | 93 | def get_robot_value_initial(self, robot_id, idx): 94 | if self.initial.exists(gtsam.symbol(chr(robot_id + ord('a')), idx)): 95 | return self.initial.atPose2(gtsam.symbol(chr(robot_id + ord('a')), idx)) 96 | 97 | def get_robot_value_result(self, robot_id, idx): 98 | if self.result.exists(gtsam.symbol(chr(robot_id + ord('a')), idx)): 99 | return self.result.atPose2(gtsam.symbol(chr(robot_id + ord('a')), idx)) 100 | 101 | def get_landmark_value_initial(self, idl): 102 | return self.initial.atPoint2(idl) 103 | 104 | def get_robot_trajectory(self, robot_id, origin): 105 | pose2_list = np.zeros([self.idx[robot_id] + 1, 3]) 106 | origin_pose = gtsam.Pose2(origin[0], origin[1], origin[2]) 107 | for i in range(self.idx[robot_id] + 1): 108 | pose = self.result.atPose2(get_symbol(robot_id, i)) 109 | pose = origin_pose.compose(pose) 110 | pose2_list[i, 0] = pose.x() 111 | pose2_list[i, 1] = pose.y() 112 | pose2_list[i, 2] = pose.theta() 113 | return pose2_list 114 | 115 | def get_landmark_list(self, origin=None): 116 | # return: [[id, x, y], ...] 117 | if origin is None: 118 | origin = [0, 0, 0] 119 | landmark_list = [] 120 | # print(origin) 121 | origin_pose = gtsam.Pose2(origin[0], origin[1], origin[2]) 122 | for key in self.result.keys(): 123 | if key < gtsam.symbol('a', 0): 124 | landmark_position = self.result.atPoint2(key) 125 | if landmark_position is not None: 126 | position_this = origin_pose.transformFrom(landmark_position) 127 | landmark_list.append([key, position_this[0], position_this[1]]) 128 | return landmark_list 129 | 130 | def get_result(self, origin=None): 131 | if origin is None: 132 | origin = [0, 0, 0] 133 | return local_to_world_values(self.result, origin) 134 | 135 | def get_latest_state(self, origin=None): 136 | if origin is None: 137 | origin = [0, 0, 0] 138 | state_list = [[] for _ in range(len(self.idx))] 139 | origin_pose = gtsam.Pose2(origin[0], origin[1], origin[2]) 140 | for i, key_int in enumerate(self.idx): 141 | key = get_symbol(i, key_int) 142 | pose = self.result.atPose2(key) 143 | state_list[i] = origin_pose.compose(pose) 144 | return state_list 145 | 146 | def get_last_key_state_pair(self, origin=None): 147 | if origin is None: 148 | origin = [0, 0, 0] 149 | state_list = [[] for _ in range(len(self.idx))] 150 | origin_pose = gtsam.Pose2(origin[0], origin[1], origin[2]) 151 | key_list = copy.deepcopy(self.idx) 152 | for i, key_int in enumerate(key_list): 153 | key = get_symbol(i, key_int) 154 | pose = self.result.atPose2(key) 155 | state_list[i] = origin_pose.compose(pose) 156 | return key_list, state_list 157 | 158 | def init_SLAM(self, robot_id, obs_robot): 159 | initialized = False 160 | if robot_id == 0: 161 | self.add_prior(gtsam.symbol(chr(robot_id + ord('a')), 0), gtsam.Pose2(0, 0, 0)) 162 | self.add_initial_pose(gtsam.symbol(chr(robot_id + ord('a')), 0), gtsam.Pose2(0, 0, 0)) 163 | initialized = True 164 | elif len(obs_robot) != 0: 165 | for obs_r_this in obs_robot: 166 | idr = int(obs_r_this[3]) 167 | if idr == 0: 168 | self.add_initial_pose(gtsam.symbol(chr(robot_id + ord('a')), 0), 169 | gtsam.Pose2(0, 0, 0).compose( 170 | gtsam.Pose2(obs_r_this[0], obs_r_this[1], obs_r_this[2]).inverse() 171 | )) 172 | initialized = True 173 | 174 | if not initialized: 175 | raise ValueError("Fail to initialize SLAM graph") 176 | 177 | # TODO: add keyframe strategy 178 | def add_one_step(self, obs_list): 179 | # obs: obs_odom, obs_landmark, obs_robot 180 | # obs_odom: [dx, dy, dtheta] 181 | # obs_landmark: [landmark0:range, bearing, id], [landmark1], ...] 182 | # obs_robot: [obs0: dx, dy, dtheta, id], [obs1], ...] 183 | if np.all(np.equal(obs_list, None)): 184 | return 185 | 186 | for i, obs in enumerate(obs_list): 187 | if not obs: 188 | continue 189 | self.idx[i] += 1 190 | obs_odom, obs_landmark, obs_robot, ground_truth = obs 191 | if self.idx[i] == 0: 192 | # add initial pose 193 | self.init_SLAM(i, obs_robot) 194 | ground_truth_pose = gtsam.Pose2(ground_truth[0], ground_truth[1], ground_truth[2]) 195 | self.ground_truth.insert(gtsam.symbol(chr(i + ord('a')), self.idx[i]), ground_truth_pose) 196 | else: 197 | # add odometry 198 | pre_pose = self.get_robot_value_result(i, self.idx[i] - 1) 199 | if not pre_pose: 200 | pre_pose = self.get_robot_value_initial(i, self.idx[i] - 1) 201 | initial_pose = pre_pose * gtsam.Pose2(obs_odom[0], obs_odom[1], obs_odom[2]) 202 | self.add_initial_pose(gtsam.symbol(chr(i + ord('a')), self.idx[i]), initial_pose) 203 | ground_truth_pose = gtsam.Pose2(ground_truth[0], ground_truth[1], ground_truth[2]) 204 | self.ground_truth.insert(gtsam.symbol(chr(i + ord('a')), self.idx[i]), ground_truth_pose) 205 | 206 | self.add_odom(gtsam.symbol(chr(i + ord('a')), self.idx[i] - 1), 207 | gtsam.symbol(chr(i + ord('a')), self.idx[i]), 208 | gtsam.Pose2(obs_odom[0], obs_odom[1], obs_odom[2])) 209 | 210 | if len(obs_landmark) != 0: 211 | # print("landmark: ", obs_landmark) 212 | for obs_l_this in obs_landmark: 213 | r, b, idl = obs_l_this 214 | idl = int(idl) 215 | # add landmark initial 216 | # self.landmark_list[idl].append(initial_pose.transformFrom( 217 | # gtsam.Point2(r * np.cos(b), r * np.sin(b)))) 218 | initial_pose = self.get_robot_value_initial(i, self.idx[i]) 219 | if not (self.initial.exists(idl) or self.result.exists(idl)): 220 | self.add_initial_landmark(idl, initial_pose.transformFrom( 221 | gtsam.Point2(r * np.cos(b), r * np.sin(b)))) 222 | # add landmark observation 223 | self.add_bearing_range(gtsam.symbol(chr(i + ord('a')), self.idx[i]), idl, r, b) 224 | 225 | if len(obs_robot) != 0: 226 | # print("idx: ", self.idx) 227 | for obs_r_this in obs_robot: 228 | idr = int(obs_r_this[3]) 229 | # add landmark observation 230 | if idr > i and obs_list[idr] is not None: 231 | # This means the latest id of robot neighbor is not yet updated to this timestamp 232 | self.add_robot_observation(gtsam.symbol(chr(i + ord('a')), self.idx[i]), 233 | gtsam.symbol(chr(idr + ord('a')), self.idx[idr] + 1), 234 | gtsam.Pose2(obs_r_this[0], obs_r_this[1], obs_r_this[2])) 235 | else: 236 | self.add_robot_observation(gtsam.symbol(chr(i + ord('a')), self.idx[i]), 237 | gtsam.symbol(chr(idr + ord('a')), self.idx[idr]), 238 | gtsam.Pose2(obs_r_this[0], obs_r_this[1], obs_r_this[2])) 239 | 240 | self.optimize() 241 | 242 | def get_marginal(self): 243 | return self.marginals 244 | 245 | def get_isam(self): 246 | return self.isam.getFactorsUnsafe(), self.result 247 | 248 | def get_ground_truth(self): 249 | return self.ground_truth -------------------------------------------------------------------------------- /nav/utils.py: -------------------------------------------------------------------------------- 1 | import gtsam 2 | import numpy as np 3 | from gtsam import symbol 4 | import networkx as nx 5 | 6 | 7 | def get_symbol(robot_id, idx): 8 | return symbol(chr(robot_id + ord('a')), idx) 9 | 10 | 11 | def theta_0_to_2pi(theta): 12 | while theta < 0.0: 13 | theta += 2 * np.pi 14 | while theta >= 2 * np.pi: 15 | theta -= 2 * np.pi 16 | return theta 17 | 18 | 19 | def world_to_local(x, y, theta, origin): 20 | # compute transformation from world frame to robot frame 21 | R_wr, t_wr = pose_vector_to_matrix(origin[0], origin[1], origin[2]) 22 | R_rw = np.transpose(R_wr) 23 | t_rw = -R_rw * t_wr 24 | 25 | R_this, t_this = pose_vector_to_matrix(x, y, theta) 26 | t_this = R_rw * t_this + t_rw 27 | 28 | return t_this[0, 0], t_this[1, 0], theta_0_to_2pi(theta - origin[2]) 29 | 30 | 31 | def point_to_local(x, y, origin): 32 | # compute transformation from world frame to robot frame 33 | R_wr, t_wr = pose_vector_to_matrix(origin[0], origin[1], origin[2]) 34 | R_rw = np.transpose(R_wr) 35 | t_rw = -R_rw * t_wr 36 | 37 | t_this = R_rw * np.matrix([[x], [y]]) + t_rw 38 | return [t_this[0, 0], t_this[1, 0]] 39 | 40 | 41 | def point_to_world(x, y, origin): 42 | # compute transformation from robot frame to world frame 43 | R_wr, t_wr = pose_vector_to_matrix(origin[0], origin[1], origin[2]) 44 | 45 | t_this = R_wr * np.matrix([[x], [y]]) + t_wr 46 | return [t_this[0, 0], t_this[1, 0]] 47 | 48 | 49 | def local_to_world_values(values: gtsam.Values, origin, robot_id=None): 50 | result = gtsam.Values() 51 | origin_pose = gtsam.Pose2(origin[0], origin[1], origin[2]) 52 | if robot_id is not None: 53 | key_min = chr(robot_id + ord('a')) 54 | key_max = chr(robot_id + ord('a') + 1) 55 | else: 56 | key_min = 0 57 | key_max = -1 58 | for key in values.keys(): 59 | if key < gtsam.symbol('a', 0): 60 | landmark_position = values.atPoint2(key) 61 | if landmark_position is not None: 62 | result.insert(key, origin_pose.transformFrom(landmark_position)) 63 | elif robot_id is not None: 64 | if gtsam.symbol(key_min, 0) <= key < gtsam.symbol(key_max, 0): 65 | robot_pose = values.atPose2(key) 66 | if robot_pose is not None: 67 | result.insert(key, origin_pose.compose(robot_pose)) 68 | else: 69 | robot_pose = values.atPose2(key) 70 | if robot_pose is not None: 71 | result.insert(key, origin_pose.compose(robot_pose)) 72 | return result 73 | 74 | 75 | def pose_vector_to_matrix(x, y, theta): 76 | # compose matrix expression of pose vector 77 | R_rw = np.matrix([[np.cos(theta), -np.sin(theta)], 78 | [np.sin(theta), np.cos(theta)]]) 79 | t_rw = np.matrix([[x], [y]]) 80 | 81 | return R_rw, t_rw 82 | 83 | 84 | def from_cos_sin(c, s): 85 | theta = np.arccos(c) 86 | if s < 0: 87 | theta = 2 * np.pi - theta 88 | return theta 89 | 90 | 91 | def generate_virtual_waypoints(state_this, state_next, speed): 92 | # return numpy.ndarray 93 | # [[x0,y0,theta0], ..., [x1,y1,theta1]] 94 | # state_this = self.robot_state_position[robot_id] 95 | if isinstance(state_this, gtsam.Pose2): 96 | state_0 = np.array([state_this.x(), state_this.y(), state_this.theta()]) 97 | elif isinstance(state_this, np.ndarray) or isinstance(state_next, list): 98 | state_0 = np.array([state_this[0], state_this[1], 0]) 99 | else: 100 | print(state_this, state_this.type) 101 | raise ValueError("Only accept gtsam.Pose2 and numpy.ndarray") 102 | if isinstance(state_next, gtsam.Pose2): 103 | state_1 = np.array([state_next.x(), state_next.y(), state_next.theta()]) 104 | elif isinstance(state_next, np.ndarray) or isinstance(state_next, list): 105 | state_1 = np.array([state_next[0], state_next[1], 0]) 106 | else: 107 | raise ValueError("Only accept gtsam.Pose2 and numpy.ndarray") 108 | 109 | step = int(np.linalg.norm(state_1[0:2] - state_0[0:2]) / speed) 110 | waypoints = np.linspace(state_0, state_1, step) 111 | 112 | return waypoints.tolist() 113 | 114 | 115 | def heuristic(node, goal): 116 | return np.norm(node[0] - goal[0]) + np.norm(node[1] - goal[1]) 117 | 118 | 119 | class A_Star: 120 | def __init__(self, rows, cols, cell_size): 121 | self.rows_scaled = int(rows / cell_size) 122 | self.cols_scaled = int(cols / cell_size) 123 | self.cell_size = cell_size 124 | 125 | def a_star(self, start, goal, obstacles): 126 | G = nx.grid_2d_graph(self.rows_scaled, self.cols_scaled) 127 | 128 | # Set edge weights (uniform in this case) 129 | for node in G.nodes(): 130 | G.nodes[node]['weight'] = 1 131 | 132 | # Set some obstacles by removing nodes 133 | obstacles_scaled = [] 134 | for obstacle in obstacles: 135 | for i in range(-1,2): 136 | for j in range(-1,2): 137 | obstacles_scaled.append((int(obstacle[1] / self.cell_size)+i, int(obstacle[2] / self.cell_size)+j)) 138 | G.remove_nodes_from(obstacles) 139 | 140 | # Find the shortest path using A* algorithm 141 | path = nx.astar_path(G, start, goal, heuristic=heuristic, weight='weight') 142 | path = np.array(path) * self.cell_size 143 | 144 | # Plot the graph and path 145 | return path[1, :] 146 | -------------------------------------------------------------------------------- /nav/virtualmap.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import gtsam 4 | from scipy.spatial.distance import cdist 5 | from scipy.linalg import cho_factor, cho_solve 6 | from nav.utils import from_cos_sin 7 | import copy 8 | import time 9 | import torch 10 | 11 | from marinenav_env.envs.utils.robot import RangeBearingMeasurement 12 | 13 | DEBUG = False 14 | 15 | 16 | def prob_to_logodds(p): 17 | return math.log(p / (1.0 - p)) 18 | 19 | 20 | def logodds_to_prob(l): 21 | return math.exp(l) / (1.0 + math.exp(l)) 22 | 23 | 24 | LOGODDS_FREE = prob_to_logodds(0.3) 25 | LOGODDS_UNKNOWN = prob_to_logodds(0.5) 26 | LOGODDS_OCCUPIED = prob_to_logodds(0.7) 27 | MIN_LOGODDS = prob_to_logodds(0.05) 28 | MAX_LOGODDS = prob_to_logodds(0.95) 29 | FREE_THRESH = prob_to_logodds(0.5) 30 | OCCUPIED_THRESH = prob_to_logodds(0.5) 31 | 32 | 33 | def get_logodds(free: bool): 34 | if free: 35 | return LOGODDS_FREE 36 | else: 37 | return LOGODDS_OCCUPIED 38 | 39 | 40 | class VirtualLandmark: 41 | def __init__(self, probability: float, x: float, y: float): 42 | self.sigma = 1.2 # for information matrix 43 | self.updated = False # True: Covariance once visited 44 | self.probability = probability # probability of being actual landmark 45 | self.x = x 46 | self.y = y 47 | self.information = None 48 | self.reset_information() 49 | 50 | def covariance(self): 51 | return np.linalg.pinv(self.information) 52 | 53 | def update_information(self, information): 54 | self.information = copy.deepcopy(information) 55 | 56 | def update_probability(self, probability): 57 | self.probability = copy.deepcopy(probability) 58 | 59 | def reset_information(self, information=None): 60 | if information is None: 61 | init_information = np.array([[1 / (self.sigma ** 2), 0], [0, 1 / (self.sigma ** 2)]]) 62 | self.information = init_information 63 | 64 | else: 65 | self.information = information 66 | 67 | def update_information_weighted(self, information): 68 | I_0 = self.information 69 | I_1 = information 70 | # a = |I_0| b = |I_1| 71 | a = np.linalg.det(I_0) 72 | b = np.linalg.det(I_1) 73 | # I_0 * x = I_1 x = I_0^{-1} * I_1 74 | # c = a * tr(x) 75 | 76 | c = a * np.trace(cho_solve(cho_factor(I_0), I_1)) 77 | d = a + b - c 78 | w = 0.5 * (2 * b - c) / d 79 | if (w < 0 and d < 0) or (w > 1 and d < 0): 80 | w = float(0) 81 | else: 82 | w = float(1) 83 | self.information = w * I_0 + (1 - w) * I_1 84 | 85 | def set_updated(self, signal=None): 86 | if signal is None: 87 | self.updated = False 88 | else: 89 | self.updated = signal 90 | 91 | 92 | class OccupancyMap: 93 | def __init__(self, min_x: float, min_y: float, 94 | max_x: float, max_y: float, 95 | cell_size: float, radius: float): 96 | self.minX = min_x 97 | self.minY = min_y 98 | self.maxX = max_x 99 | self.maxY = max_y 100 | self.cell_size = cell_size 101 | self.radius = radius 102 | self.num_cols = int(math.floor((self.maxX - self.minX) / self.cell_size)) 103 | self.num_rows = int(math.floor((self.maxY - self.minY) / self.cell_size)) 104 | self.data = np.full((self.num_rows, self.num_cols), LOGODDS_UNKNOWN) 105 | 106 | def update_grid(self, col: int, row: int, free: bool): 107 | if col < 0 or col >= self.num_cols or row < 0 or row >= self.num_rows: 108 | return 109 | logodds = get_logodds(free) + self.data[row, col] 110 | logodds = min(MAX_LOGODDS, max(logodds, MIN_LOGODDS)) 111 | self.data[row, col] = logodds 112 | 113 | def update_landmark(self, point): 114 | col = int((point[0] - self.minX) / self.cell_size) 115 | row = int((point[1] - self.minY) / self.cell_size) 116 | self.update_grid(col, row, False) 117 | 118 | def update_robot(self, point): 119 | col = (point[0] - self.minX) / self.cell_size 120 | row = (point[1] - self.minY) / self.cell_size 121 | col_int = int(col) 122 | row_int = int(row) 123 | radius = math.ceil(self.radius / self.cell_size) 124 | min_col = max(col_int - radius, 0) 125 | min_row = max(row_int - radius, 0) 126 | max_col = min(col_int + radius, self.num_cols) 127 | max_row = min(row_int + radius, self.num_rows) 128 | # ignore the part outside the region of interest 129 | if max_row - min_row <= 0 or max_col - min_col <= 0: 130 | return 131 | indices = np.indices((max_row - min_row, max_col - min_col)).reshape(2, -1).T 132 | indices[:, 0] += min_row 133 | indices[:, 1] += min_col 134 | indices_float = indices.astype(float) 135 | indices_float[:, :] += 0.5 136 | distances = cdist(indices_float, np.array([[row, col]]), 'euclidean').flatten() 137 | indices_within = np.where(distances < radius)[0] 138 | for idx in indices_within: 139 | self.update_grid(indices[idx][1], indices[idx][0], True) 140 | 141 | def reset(self): 142 | self.data = np.full((self.num_rows, self.num_cols), LOGODDS_UNKNOWN) 143 | 144 | def update(self, slam_result: gtsam.Values): 145 | for key in slam_result.keys(): 146 | if key < gtsam.symbol('a', 0): # landmark case 147 | self.update_landmark(slam_result.atPoint2(key)) 148 | else: # robot case 149 | pose = slam_result.atPose2(key) 150 | self.update_robot(np.array([pose.x(), pose.y()])) 151 | 152 | def to_probability(self): 153 | data = np.vectorize(logodds_to_prob)(self.data) 154 | return data 155 | 156 | def from_probability(self, data): 157 | self.data = np.vectorize(prob_to_logodds)(data) 158 | 159 | 160 | def get_range_pose_point(pose, point): 161 | t_ = pose[:2] 162 | d = point - t_ 163 | r = np.linalg.norm(d) 164 | if r < 1e-6: 165 | return r, np.zeros(3), np.zeros(2) 166 | D_r_d = d / r 167 | 168 | c_p = np.cos(pose[2]) 169 | s_p = np.sin(pose[2]) 170 | 171 | D_d_pose = np.array([[-c_p, s_p, 0.0], 172 | [-s_p, -c_p, 0.0]]) 173 | Hpose = np.matmul(D_r_d, D_d_pose) 174 | Hpoint = D_r_d 175 | 176 | return r, Hpose, Hpoint 177 | 178 | 179 | def get_range_pose_point_batch(pose_batch, point_batch): 180 | t_batch = pose_batch[:, :2] 181 | d_batch = point_batch - t_batch 182 | r_batch = torch.norm(d_batch, dim=1) 183 | 184 | D_r_d_batch = d_batch / r_batch.unsqueeze(-1) 185 | 186 | c_p_batch = torch.cos(pose_batch[:, 2]) 187 | s_p_batch = torch.sin(pose_batch[:, 2]) 188 | 189 | D_d_pose_batch = torch.stack([ 190 | torch.stack([-c_p_batch, s_p_batch, torch.zeros_like(c_p_batch)]), 191 | torch.stack([-s_p_batch, -c_p_batch, torch.zeros_like(c_p_batch)]) 192 | ], dim=0) 193 | D_d_pose_batch = D_d_pose_batch.permute(2, 0, 1) 194 | 195 | Hpose_batch = torch.matmul(D_r_d_batch.unsqueeze(1), D_d_pose_batch) 196 | Hpoint_batch = D_r_d_batch 197 | 198 | return nan_2_zero(r_batch), nan_2_zero(Hpose_batch.squeeze()), nan_2_zero(Hpoint_batch) 199 | 200 | 201 | def nan_2_zero(x): 202 | mask_nan = torch.isnan(x) 203 | return torch.where(mask_nan, torch.tensor(0.0), x) 204 | 205 | 206 | def get_bearing_pose_point(pose, point, jacobian=True): 207 | if jacobian: 208 | theta = pose[2] 209 | d, D_d_pose, D_d_point = unrotate(theta, point - pose[:2]) 210 | result, D_result_d = relative_bearing(d) 211 | Hpose = np.matmul(D_result_d, D_d_pose) 212 | Hpoint = np.matmul(D_result_d, D_d_point) 213 | return result, Hpose, Hpoint 214 | else: 215 | theta = pose[2] 216 | d, _, _ = unrotate(theta, point - pose[:2]) 217 | return relative_bearing(d) 218 | 219 | 220 | def get_bearing_pose_point_batch(pose_batch, point_batch): 221 | theta_batch = pose_batch[:, 2] 222 | d_batch, D_d_pose_batch, D_d_point_batch = unrotate_batch(theta_batch, point_batch - pose_batch[:, :2]) 223 | result_batch, D_result_d_batch = relative_bearing_batch(d_batch) 224 | Hpose_batch = torch.matmul(D_result_d_batch.unsqueeze(1), D_d_pose_batch) 225 | Hpoint_batch = torch.matmul(D_result_d_batch.unsqueeze(1), D_d_point_batch) 226 | return result_batch, Hpose_batch.squeeze(), Hpoint_batch.squeeze() 227 | 228 | 229 | def unrotate(theta, p): 230 | c = np.cos(theta) 231 | s = np.sin(theta) 232 | q = np.array([c * p[0] + s * p[1], -s * p[0] + c * p[1]]) 233 | H1 = np.array([[-1.0, 0.0, q[1]], [0.0, -1.0, -q[0]]]) 234 | H2 = np.array([[c, s], [-s, c]]) 235 | return q, H1, H2 236 | 237 | 238 | def unrotate_batch(theta_batch, p_batch): 239 | c = torch.cos(theta_batch) 240 | s = torch.sin(theta_batch) 241 | q = torch.stack([c * p_batch[:, 0] + s * p_batch[:, 1], 242 | -s * p_batch[:, 0] + c * p_batch[:, 1]], dim=1) 243 | H11 = torch.stack([-torch.ones_like(c), torch.zeros_like(c), q[:, 1]], dim=1) 244 | H12 = torch.stack([torch.zeros_like(c), -torch.ones_like(c), -q[:, 0]], dim=1) 245 | H1 = torch.stack([H11, H12], dim=1) 246 | H2 = torch.stack([torch.stack([c, s], dim=1), torch.stack([-s, c], dim=1)], dim=1) 247 | 248 | return q, H1, H2 249 | 250 | 251 | def from_cos_sin_batch(c_batch, s_batch): 252 | theta_batch = torch.acos(c_batch) 253 | mask = s_batch < 0 254 | theta_batch[mask] = 2 * np.pi - theta_batch[mask] 255 | return theta_batch 256 | 257 | 258 | def relative_bearing(d, jacobian=True): 259 | if jacobian: 260 | x = d[0] 261 | y = d[1] 262 | d2 = x ** 2 + y ** 2 263 | n = np.sqrt(d2) 264 | if np.abs(n) > 1e-5: 265 | return from_cos_sin(x / n, y / n), np.array([-y / d2, x / d2]) 266 | else: 267 | return 0, np.array([0, 0]) 268 | else: 269 | x = d[0] 270 | y = d[1] 271 | d2 = x ** 2 + y ** 2 272 | n = np.sqrt(d2) 273 | if np.abs(n) > 1e-5: 274 | return from_cos_sin(x / n, y / n) 275 | else: 276 | return 0 277 | 278 | 279 | def relative_bearing_batch(d_batch): 280 | x = d_batch[:, 0] 281 | y = d_batch[:, 1] 282 | d2 = x ** 2 + y ** 2 283 | n = torch.sqrt(d2) 284 | theta_batch = torch.zeros_like(n) 285 | H = torch.stack([torch.zeros_like(n), torch.zeros_like(n)], dim=1) 286 | mask = n > 1e-5 287 | theta_batch[mask] = from_cos_sin_batch(x[mask] / n[mask], y[mask] / n[mask]) 288 | H[mask, :] = torch.stack([-y[mask] / d2[mask], x[mask] / d2[mask]], dim=1) 289 | return theta_batch, H 290 | 291 | 292 | def pose_2_point_measurement(pose: np.ndarray, point, sigma_b, sigma_r, jacobian: bool): 293 | sigmas = np.array([[sigma_b], [sigma_r]]) 294 | if not jacobian: 295 | bearing = get_bearing_pose_point(pose, point, False) 296 | range = get_range_pose_point(pose, point, False) 297 | return [bearing, range, sigmas] 298 | # bearing 299 | # range 300 | # Sigmas 2 by 1 301 | else: 302 | bearing, Hx_bearing, Hl_bearing = get_bearing_pose_point(pose, point) 303 | range, Hx_range, Hl_range = get_range_pose_point(pose, point) 304 | # bearing 305 | # range 306 | # Sigmas 2 by 1 307 | # Hx_bearing_range 2 by 3 308 | # Hl_bearing_range 2 by 2 309 | return [bearing, range, sigmas, 310 | np.concatenate(([Hx_bearing], [Hx_range]), axis=0), 311 | np.concatenate(([Hl_bearing], [Hl_range]), axis=0)] 312 | 313 | 314 | def pose_2_point_measurement_batch(pose_batch, point_batch): 315 | n1, n2, n3 = point_batch.shape 316 | n5, n4 = pose_batch.shape 317 | pose_batch = pose_batch.repeat(1, n2).reshape(n5, n2, n4) 318 | 319 | bearing_batch, Hx_bearing_batch, Hl_bearing_batch = get_bearing_pose_point_batch(pose_batch.view(-1, n4), 320 | point_batch.view(-1, n3)) 321 | range_batch, Hx_range_batch, Hl_range_batch = get_range_pose_point_batch(pose_batch.view(-1, n4), 322 | point_batch.view(-1, n3)) 323 | # bearing 324 | # range 325 | # Sigmas 2 by 1 326 | # Hx_bearing_range 2 by 3 327 | # Hl_bearing_range 2 by 2 328 | Hx = torch.stack([Hx_bearing_batch, Hx_range_batch], dim=1) 329 | Hl = torch.stack([Hl_bearing_batch, Hl_range_batch], dim=1) 330 | return [bearing_batch.view(n1, n2), range_batch.view(n1, n2), Hx.view(n1, n2, 2, 3), Hl.view(n1, n2, 2, 2)] 331 | 332 | 333 | def predict_virtual_landmark(state: np.ndarray, information_matrix, 334 | virtual_landmark_position, sigma_range, sigma_bearing): 335 | bearing, range, sigmas, Hx, Hl = pose_2_point_measurement(state, 336 | virtual_landmark_position, 337 | sigma_b=sigma_bearing, 338 | sigma_r=sigma_range, 339 | jacobian=True) 340 | try: 341 | R = np.diag(np.squeeze(sigmas)) ** 2 342 | # Hl_Hl_Hl = (Hl^T * Hl)^{-1}* Hl^T 343 | # cov = Hl_Hl_Hl * [Hx * Info_Mat^{-1} * Hx^T + R] * Hl_Hl_Hl^T 344 | Hl_Hl_Hl = np.matmul(np.linalg.pinv(np.matmul(Hl.transpose(), Hl)), Hl.transpose()) 345 | A = np.matmul(Hx, cho_solve(cho_factor(information_matrix), Hx.transpose())) + R 346 | cov = np.matmul(np.matmul(Hl_Hl_Hl, A), Hl_Hl_Hl.transpose()) 347 | except: 348 | print("Hx, Hl:", Hx, Hl) 349 | print("R:", R) 350 | return np.linalg.pinv(cov) 351 | 352 | 353 | # process virtual landmark data in batch 354 | def predict_virtual_landmark_batch(Hx, Hl, sigmas, information_matrix): 355 | # Compute R for the entire batch 356 | R_batch = torch.diag_embed(sigmas) ** 2 357 | 358 | # Compute Hl_Hl_Hl for the entire batch 359 | Hl_transpose = Hl.transpose(2, 3) 360 | Hl_Hl_Hl_batch = torch.matmul(torch.pinverse(torch.matmul(Hl_transpose, Hl)), Hl_transpose) 361 | L_batch = torch.linalg.cholesky(information_matrix) 362 | 363 | # Compute A for the entire batch 364 | A_batch = torch.matmul(Hx, torch.cholesky_solve(Hx.transpose(2, 3), 365 | L_batch[:, None, :, :])) + R_batch[None, None, :, :] 366 | 367 | # Compute cov for the entire batch 368 | cov_batch = torch.matmul(torch.matmul(Hl_Hl_Hl_batch, A_batch), Hl_Hl_Hl_batch.transpose(2, 3)) 369 | info_batch = torch.pinverse(cov_batch) 370 | return info_batch 371 | 372 | 373 | class VirtualMap: 374 | def __init__(self, parameters): 375 | self.maxX = parameters["maxX"] 376 | self.maxY = parameters["maxY"] 377 | self.minX = parameters["minX"] 378 | self.minY = parameters["minY"] 379 | self.radius = parameters["radius"] 380 | self.cell_size = parameters["cell_size"] # cell size for virtual map 381 | self.num_cols = int(math.floor((self.maxX - self.minX) / self.cell_size)) 382 | self.num_rows = int(math.floor((self.maxY - self.minY) / self.cell_size)) 383 | 384 | self.data = np.empty((self.num_rows, self.num_cols), dtype=object) 385 | 386 | self.range_bearing_model = RangeBearingMeasurement() 387 | self.use_torch = True 388 | 389 | if self.use_torch: 390 | self.indices_within = None 391 | self.generate_sensor_model() 392 | 393 | # Initialize occupancy map with unknown grid 394 | for i in range(0, self.num_rows): 395 | for j in range(0, self.num_cols): 396 | x = j * self.cell_size + self.cell_size * .5 + self.minX 397 | y = i * self.cell_size + self.cell_size * .5 + self.minY 398 | self.data[i, j] = VirtualLandmark(0, x, y) 399 | 400 | def generate_sensor_model(self): 401 | radius = math.ceil(self.radius / self.cell_size) 402 | grid = torch.meshgrid(torch.arange(-radius, radius, dtype=torch.int32), 403 | torch.arange(-radius, radius, dtype=torch.int32), 404 | indexing='ij') 405 | indices = torch.stack(grid, dim=-1).view(-1, 2) 406 | indices_float = indices.to(torch.float32) 407 | distances = torch.norm(indices_float, dim=-1) 408 | mask = distances < radius 409 | self.indices_within = indices[mask] 410 | 411 | def get_parameters(self): 412 | param = {"maxX": self.maxX, "minX": self.minX, "maxY": self.maxY, "minY": self.minY, 413 | "cell_size": self.cell_size, "radius": self.radius} 414 | return param 415 | 416 | def reset_probability(self, data=None): 417 | if data is None: 418 | np.vectorize(lambda obj, prob: obj.update_probability(prob))(self.data, np.zeros(self.data.shape)) 419 | else: 420 | np.vectorize(lambda obj, prob: obj.update_probability(prob))(self.data, data) 421 | 422 | def reset_information(self): 423 | np.vectorize(lambda obj: obj.reset_information())(self.data) 424 | 425 | def update_probability(self, slam_result: gtsam.Values): 426 | occmap = OccupancyMap(self.minX, self.minY, 427 | self.maxX, self.maxY, 428 | self.cell_size, self.radius) 429 | occmap.update(slam_result) 430 | self.reset_probability(occmap.to_probability()) 431 | 432 | def update_probability_robot(self, pose): 433 | occmap = OccupancyMap(self.minX, self.minY, 434 | self.maxX, self.maxY, 435 | self.cell_size, self.radius) 436 | occmap.from_probability(self.data[:, :].probability) 437 | occmap.update_robot([np.array([pose.x(), pose.y()])]) 438 | 439 | self.reset_probability(occmap.to_probability()) 440 | 441 | def update_probability_data(self, point): 442 | occmap = OccupancyMap(self.minX, self.minY, 443 | self.maxX, self.maxY, 444 | self.cell_size, self.radius) 445 | occmap.from_probability(self.data[:, :].probability) 446 | occmap.update_landmark(point) 447 | self.reset_probability(occmap.to_probability()) 448 | 449 | def update_information(self, slam_result: gtsam.Values, marginals: gtsam.Marginals, signal=True): 450 | np.vectorize(lambda obj, prob: obj.set_updated(prob))(self.data, np.full(self.data.shape, False)) 451 | self.reset_information() 452 | time0 = time.time() 453 | # if len(slam_result.keys()) * self.cell_size < 100 or not self.use_torch: 454 | if not self.use_torch: 455 | for key in slam_result.keys(): 456 | if key < gtsam.symbol('a', 0): # landmark case 457 | pass 458 | else: # robot case 459 | pose = slam_result.atPose2(key) 460 | self.update_information_robot(np.array([pose.x(), pose.y(), pose.theta()]), 461 | marginals.marginalInformation(key), signal) 462 | else: 463 | poses_array = [] 464 | information_matrix_array = [] 465 | cnt = 0 466 | for key in slam_result.keys(): 467 | if key < gtsam.symbol('a', 0): # landmark case 468 | pass 469 | else: 470 | pose = slam_result.atPose2(key) 471 | poses_array.append(np.array([pose.x(), pose.y(), pose.theta()])) 472 | try: 473 | marginal = marginals.marginalInformation(key) 474 | except RuntimeError: 475 | marginal = np.zeros((3, 3)) 476 | information_matrix_array.append(marginal) 477 | cnt += 1 478 | self.update_information_robot_batch(torch.tensor(np.array(poses_array)), 479 | torch.tensor(np.array(information_matrix_array)), signal) 480 | time1 = time.time() 481 | if DEBUG: 482 | with open('log.txt', 'a') as file: 483 | print("time information: ", time1 - time0, file=file) 484 | 485 | def update_information_robot_batch(self, poses, information_matrix, signal): 486 | indices, points = self.find_neighbor_indices_batch(poses[:, :2]) 487 | _, _, Hx, Hl = pose_2_point_measurement_batch(poses, points) 488 | sigmas = torch.tensor(np.array([self.range_bearing_model.sigma_b, self.range_bearing_model.sigma_r])) 489 | 490 | info_batch = predict_virtual_landmark_batch(Hx, Hl, sigmas, information_matrix) 491 | info_batch = info_batch.view(-1, 2, 2) 492 | for n, [i, j] in enumerate(indices.view(-1, 2).numpy()): 493 | if i < 0 or i >= self.num_rows or j < 0 or j >= self.num_cols: 494 | continue 495 | # not yet part of the map 496 | # if self.data[i, j].probability < 0.49 and signal: 497 | # continue 498 | if self.data[i, j].updated: 499 | self.data[i, j].update_information_weighted(info_batch[n, :, :]) 500 | else: 501 | self.data[i, j].reset_information(info_batch[n, :, :]) 502 | self.data[i, j].set_updated() 503 | 504 | def find_neighbor_indices(self, point): 505 | col = (point[0] - self.minX) / self.cell_size 506 | row = (point[1] - self.minY) / self.cell_size 507 | col_int = int(col) 508 | row_int = int(row) 509 | radius = math.ceil(self.radius / self.cell_size) 510 | min_col = max(col_int - radius, 0) 511 | min_row = max(row_int - radius, 0) 512 | max_col = min(col_int + radius, self.num_cols) 513 | max_row = min(row_int + radius, self.num_rows) 514 | indices = np.indices((max_row - min_row, max_col - min_col)).reshape(2, -1).T 515 | indices[:, 0] += min_row 516 | indices[:, 1] += min_col 517 | indices_float = indices.astype(float) 518 | indices_float[:, :] += 0.5 519 | distances = cdist(indices_float, np.array([[row, col]]), 'euclidean').flatten() 520 | indices_within = np.where(distances < radius)[0] 521 | return indices[indices_within] 522 | 523 | def find_neighbor_indices_batch(self, point_batch): 524 | col = (point_batch[:, 0] - self.minX) / self.cell_size 525 | row = (point_batch[:, 1] - self.minY) / self.cell_size 526 | center_batch = torch.stack([torch.round(row).to(dtype=torch.int32), torch.round(col).to(dtype=torch.int32)], 527 | dim=-1) 528 | radius = math.ceil(self.radius / self.cell_size) 529 | indices = self.indices_within.clone() 530 | indices = indices + center_batch[:, None, :] 531 | return indices, self.indices_batch_2_xy_batch(indices) 532 | 533 | def indices_batch_2_xy_batch(self, indices): 534 | x = indices[:, :, 1] * self.cell_size + self.cell_size * .5 + self.minX 535 | y = indices[:, :, 0] * self.cell_size + self.cell_size * .5 + self.minY 536 | return torch.stack([x, y], dim=-1) 537 | 538 | def update_information_robot(self, state: np.ndarray, information_matrix, signal): 539 | indices = self.find_neighbor_indices(np.array([state[0], state[1]])) 540 | for [i, j] in indices: 541 | # if self.data[i, j].probability < 0.49 and signal: 542 | # continue 543 | 544 | info_this = predict_virtual_landmark(state, 545 | information_matrix, 546 | np.array([self.data[i, j].x, self.data[i, j].y]), 547 | sigma_range=self.range_bearing_model.sigma_r, 548 | sigma_bearing=self.range_bearing_model.sigma_b) 549 | if self.data[i, j].updated: 550 | self.data[i, j].update_information_weighted(info_this) 551 | else: 552 | self.data[i, j].reset_information(info_this) 553 | self.data[i, j].set_updated() 554 | 555 | def update(self, values: gtsam.Values, marginals: gtsam.Marginals = None): 556 | self.update_probability(values) 557 | if marginals is not None: 558 | self.update_information(values, marginals, False) 559 | 560 | 561 | def get_probability_matrix(self): 562 | probability_matrix = np.vectorize(lambda obj: obj.probability)(self.data) 563 | return probability_matrix 564 | 565 | def get_virtual_map(self): 566 | return self.data 567 | 568 | def get_sum_uncertainty(self, type_optima="A"): 569 | sum_uncertainty = 0.0 570 | for i in range(0, self.num_rows): 571 | for j in range(0, self.num_cols): 572 | # not yet observed or explored 573 | # with open('log.txt', 'a') as file: 574 | # print(self.data[i, j].probability, np.trace(self.data[i, j].covariance()),file=file) 575 | # if self.data[i, j].probability > 0.49: 576 | # continue 577 | # cnt += 1 578 | if type_optima == "A": 579 | if np.trace(self.data[i, j].information) < 1e-6: 580 | sum_uncertainty += 1000000 581 | if DEBUG: 582 | with open('log_covariance.txt', 'a') as file: 583 | print("information:", self.data[i, j].information, 584 | np.linalg.det(self.data[i, j].information), file=file) 585 | print("covariance:", self.data[i, j].covariance(), 586 | np.linalg.det(self.data[i, j].covariance()), file=file) 587 | else: 588 | sum_uncertainty += np.trace(self.data[i, j].covariance()) 589 | elif type_optima == "D": 590 | if np.linalg.det(self.data[i, j].information) < 1e-6: 591 | sum_uncertainty += 1000000 592 | if DEBUG: 593 | with open('log_covariance.txt', 'a') as file: 594 | print("information:", self.data[i, j].information, 595 | np.linalg.det(self.data[i, j].information), file=file) 596 | print("covariance:", self.data[i, j].covariance(), 597 | np.linalg.det(self.data[i, j].covariance()), file=file) 598 | else: 599 | sum_uncertainty += np.linalg.det(self.data[i, j].covariance()) 600 | return sum_uncertainty 601 | -------------------------------------------------------------------------------- /real_pipeline.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobustFieldAutonomyLab/Multi-Robot-EM-Exploration/e17bf2444b48b89a5f776abccb6e71e3837d599b/real_pipeline.jpeg -------------------------------------------------------------------------------- /scripts/test/test_multi_SLAM.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import random 4 | 5 | sys.path.insert(0, "../../") 6 | import nav.exp_max 7 | from tqdm import tqdm 8 | import numpy as np 9 | 10 | n = 1 11 | max_ite = 300 12 | cnt = 0 13 | pbar = tqdm(total=n) 14 | 15 | folder_name = "statistic_file" 16 | 17 | # Specify the path where you want to create the folder 18 | path = os.path.join(os.getcwd(), folder_name) 19 | if not os.path.exists(path) or not os.path.isdir(path): 20 | os.mkdir(path) 21 | path_ = os.path.join(path, "NF") 22 | if not os.path.exists(path_) or not os.path.isdir(path_): 23 | os.mkdir(path_) 24 | 25 | with open("log.txt", 'w') as f: 26 | f.write("") 27 | params = nav.exp_max.ExpParams(env_width=100, 28 | env_height=100, 29 | num_obs=30, 30 | num_cooperative=3, 31 | boundary_dist=4, 32 | cell_size=2, 33 | start_center=np.array([20, 50]), 34 | sensor_range=7.5) 35 | # params = nav.exp_max.ExpParams(num_obs=20, map_path = "map.txt", env_width=50, env_height = 50) 36 | 37 | for i in range(n): 38 | ev = nav.exp_max.ExpVisualizer(num=i, seed=755, method="NF", params=params, 39 | folder_path=path) 40 | 41 | ev.init_visualize() 42 | success = False 43 | # try: 44 | success = ev.explore_one_step(max_ite, "tmp/test_virtual_map") 45 | 46 | # except: 47 | # i = i - 1 48 | # if success: 49 | # cnt += 1 50 | pbar.update(1) # Manually update the progress bar 51 | 52 | pbar.close() 53 | -------------------------------------------------------------------------------- /system_requirements: -------------------------------------------------------------------------------- 1 | ubuntu 20.04 2 | --------------------------------------------------------------------------------