├── MultiRobotFormationControl.mp4 ├── MultiRobotFormationControl.pptx ├── README.md ├── Report.pdf └── src ├── python ├── corridor.pgm ├── corridor.yaml ├── map.pgm ├── map.yaml ├── maze.pgm ├── maze.yaml └── rrt.py └── ros ├── formation_move.py ├── formation_move_decentralized.py ├── launch ├── corridor_diamond.launch ├── corridor_line.launch ├── corridor_straight.launch ├── formation_move.launch ├── formation_move_decentralized.launch ├── maze_diamond.launch ├── simple_column.launch ├── simple_diamond.launch └── square_diamond.launch ├── models └── simple_world │ ├── model.config │ └── model.sdf ├── plot_errs.py ├── velocity_controller ├── decentralized_links.py ├── get_combined_velocity.py ├── get_combined_velocity_decentralized.py ├── init_formations.py ├── maintain_formation.py ├── maintain_formation_decentralized.py ├── obstacle_avoidance.py ├── precomputed_rrt_paths.py └── rrt_navigation.py └── worlds ├── corridor.world ├── maze.world ├── simple.world └── square.world /MultiRobotFormationControl.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/MultiRobotFormationControl.mp4 -------------------------------------------------------------------------------- /MultiRobotFormationControl.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/MultiRobotFormationControl.pptx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Robot-Formation-Control 2 | 3 | A **group project** implemented in February 2020 for the third year Mobile Robot Systems course at the University of Cambridge 4 | 5 | The project report can be found at: `report.pdf` 6 | 7 | The results of the project can be found in a video I compiled (`MultiRobotFormationControl.mp4`) 8 | 9 | An overview of the project work can be found in the presentation `MultiRobotFormationControl.pptx`. 10 | 11 | ## Requirements 12 | - Copy the 'turtlebot3', 'turtlebot3_msgs', and 'turtlebot3_simulations' folders from 'catkin_ws/src/' from the original exercises 13 | - Copy the 'project' folder into 'catkin_ws/src/exercises' and run the code from there. 14 | 15 | ## To Run 16 | - Launch [Gazebo](http://gazebosim.org) using the environment you wish 17 | - Go to init_formations.py and set MAP_PARAMS to the map you want (e.g SIMPLE_MAP) 18 | - If you want to run RRT, set RUN_RRT to True 19 | - Predefined paths are stored in the precomputed_rrt_paths.py file 20 | *NOTE: If you change the starting positions of the robots in the environment, you need to recompute a new path, then either store it in the precomputed paths file or reset the starting positions* 21 | 22 | To run the decentralised version, start any map as usual then launch formation_move_decentralized.launch for each robot, supplying id as an arg (e.g id:=1) 23 | 24 | *Project contributors: Benjamin Philps (BenjaminPhi5), Sumaiyah Kola (sumaiyah) and Ajay Ahir (DoodleBobBuffPants)* 25 | -------------------------------------------------------------------------------- /Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/Report.pdf -------------------------------------------------------------------------------- /src/python/corridor.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/src/python/corridor.pgm -------------------------------------------------------------------------------- /src/python/corridor.yaml: -------------------------------------------------------------------------------- 1 | image: corridor.pgm 2 | resolution: 0.050000 3 | origin: [-22.800000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/python/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/src/python/map.pgm -------------------------------------------------------------------------------- /src/python/map.yaml: -------------------------------------------------------------------------------- 1 | image: ./map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/python/maze.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sumaiyah/Multi-Robot-Formation-Control/0f8e1038e6016259bd32a30048cae1c032f3b7b8/src/python/maze.pgm -------------------------------------------------------------------------------- /src/python/maze.yaml: -------------------------------------------------------------------------------- 1 | image: maze.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /src/python/rrt.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | import matplotlib.pylab as plt 6 | import matplotlib.patches as patches 7 | import numpy as np 8 | import os 9 | import re 10 | import scipy.signal 11 | import sys 12 | 13 | directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../ros/velocity_controller') 14 | sys.path.insert(0, directory) 15 | from init_formations import MAP_PARAMS 16 | 17 | X = 0 18 | Y = 1 19 | YAW = 2 20 | 21 | FREE = 0 22 | UNKNOWN = 1 23 | OCCUPIED = 2 24 | 25 | ROBOT_RADIUS = 0.105 / 2. 26 | 27 | BOUNDS = MAP_PARAMS["RRT_BOUNDS"] 28 | MAX_ITERATIONS = MAP_PARAMS["RRT_ITERATIONS"] 29 | 30 | 31 | def sample_random_position(occupancy_grid): 32 | position = np.zeros(2, dtype=np.float32) 33 | 34 | # This is implementing the first primitive of the RRT algorithm: sample configurations in free C-space. 35 | # C free = C \ C obs 36 | 37 | # sample function not including yaw, capped at dimensions of the arena 38 | 39 | sample_pos = lambda: np.array([np.random.uniform(low=BOUNDS[X][0], high=BOUNDS[X][1]), 40 | np.random.uniform(low=BOUNDS[Y][0], high=BOUNDS[Y][1])]) 41 | 42 | position = sample_pos() 43 | while not is_valid(position, occupancy_grid): 44 | position = sample_pos() 45 | 46 | return position 47 | 48 | # function to check that this position is valid for the robot to be in (check points at the robot's circumpherence) 49 | def is_valid(position, occupancy_grid): 50 | # function for checking if a position is valid 51 | pos_is_valid = lambda pos: occupancy_grid.is_free(pos) and not occupancy_grid.is_occupied(pos) 52 | 53 | # check 4 corners (and the centre itself) (for now), this can be extended 54 | r = ROBOT_RADIUS 55 | rob_radius_points = [[0,0], [0, r], [0, -r], [r, 0], [-r, 0]] 56 | for radius_marker in rob_radius_points: 57 | # get radius position 58 | radius_pos = position + radius_marker 59 | # check if it is valid 60 | if not pos_is_valid(radius_pos): 61 | return False 62 | 63 | # no obstacles found at this position, return true 64 | return True 65 | 66 | 67 | def adjust_pose(node, final_position, occupancy_grid): 68 | node._pose[YAW] = (node._pose[YAW] + (2.*np.pi)) % (2.*np.pi) 69 | 70 | final_pose = node.pose.copy() 71 | final_pose[:2] = final_position 72 | # final node is constructed using the final_position x and y components 73 | # the aim of this task is to find out if it is valid, and if so compute a corresponding valid YAW 74 | final_node = Node(final_pose) 75 | 76 | # MISSING: Check whether there exists a simple path that links node.pose 77 | # to final_position. This function needs to return a new node that has 78 | # the same position as final_position and a valid yaw. The yaw is such that 79 | # there exists an arc of a circle that passes through node.pose and the 80 | # adjusted final pose. If no such arc exists (e.g., collision) return None. 81 | # Assume that the robot always goes forward. 82 | # Feel free to use the find_circle() function below. 83 | 84 | 85 | # use the current node and the next node, check if there is a circle 86 | # if no arc return None 87 | # can the circle function return more than one value... who knowns 88 | # can you not I do not 89 | 90 | # okay to do list 91 | # func to get points on a circle (find which value divides up the most since if no chnge in x big change in y) 92 | # func to check if a point is valid for the robot (from above) 93 | # code to check if the points are on the diameter of the circle 94 | # code to find out which way the robot is facing and if it matches (by rotating the stuff on the circle) 95 | # we know if it matches if the yaw matches the direction angle or the direction angle plus pi 96 | # code to compute the new yaw, the current direction plus pi 97 | 98 | # get the circle that connects the robot to the result node 99 | centre, radius = find_circle(node, final_node) 100 | 101 | # theta of the robot from the starting node YAW 102 | theta_robot = node.pose[YAW] 103 | 104 | # compute if theta direction is valid given the circle 105 | # first get the vector from the robot to the centre 106 | radius_vec = - node.pose[:2] + centre 107 | 108 | # get radius angle 109 | theta_rad = get_angle_of_vector(radius_vec) 110 | 111 | # rotate the vector by -pi/2 to get the tangent of the circle 112 | p_div_2 = np.pi / 2. 113 | tangent_vec = np.matmul([ 114 | [np.cos(p_div_2), np.sin(p_div_2)], 115 | [-np.sin(p_div_2), np.cos(p_div_2)] 116 | ], 117 | radius_vec) 118 | 119 | # check if the robot's yaw is within pi/4 rads either side of the radius angle 120 | # if not, the arc is not valid, return None 121 | pi_div2 = np.pi / 2 122 | # the robot must be within this range otherwise it would have to modify w as it was moving between points 123 | # to smoothly curve between them, here we are just assuming a constant w between positions I think. 124 | if theta_rad - pi_div2 <= theta_robot <= theta_rad + pi_div2: 125 | # the angle is valid, compute the new yaw 126 | 127 | # see paper write up, the new yaw is 2 * theta_radius - theta_robot 128 | # if (thetar - theta robot) > 0, its going anticlockwise, yaw = theta r + (theta r - theta robot) = 2 theta r - theta robot 129 | # if (thetar - theta robot) < 0, its going clockwise, yaw = theta r - (theta robot - theta r) = 2 theta r - theta robot 130 | final_pose[YAW] = (2 * theta_rad) - theta_robot 131 | 132 | # now we need to sample this arc to check that there are no collisions 133 | # make the image much bigger with more obstacles so that in the report, you can show that your rrt really does go around the obstacles. 134 | 135 | # get the new centre of the circle given the arc that the robot actually will traverse 136 | centre = get_new_circle_centre(node.pose, final_pose) 137 | 138 | dist_between_points = lambda a, b: np.sqrt(np.square(a[X]-b[X]) + np.square(a[Y]-b[Y])) 139 | # next get the angle that subtends the arc of the circle 140 | #print(dist_between_points(centre, final_position)) 141 | #print(dist_between_points(node.position, centre)) 142 | assert dist_between_points(centre, final_position) - dist_between_points(node.position, centre) < 0.001 143 | 144 | # opp = distance between final position and starting position 145 | # hype = distance between centre and either of the poses 146 | opp = dist_between_points(final_position, node.position) / 2. 147 | hyp = dist_between_points(final_position, centre) 148 | 149 | # angle that subtends the arc is two times the angle in the triangle defined using opp and hyp 150 | phi = np.arcsin(opp/hyp) * 2. 151 | #print("ratio:", opp/hyp) 152 | #print("phi: ", phi) 153 | # if np.isnan(phi): 154 | # phi = np.pi 155 | 156 | # step size at which to check points on the line between start and finish for occupancy 157 | step_size = 0.1 158 | 159 | # check for straight line case (infinte new circle radius): 160 | straight_tolerance = 0.01 161 | if abs(theta_rad - theta_robot) < straight_tolerance: 162 | #print("Picked straight line") 163 | # if there is a straight line between the points, check validity on points on the line between them 164 | distance = opp * 2. 165 | vector = final_position - node.position 166 | steps = distance / step_size 167 | for step in range(0, int(round(steps+1))): 168 | step_position = node.position + step * step_size * vector 169 | if not is_valid(step_position, occupancy_grid): 170 | # print("a: fail") 171 | return None, np.float("inf") 172 | 173 | # here our arc length is just the distance between the points 174 | arc_length = np.sqrt(np.square(final_position[X] - node.position[X]) + np.square(final_position[Y]- node.position[Y])) 175 | 176 | elif dist_between_points(final_position, node.position) < 2. * ROBOT_RADIUS: 177 | # tiny circle, just compute is valid at the destination 178 | if not is_valid(final_position, occupancy_grid): 179 | # print("b: fail") 180 | return None, np.float("inf") 181 | 182 | else: # we are going in a clockwise or anticlockwise circle 183 | # compute new arc length 184 | new_radius_length = hyp 185 | arc_length = new_radius_length * phi 186 | steps = arc_length / step_size 187 | step_angle = phi / steps 188 | 189 | #print("radisu length: ", new_radius_length) 190 | #print("arc length: ", arc_length) 191 | #print("steps: ", steps) 192 | 193 | points = [] 194 | 195 | # offset is pi/2 if going clockwise, or -pi/2 if going anticlockwise 196 | # using the **origianl** circle's theta radius 197 | offset = -np.pi/2. if (theta_rad - theta_robot) > 0 else +np.pi/2. # theta_rad-theta_tobot is < 0 for anticlockwise 198 | direction = 1 if (theta_rad - theta_robot) > 0 else -1. # theta_rad-theta_tobot is < 0 for anticlockwise 199 | 200 | # step along the arc, looking for collisions 201 | if np.isnan(steps): 202 | steps = 0 203 | for step in range(0, int(round(steps+1))): 204 | angle = node.pose[YAW] + offset + (step * step_angle * direction) 205 | 206 | # compute x, y, coords 207 | x = centre[X] + new_radius_length * np.cos(angle) 208 | y = centre[Y] + new_radius_length * np.sin(angle) 209 | 210 | points.append([x, y]) 211 | 212 | # compute the position is okay in the map 213 | # if it is not, return False 214 | if not is_valid(np.array([x,y]), occupancy_grid): 215 | # print("c: fail") 216 | return None, np.float("inf") 217 | 218 | # check the destination 219 | if not is_valid(final_position, occupancy_grid): 220 | # print("d: fail") 221 | return None, np.float("inf") 222 | 223 | #print("#######################") 224 | #print("a: ", node.position) 225 | #print("b: ", final_position) 226 | #print("step points: ", points) 227 | #print("#######################") 228 | 229 | else: 230 | # print("e: fail") 231 | return None, np.float("inf") 232 | 233 | #print("robot theta: ", theta_robot) 234 | #print("tangent theta: ", theta_rad) 235 | #print("------------------------------") 236 | #print("------------------------------") 237 | 238 | #print("arc length: ", arc_length) 239 | final_node = Node(final_pose) 240 | return final_node, arc_length 241 | 242 | def get_new_circle_centre(pose_a, pose_b): 243 | # get gradients of radius lines from points a and b, by shifting tangent gradient by pi/4 degrees 244 | 245 | m_a = np.tan(pose_a[YAW] + (np.pi/2.)) 246 | m_b = np.tan(pose_b[YAW] + (np.pi/2.)) 247 | #m_a = np.tan(pose_a[YAW]) 248 | #m_b = np.tan(pose_b[YAW]) 249 | 250 | # compute constant c components using the pose coords and the gradients 251 | # y = mx + c, => c = y - mx 252 | c_a = pose_a[Y] - m_a * pose_a[X] 253 | c_b = pose_b[Y] - m_b * pose_b[X] 254 | 255 | # compute the centre of the new circle 256 | centre = np.array([ 257 | (c_b - c_a) / (m_a - m_b), 258 | (c_a - m_a * c_b / m_b) / (1 - m_a / m_b) 259 | ]) 260 | 261 | return centre 262 | 263 | 264 | def get_angle_of_vector(vector): 265 | angle = np.arctan(vector[Y]/vector[X]) 266 | # make sure our angle is positive only 267 | if vector[X] < 0: 268 | angle = np.pi + angle 269 | angle = (angle + (np.pi * 2)) % (np.pi * 2) 270 | 271 | return angle 272 | 273 | # Defines an occupancy grid. 274 | class OccupancyGrid(object): 275 | def __init__(self, values, origin, resolution): 276 | self._original_values = values.copy() 277 | self._values = values.copy() 278 | # Inflate obstacles (using a convolution). 279 | inflated_grid = np.zeros_like(values) 280 | inflated_grid[values == OCCUPIED] = 1. 281 | w = 2 * int(ROBOT_RADIUS / resolution) + 1 282 | inflated_grid = scipy.signal.convolve2d(inflated_grid, np.ones((w, w)), mode='same') 283 | self._values[inflated_grid > 0.] = OCCUPIED 284 | self._origin = np.array(origin[:2], dtype=np.float32) 285 | self._origin -= resolution / 2. 286 | assert origin[YAW] == 0. 287 | self._resolution = resolution 288 | 289 | @property 290 | def values(self): 291 | return self._values 292 | 293 | @property 294 | def resolution(self): 295 | return self._resolution 296 | 297 | @property 298 | def origin(self): 299 | return self._origin 300 | 301 | def draw(self): 302 | plt.imshow(self._original_values.T, interpolation='none', origin='lower', 303 | extent=[self._origin[X], 304 | self._origin[X] + self._values.shape[0] * self._resolution, 305 | self._origin[Y], 306 | self._origin[Y] + self._values.shape[1] * self._resolution]) 307 | plt.set_cmap('gray_r') 308 | 309 | def get_index(self, position): 310 | # computes an index into the values table for that position. 311 | idx = ((position - self._origin) / self._resolution).astype(np.int32) 312 | if len(idx.shape) == 2: 313 | idx[:, 0] = np.clip(idx[:, 0], 0, self._values.shape[0] - 1) 314 | idx[:, 1] = np.clip(idx[:, 1], 0, self._values.shape[1] - 1) 315 | return (idx[:, 0], idx[:, 1]) 316 | idx[0] = np.clip(idx[0], 0, self._values.shape[0] - 1) 317 | idx[1] = np.clip(idx[1], 0, self._values.shape[1] - 1) 318 | return tuple(idx) 319 | 320 | def get_position(self, i, j): 321 | return np.array([i, j], dtype=np.float32) * self._resolution + self._origin 322 | 323 | def is_occupied(self, position): 324 | return self._values[self.get_index(position)] == OCCUPIED 325 | 326 | def is_free(self, position): 327 | return self._values[self.get_index(position)] == FREE 328 | 329 | 330 | # Defines a node of the graph. 331 | class Node(object): 332 | def __init__(self, pose): 333 | self._pose = pose.copy() 334 | self._neighbors = [] 335 | self._parent = None 336 | self._cost = 0. 337 | 338 | @property 339 | def pose(self): 340 | return self._pose 341 | 342 | def add_neighbor(self, node): 343 | self._neighbors.append(node) 344 | 345 | def remove_neighbour(self, node): 346 | self._neighbors.remove(node) 347 | 348 | @property 349 | def parent(self): 350 | return self._parent 351 | 352 | @parent.setter 353 | def parent(self, node): 354 | self._parent = node 355 | 356 | @property 357 | def neighbors(self): 358 | return self._neighbors 359 | 360 | @property 361 | def position(self): 362 | return self._pose[:2] 363 | 364 | @property 365 | def yaw(self): 366 | return self._pose[YAW] 367 | 368 | @property 369 | def direction(self): 370 | return np.array([np.cos(self._pose[YAW]), np.sin(self._pose[YAW])], dtype=np.float32) 371 | 372 | @property 373 | def cost(self): 374 | return self._cost 375 | 376 | @cost.setter 377 | def cost(self, c): 378 | self._cost = c 379 | 380 | @yaw.setter 381 | def yaw(self, y): 382 | self._pose[YAW] = y 383 | 384 | 385 | def rrt(start_pose, goal_position, occupancy_grid): 386 | # RRT builds a graph one node at a time. 387 | graph = [] 388 | start_node = Node(start_pose) 389 | # cost of the start node is 0 390 | start_node.cost = 0 391 | final_node = None 392 | if not occupancy_grid.is_free(goal_position): 393 | # print('Goal position is not in the free space.') 394 | return start_node, final_node 395 | graph.append(start_node) 396 | for _ in range(MAX_ITERATIONS): 397 | position = sample_random_position(occupancy_grid) 398 | # With a random chance, draw the goal position. 399 | if np.random.rand() < .05: 400 | position = goal_position 401 | # Find closest node in graph. 402 | # In practice, one uses an efficient spatial structure (e.g., quadtree). 403 | potential_parent = sorted(((n, np.linalg.norm(position - n.position)) for n in graph), key=lambda x: x[1]) 404 | 405 | ## STEP 1 OF RRT* 406 | # Pick potential nodes at least some distance away but not too far. 407 | # We also verify that the angles are aligned (within pi / 4). 408 | potentials_in_radius = [] 409 | search_radius = 1.5 410 | for n, d in potential_parent: 411 | #if d < search_radius: 412 | if d > .2 and d < search_radius and n.direction.dot(position - n.position) / d > 0.70710678118: 413 | potentials_in_radius.append(n) 414 | #else: 415 | # no longer in the search radius, so break 416 | #break 417 | # else: 418 | # continue 419 | 420 | 421 | # if the list is empty, no suitable parent found, continue 422 | if len(potentials_in_radius) == 0: 423 | continue 424 | 425 | # now find the best parent node for the new position, using the cost function 426 | # using the nearest neighbour (first in the list) as the default 427 | u = potentials_in_radius[0] 428 | v, arc_length = adjust_pose(u, position, occupancy_grid) 429 | # cost(node_n+1) = cost(node_n) + arc_length(node_n, node_n+1) 430 | cost = u.cost + arc_length 431 | 432 | for i in range(1, len(potentials_in_radius)): 433 | potential_u = potentials_in_radius[i] 434 | potential_v, arc_length = adjust_pose(potential_u, position, occupancy_grid) 435 | potential_cost = potential_u.cost + arc_length 436 | 437 | # a better node has been found, update u, v, cost 438 | if potential_cost < cost and v is not None: 439 | u = potential_u 440 | v = potential_v 441 | cost = potential_cost 442 | 443 | # if all v's are None, no good v found, continue 444 | if v is None: 445 | # print("NOTE: NO GOOD NODES FOUND!!!") 446 | continue 447 | 448 | # found the u with the minimum cost, using the RRT* adjustments above 449 | # now set the cost using this parent u and update the cost 450 | u.add_neighbor(v) 451 | v.parent = u 452 | # set the cost 453 | v.cost = cost 454 | 455 | graph.append(v) 456 | 457 | ## STEP 2 OF RRT* 458 | # we go through our list of potential us, ignoring the chosen u 459 | # if the potential node's cost is lower if its parent is the new node v, 460 | # update its parent to be v 461 | # 1) change the parent 462 | # 2) update the cost 463 | # 3) update the neighbours list of the old parent 464 | for w in potentials_in_radius: 465 | # get a new w at the same position 466 | w_new, arc_length = adjust_pose(v, w.position, occupancy_grid) 467 | # print(v._pose[YAW] * 180./np.pi, v.position, w.position) 468 | # get the new cost and compare to the old 469 | old_cost = w.cost 470 | new_cost = v.cost + arc_length 471 | # print("old cost: ", old_cost, " | new cost: ", new_cost, " | picked: ", new_cost < old_cost) 472 | if new_cost < old_cost: 473 | # w would be better with the new node as its parent! 474 | # 1. update its parent 475 | old_parent = w.parent 476 | w.parent = v 477 | v.add_neighbor(w) 478 | # update its cost 479 | w.cost = new_cost 480 | # update its yaw 481 | w.yaw = w_new.yaw 482 | 483 | # update its old parent 484 | old_parent.remove_neighbour(w) 485 | 486 | 487 | if np.linalg.norm(v.position - goal_position) < .2: 488 | final_node = v 489 | break 490 | # okay, so I have done the first part of RRT*, now I need to do the second part of RRT*... hmmm okay... 491 | return start_node, final_node 492 | 493 | 494 | def find_circle(node_a, node_b): 495 | def perpendicular(v): 496 | w = np.empty_like(v) 497 | w[X] = -v[Y] 498 | w[Y] = v[X] 499 | return w 500 | db = perpendicular(node_b.direction) 501 | dp = node_a.position - node_b.position 502 | t = np.dot(node_a.direction, db) 503 | if np.abs(t) < 1e-3: 504 | # By construction node_a and node_b should be far enough apart, 505 | # so they must be on opposite end of the circle. 506 | center = (node_b.position + node_a.position) / 2. 507 | radius = np.linalg.norm(center - node_b.position) 508 | else: 509 | radius = np.dot(node_a.direction, dp) / t 510 | center = radius * db + node_b.position 511 | return center, np.abs(radius) 512 | 513 | 514 | def read_pgm(filename, byteorder='>'): 515 | """Read PGM file.""" 516 | with open(filename, 'rb') as fp: 517 | buf = fp.read() 518 | try: 519 | header, width, height, maxval = re.search( 520 | b'(^P5\s(?:\s*#.*[\r\n])*' 521 | b'(\d+)\s(?:\s*#.*[\r\n])*' 522 | b'(\d+)\s(?:\s*#.*[\r\n])*' 523 | b'(\d+)\s(?:\s*#.*[\r\n]\s)*)', buf).groups() 524 | except AttributeError: 525 | raise ValueError('Invalid PGM file: "{}"'.format(filename)) 526 | maxval = int(maxval) 527 | height = int(height) 528 | width = int(width) 529 | img = np.frombuffer(buf, 530 | dtype='u1' if maxval < 256 else byteorder + 'u2', 531 | count=width * height, 532 | offset=len(header)).reshape((height, width)) 533 | return img.astype(np.float32) / 255. 534 | 535 | 536 | def draw_solution(start_node, final_node=None): 537 | ax = plt.gca() 538 | 539 | def draw_path(u, v, arrow_length=.1, color=(.8, .8, .8), lw=1): 540 | du = u.direction 541 | plt.arrow(u.pose[X], u.pose[Y], du[0] * arrow_length, du[1] * arrow_length, 542 | head_width=.05, head_length=.1, fc=color, ec=color) 543 | dv = v.direction 544 | plt.arrow(v.pose[X], v.pose[Y], dv[0] * arrow_length, dv[1] * arrow_length, 545 | head_width=.05, head_length=.1, fc=color, ec=color) 546 | center, radius = find_circle(u, v) 547 | du = u.position - center 548 | theta1 = np.arctan2(du[1], du[0]) 549 | dv = v.position - center 550 | theta2 = np.arctan2(dv[1], dv[0]) 551 | # Check if the arc goes clockwise. 552 | if np.cross(u.direction, du).item() > 0.: 553 | theta1, theta2 = theta2, theta1 554 | ax.add_patch(patches.Arc(center, radius * 2., radius * 2., 555 | theta1=theta1 / np.pi * 180., theta2=theta2 / np.pi * 180., 556 | color=color, lw=lw)) 557 | 558 | points = [] 559 | s = [(start_node, None)] # (node, parent). 560 | while s: 561 | v, u = s.pop() 562 | if hasattr(v, 'visited'): 563 | continue 564 | v.visited = True 565 | # Draw path from u to v. 566 | if u is not None: 567 | draw_path(u, v) 568 | points.append(v.pose[:2]) 569 | for w in v.neighbors: 570 | s.append((w, v)) 571 | 572 | points = np.array(points) 573 | plt.scatter(points[:, 0], points[:, 1], s=10, marker='o', color=(.8, .8, .8)) 574 | if final_node is not None: 575 | plt.scatter(final_node.position[0], final_node.position[1], s=10, marker='o', color='k') 576 | # Draw final path. 577 | v = final_node 578 | while v.parent is not None: 579 | draw_path(v.parent, v, color='k', lw=2) 580 | v = v.parent 581 | -------------------------------------------------------------------------------- /src/ros/formation_move.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import absolute_import 4 | from __future__ import division 5 | from __future__ import print_function 6 | 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | import os 10 | import random 11 | import re 12 | import rospy 13 | import sys 14 | import yaml 15 | 16 | directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../python') 17 | sys.path.insert(0, directory) 18 | import rrt 19 | 20 | directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '/velocity_controller') 21 | sys.path.insert(0, directory) 22 | from init_formations import FORMATION, LEADER_ID, MAP_PARAMS, RUN_RRT 23 | import get_combined_velocity as gcv 24 | import rrt_navigation 25 | 26 | 27 | # Robot motion commands: 28 | # http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html 29 | from geometry_msgs.msg import Twist 30 | # Laser scan message: 31 | # http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html 32 | from sensor_msgs.msg import LaserScan 33 | # For groundtruth information. 34 | from gazebo_msgs.msg import ModelStates 35 | from tf.transformations import euler_from_quaternion 36 | 37 | ROBOT_NAMES = ["tb3_0", "tb3_1", "tb3_2", "tb3_3", "tb3_4"] 38 | 39 | GOAL_POSITION = MAP_PARAMS["GOAL_POSITION"] 40 | 41 | EPSILON = .2 42 | 43 | MAP = MAP_PARAMS["MAP_NAME"] 44 | 45 | ERRORS = [[] for _ in range(len(ROBOT_NAMES)-1)] 46 | 47 | X = 0 48 | Y = 1 49 | YAW = 2 50 | 51 | FREE = 0 52 | UNKNOWN = 1 53 | OCCUPIED = 2 54 | 55 | class SimpleLaser(object): 56 | def __init__(self, name): 57 | rospy.Subscriber('/' + name + '/scan', LaserScan, self.callback) 58 | self._angles = [0., np.pi / 4., -np.pi / 4., np.pi / 2., -np.pi / 2.] 59 | self._width = np.pi / 180. * 10. # 10 degrees cone of view. 60 | self._measurements = [float('inf')] * len(self._angles) 61 | self._indices = None 62 | 63 | def callback(self, msg): 64 | # Helper for angles. 65 | def _within(x, a, b): 66 | pi2 = np.pi * 2. 67 | x %= pi2 68 | a %= pi2 69 | b %= pi2 70 | if a < b: 71 | return a <= x and x <= b 72 | return a <= x or x <= b; 73 | 74 | # Compute indices the first time. 75 | if self._indices is None: 76 | self._indices = [[] for _ in range(len(self._angles))] 77 | for i, d in enumerate(msg.ranges): 78 | angle = msg.angle_min + i * msg.angle_increment 79 | for j, center_angle in enumerate(self._angles): 80 | if _within(angle, center_angle - self._width / 2., center_angle + self._width / 2.): 81 | self._indices[j].append(i) 82 | 83 | ranges = np.array(msg.ranges) 84 | for i, idx in enumerate(self._indices): 85 | # We do not take the minimum range of the cone but the 10-th percentile for robustness. 86 | self._measurements[i] = np.percentile(ranges[idx], 10) 87 | 88 | @property 89 | def ready(self): 90 | return not np.isnan(self._measurements[0]) 91 | 92 | @property 93 | def measurements(self): 94 | return self._measurements 95 | 96 | 97 | class GroundtruthPose(object): 98 | def __init__(self, name='turtlebot3_burger'): 99 | rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback) 100 | self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) 101 | self._name = name 102 | 103 | def callback(self, msg): 104 | idx = [i for i, n in enumerate(msg.name) if n == self._name] 105 | if not idx: 106 | raise ValueError('Specified name "{}" does not exist.'.format(self._name)) 107 | idx = idx[0] 108 | self._pose[0] = msg.pose[idx].position.x 109 | self._pose[1] = msg.pose[idx].position.y 110 | _, _, yaw = euler_from_quaternion([ 111 | msg.pose[idx].orientation.x, 112 | msg.pose[idx].orientation.y, 113 | msg.pose[idx].orientation.z, 114 | msg.pose[idx].orientation.w]) 115 | self._pose[2] = yaw 116 | 117 | @property 118 | def ready(self): 119 | return not np.isnan(self._pose[0]) 120 | 121 | @property 122 | def pose(self): 123 | return self._pose 124 | 125 | def save_errors(robot_poses, desired_positions): 126 | follower_positions = np.array([robot_poses[i][:2] for i in range(len(robot_poses)) if i != LEADER_ID]) 127 | pose_err = [np.linalg.norm(f_pos - d_pos) for f_pos, d_pos in zip(follower_positions, desired_positions)] 128 | 129 | for i in range(len(pose_err)): 130 | ERRORS[i].append(pose_err[i]) 131 | 132 | def plot_errors(): 133 | sampled_errs = [ERRORS[i][::20] for i in range(len(ERRORS))] 134 | 135 | x = np.arange(len(sampled_errs[0])) 136 | for i in range(len(sampled_errs)): 137 | plt.plot(x, sampled_errs[i]) 138 | 139 | plt.xlabel('Time') 140 | plt.ylabel('Error') 141 | plt.legend([ROBOT_NAMES[i] for i in range(len(ROBOT_NAMES)) if i != LEADER_ID]) 142 | plt.show() 143 | 144 | def run(): 145 | rospy.init_node('obstacle_avoidance') 146 | 147 | # Update control every 50 ms. 148 | rate_limiter = rospy.Rate(50) 149 | 150 | publishers = [None] * len(ROBOT_NAMES) 151 | lasers = [None] * len(ROBOT_NAMES) 152 | groundtruths = [None] * len(ROBOT_NAMES) 153 | 154 | # RRT path 155 | # If RUN_RRT is False, load the predefined path 156 | if not RUN_RRT: 157 | current_path = MAP_PARAMS["RRT_PATH"] 158 | else: 159 | current_path = None 160 | 161 | vel_msgs = [None] * len(ROBOT_NAMES) 162 | for i,name in enumerate(ROBOT_NAMES): 163 | publishers[i] = rospy.Publisher('/' + name + '/cmd_vel', Twist, queue_size=5) 164 | lasers[i] = SimpleLaser(name) 165 | groundtruths[i] = GroundtruthPose(name) 166 | 167 | # Load map. (in here so it is only computed once) 168 | with open(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.yaml'.format(MAP))) as fp: 169 | data = yaml.load(fp) 170 | img = rrt.read_pgm(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.pgm'.format(MAP)), data['image']) 171 | occupancy_grid = np.empty_like(img, dtype=np.int8) 172 | occupancy_grid[:] = UNKNOWN 173 | occupancy_grid[img < .1] = OCCUPIED 174 | occupancy_grid[img > .9] = FREE 175 | # Transpose (undo ROS processing). 176 | occupancy_grid = occupancy_grid.T 177 | # Invert Y-axis. 178 | occupancy_grid = occupancy_grid[:, ::-1] 179 | occupancy_grid = rrt.OccupancyGrid(occupancy_grid, data['origin'], data['resolution']) 180 | 181 | while not rospy.is_shutdown(): 182 | # Make sure all measurements are ready. 183 | if not all(laser.ready for laser in lasers) or not all(groundtruth.ready for groundtruth in groundtruths): 184 | rate_limiter.sleep() 185 | continue 186 | 187 | # Compute RRT for the leader only 188 | while not current_path: 189 | start_node, final_node = rrt.rrt(groundtruths[LEADER_ID].pose, GOAL_POSITION, occupancy_grid) 190 | 191 | current_path = rrt_navigation.get_path(final_node) 192 | # print("CURRENT PATH: ", current_path) 193 | 194 | # get the RRT velocity for the leader robot 195 | position = np.array([groundtruths[LEADER_ID].pose[X] + EPSILON*np.cos(groundtruths[LEADER_ID].pose[YAW]), 196 | groundtruths[LEADER_ID].pose[Y] + EPSILON*np.sin(groundtruths[LEADER_ID].pose[YAW])], dtype=np.float32) 197 | rrt_velocity = rrt_navigation.get_velocity(position, np.array(current_path, dtype=np.float32)) 198 | 199 | # get robot poses 200 | robot_poses = np.array([groundtruths[i].pose for i in range(len(groundtruths))]) 201 | 202 | # get the velocities for all the robots 203 | us, ws, desired_positions = gcv.get_combined_velocities(robot_poses=robot_poses, leader_rrt_velocity=rrt_velocity, lasers=lasers) 204 | 205 | save_errors(robot_poses, desired_positions) 206 | 207 | for i in range(len(us)): 208 | vel_msgs[i] = Twist() 209 | vel_msgs[i].linear.x = us[i] 210 | vel_msgs[i].angular.z = ws[i] 211 | 212 | for i,_ in enumerate(ROBOT_NAMES): 213 | publishers[i].publish(vel_msgs[i]) 214 | 215 | rate_limiter.sleep() 216 | 217 | 218 | if __name__ == '__main__': 219 | try: 220 | run() 221 | except rospy.ROSInterruptException: 222 | pass 223 | plot_errors() 224 | -------------------------------------------------------------------------------- /src/ros/formation_move_decentralized.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import absolute_import 4 | from __future__ import division 5 | from __future__ import print_function 6 | 7 | import argparse 8 | import numpy as np 9 | import os 10 | import random 11 | import re 12 | import rospy 13 | import sys 14 | import yaml 15 | 16 | directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../python') 17 | sys.path.insert(0, directory) 18 | import rrt 19 | 20 | directory = os.path.join(os.path.dirname(os.path.realpath(__file__)), '/velocity_controller') 21 | sys.path.insert(0, directory) 22 | from init_formations import FORMATION, LEADER_ID, MAP_PARAMS, RUN_RRT 23 | import get_combined_velocity_decentralized as gcv 24 | import rrt_navigation 25 | 26 | 27 | # Robot motion commands: 28 | # http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html 29 | from geometry_msgs.msg import Twist 30 | # Laser scan message: 31 | # http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html 32 | from sensor_msgs.msg import LaserScan 33 | # For groundtruth information. 34 | from gazebo_msgs.msg import ModelStates 35 | from tf.transformations import euler_from_quaternion 36 | 37 | ROBOT_NAMES = ["tb3_0", "tb3_1", "tb3_2", "tb3_3", "tb3_4"] 38 | 39 | # This robot's information, default to robot 0 40 | ROBOT_ID = 0 41 | ROBOT_NAME = ROBOT_NAMES[ROBOT_ID] 42 | 43 | # Belief of leader 44 | LEADER_NAME = ROBOT_NAMES[LEADER_ID] 45 | LEADER_POSE = [None] * 3 46 | LEADER_VELOCITY = [None] * 2 47 | 48 | ERRORS = [] 49 | 50 | GOAL_POSITION = MAP_PARAMS["GOAL_POSITION"] 51 | 52 | EPSILON = .2 53 | 54 | MAP = MAP_PARAMS["MAP_NAME"] 55 | 56 | X = 0 57 | Y = 1 58 | YAW = 2 59 | 60 | FREE = 0 61 | UNKNOWN = 1 62 | OCCUPIED = 2 63 | 64 | class SimpleLaser(object): 65 | def __init__(self, name): 66 | rospy.Subscriber('/' + name + '/scan', LaserScan, self.current_callback) 67 | rospy.Subscriber('/' + LEADER_NAME + '/scan', LaserScan, self.leader_callback) 68 | self._angles = [0., np.pi / 4., -np.pi / 4., np.pi / 2., -np.pi / 2.] 69 | self._width = np.pi / 180. * 10. # 10 degrees cone of view. 70 | self._current_measurements = [float('inf')] * len(self._angles) 71 | self._leader_measurements = [float('inf')] * len(self._angles) 72 | self._indices = None 73 | 74 | def current_callback(self, msg): 75 | self.callback(msg, self._current_measurements) 76 | 77 | def leader_callback(self, msg): 78 | self.callback(msg, self._leader_measurements) 79 | 80 | def callback(self, msg, measurement_arr): 81 | # Helper for angles. 82 | def _within(x, a, b): 83 | pi2 = np.pi * 2. 84 | x %= pi2 85 | a %= pi2 86 | b %= pi2 87 | if a < b: 88 | return a <= x and x <= b 89 | return a <= x or x <= b; 90 | 91 | # Compute indices the first time. 92 | if self._indices is None: 93 | self._indices = [[] for _ in range(len(self._angles))] 94 | for i, d in enumerate(msg.ranges): 95 | angle = msg.angle_min + i * msg.angle_increment 96 | for j, center_angle in enumerate(self._angles): 97 | if _within(angle, center_angle - self._width / 2., center_angle + self._width / 2.): 98 | self._indices[j].append(i) 99 | 100 | ranges = np.array(msg.ranges) 101 | for i, idx in enumerate(self._indices): 102 | # We do not take the minimum range of the cone but the 10-th percentile for robustness. 103 | measurement_arr[i] = np.percentile(ranges[idx], 10) 104 | 105 | @property 106 | def ready(self): 107 | return not np.isnan(self._current_measurements[0]) and not np.isnan(self._leader_measurements[0]) 108 | 109 | @property 110 | def measurements(self): 111 | return self._current_measurements 112 | 113 | @property 114 | def leader_measurements(self): 115 | return self._leader_measurements 116 | 117 | 118 | class GroundtruthPose(object): 119 | def __init__(self, name='turtlebot3_burger'): 120 | rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback) 121 | self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) 122 | self._leader_pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) 123 | self._name = name 124 | 125 | def callback(self, msg): 126 | # Store belief of current robot and leader, as it is the only state required 127 | ind_name = [(i, n) for i, n in enumerate(msg.name) if n == self._name or n == LEADER_NAME] 128 | if not ind_name: 129 | raise ValueError('Specified name "{}" does not exist.'.format(self._name + " or " + LEADER_NAME)) 130 | for ind, name in ind_name: 131 | # Pose for current robot 132 | pose = np.array(self._pose) 133 | pose[0] = msg.pose[ind].position.x 134 | pose[1] = msg.pose[ind].position.y 135 | _, _, yaw = euler_from_quaternion([ 136 | msg.pose[ind].orientation.x, 137 | msg.pose[ind].orientation.y, 138 | msg.pose[ind].orientation.z, 139 | msg.pose[ind].orientation.w]) 140 | pose[2] = yaw 141 | 142 | if name == self._name: 143 | self._pose = np.array(pose) 144 | if name == LEADER_NAME: 145 | self._leader_pose = np.array(pose) 146 | 147 | @property 148 | def ready(self): 149 | return not np.isnan(self._pose[0]) and not np.isnan(self._leader_pose[0]) 150 | 151 | @property 152 | def pose(self): 153 | return self._pose 154 | 155 | @property 156 | def leader_pose(self): 157 | return self._leader_pose 158 | 159 | def save_error(robot_position, desired_position): 160 | if ROBOT_ID != LEADER_ID: 161 | ERRORS.append(np.linalg.norm(robot_position - desired_position)) 162 | 163 | def write_error(): 164 | if ROBOT_ID != LEADER_ID: 165 | labeled_err = {ROBOT_NAME : ERRORS} 166 | with open('errors.txt', 'a+') as f: 167 | f.write(str(labeled_err)) 168 | 169 | def run(): 170 | rospy.init_node('obstacle_avoidance') 171 | 172 | # Update control every 50 ms. 173 | rate_limiter = rospy.Rate(50) 174 | 175 | publisher = rospy.Publisher('/' + ROBOT_NAME + '/cmd_vel', Twist, queue_size=5) 176 | laser = SimpleLaser(ROBOT_NAME) 177 | groundtruth = GroundtruthPose(ROBOT_NAME) 178 | vel_msg = None 179 | 180 | # RRT path 181 | # If RUN_RRT is False, load the predefined path 182 | if not RUN_RRT: 183 | current_path = MAP_PARAMS["RRT_PATH"] 184 | else: 185 | current_path = None 186 | 187 | # Load map. (in here so it is only computed once) 188 | with open(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.yaml'.format(MAP))) as fp: 189 | data = yaml.load(fp) 190 | img = rrt.read_pgm(os.path.expanduser('~/catkin_ws/src/exercises/project/python/{}.pgm'.format(MAP)), data['image']) 191 | occupancy_grid = np.empty_like(img, dtype=np.int8) 192 | occupancy_grid[:] = UNKNOWN 193 | occupancy_grid[img < .1] = OCCUPIED 194 | occupancy_grid[img > .9] = FREE 195 | # Transpose (undo ROS processing). 196 | occupancy_grid = occupancy_grid.T 197 | # Invert Y-axis. 198 | occupancy_grid = occupancy_grid[:, ::-1] 199 | occupancy_grid = rrt.OccupancyGrid(occupancy_grid, data['origin'], data['resolution']) 200 | 201 | while not rospy.is_shutdown(): 202 | # Make sure all measurements are ready. 203 | if not laser.ready or not groundtruth.ready: 204 | rate_limiter.sleep() 205 | continue 206 | 207 | LEADER_POSE = groundtruth.leader_pose 208 | 209 | # Compute RRT on the leader only 210 | if ROBOT_ID == LEADER_ID: 211 | while not current_path: 212 | start_node, final_node = rrt.rrt(LEADER_POSE, GOAL_POSITION, occupancy_grid) 213 | 214 | current_path = rrt_navigation.get_path(final_node) 215 | # print("CURRENT PATH: ", current_path) 216 | 217 | # get the RRT velocity for the leader robot 218 | position = np.array([LEADER_POSE[X] + EPSILON*np.cos(LEADER_POSE[YAW]), 219 | LEADER_POSE[Y] + EPSILON*np.sin(LEADER_POSE[YAW])], dtype=np.float32) 220 | LEADER_VELOCITY = rrt_navigation.get_velocity(position, np.array(current_path, dtype=np.float32)) 221 | else: 222 | # Let the leader velocity be 0, since the leader pose will update and the 223 | # formation velocity will correctly move the robot 224 | LEADER_VELOCITY = np.array([0., 0.]) 225 | 226 | # get the velocity for this robot 227 | u, w, desired_position = gcv.get_combined_velocity(groundtruth.pose, LEADER_POSE, LEADER_VELOCITY, laser, ROBOT_ID) 228 | 229 | save_error(groundtruth.pose[:2], desired_position) 230 | 231 | vel_msg = Twist() 232 | vel_msg.linear.x = u 233 | vel_msg.angular.z = w 234 | 235 | publisher.publish(vel_msg) 236 | 237 | rate_limiter.sleep() 238 | 239 | 240 | if __name__ == '__main__': 241 | parser = argparse.ArgumentParser(description='Runs decentralized formation control') 242 | parser.add_argument('--id', action='store', default='0', help='Method.', choices=[str(i) for i,_ in enumerate(ROBOT_NAMES)]) 243 | args, unknown = parser.parse_known_args() 244 | 245 | ROBOT_ID = int(args.id) 246 | ROBOT_NAME = ROBOT_NAMES[ROBOT_ID] 247 | 248 | try: 249 | run() 250 | except rospy.ROSInterruptException: 251 | pass 252 | 253 | write_error() 254 | -------------------------------------------------------------------------------- /src/ros/launch/corridor_diamond.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/corridor_line.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/corridor_straight.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/formation_move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/ros/launch/formation_move_decentralized.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/ros/launch/maze_diamond.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/simple_column.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/simple_diamond.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/launch/square_diamond.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /src/ros/models/simple_world/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Exercise World 5 | 1.0 6 | model.sdf 7 | 8 | 9 | asp45 10 | asp45@cl.cam.ac.uk 11 | 12 | 13 | 14 | Simple world for the exercises. 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/ros/models/simple_world/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 8 | 9 | 10 | 4.3 0.15 0.5 11 | 12 | 13 | 0 0 0.25 0 -0 0 14 | 15 | 16 | 0 0 0.25 0 -0 0 17 | 18 | 19 | 4.3 0.15 0.5 20 | 21 | 22 | 23 | 1 1 1 1 24 | 25 | 26 | 0 2.075 0 0 -0 0 27 | 28 | 29 | 30 | 31 | 32 | 4.3 0.15 0.5 33 | 34 | 35 | 0 0 0.25 0 -0 0 36 | 37 | 38 | 0 0 0.25 0 -0 0 39 | 40 | 41 | 4.3 0.15 0.5 42 | 43 | 44 | 45 | 1 1 1 1 46 | 47 | 48 | -2.075 0 0 0 0 -1.5708 49 | 50 | 51 | 52 | 53 | 54 | 4.3 0.15 0.5 55 | 56 | 57 | 0 0 0.25 0 -0 0 58 | 59 | 60 | 0 0 0.25 0 -0 0 61 | 62 | 63 | 4.3 0.15 0.5 64 | 65 | 66 | 67 | 1 1 1 1 68 | 69 | 70 | 0 -2.075 0 0 -0 0 71 | 72 | 73 | 74 | 75 | 76 | 4.3 0.15 0.5 77 | 78 | 79 | 0 0 0.25 0 -0 0 80 | 81 | 82 | 0 0 0.25 0 -0 0 83 | 84 | 85 | 4.3 0.15 0.5 86 | 87 | 88 | 89 | 1 1 1 1 90 | 91 | 92 | 2.075 0 0 0 -0 1.5708 93 | 94 | 95 | 96 | 97 | 98 | 99 | 0.3 100 | 0.5 101 | 102 | 103 | 0 0 0.25 0 -0 0 104 | 105 | 106 | 0 0 0.25 0 -0 0 107 | 108 | 109 | 0.3 110 | 0.5 111 | 112 | 113 | 114 | 1 1 1 1 115 | 116 | 117 | 0.3 0.2 0 0 0 0 118 | 119 | 1 120 | 121 | 122 | -------------------------------------------------------------------------------- /src/ros/plot_errs.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import os 4 | 5 | robot_names = [] 6 | errors = [] 7 | 8 | with open(os.path.expanduser("~/.ros/errors.txt"), "r") as f: 9 | list_of_errors = f.readlines()[0].split("}")[:-1] 10 | list_of_errors = [error+"}" for error in list_of_errors] 11 | for error in list_of_errors: 12 | for k, v in eval(error).items(): 13 | robot_names.append(k) 14 | errors.append(v) 15 | 16 | sampled_errs = [errors[i][::20] for i in range(len(errors))] 17 | min_len = min([len(err) for err in sampled_errs]) 18 | 19 | x = np.arange(min_len) 20 | for i in range(len(sampled_errs)): 21 | plt.plot(x, sampled_errs[i][:min_len]) 22 | 23 | plt.xlabel('Time (s)') 24 | plt.ylabel('Error (m)') 25 | plt.legend(robot_names) 26 | 27 | os.remove(os.path.expanduser("~/.ros/errors.txt")) 28 | 29 | plt.show() 30 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/decentralized_links.py: -------------------------------------------------------------------------------- 1 | #The robot links for decentralization, as indexes into the formation 2 | 3 | LINE_LINKS = { 4 | 0: [1], 5 | 1: [0, 2], 6 | 2: [1, 3], 7 | 3: [2, 4], 8 | 4: [3] 9 | } 10 | 11 | COLUMN_LINKS = { 12 | 0: [1], 13 | 1: [0, 2], 14 | 2: [1, 3], 15 | 3: [2, 4], 16 | 4: [3] 17 | } 18 | 19 | DIAMOND_LINKS = { 20 | 0: [2], 21 | 1: [2], 22 | 2: [0, 1, 3, 4], 23 | 3: [2], 24 | 4: [2] 25 | } 26 | 27 | WEDGE_LINKS = { 28 | 0: [1], 29 | 1: [0, 2], 30 | 2: [1, 3], 31 | 3: [2, 4], 32 | 4: [3] 33 | } 34 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/get_combined_velocity.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | from init_formations import LEADER_ID 6 | from maintain_formation import maintain_formation, CONTROLLED_ZONE 7 | 8 | import numpy as np 9 | import obstacle_avoidance 10 | import rrt_navigation 11 | 12 | # Feedback linearisation epsilon 13 | EPSILON = .2 14 | 15 | ROBOT_DISTANCE = .15 16 | ROBOT_EXTRA_DISTANCE = .21 17 | 18 | X = 0 19 | Y = 1 20 | YAW = 2 21 | 22 | def get_obstacle_avoidance_velocities(robot_poses, lasers): 23 | xyoa_velocities = [] 24 | for i in range(len(robot_poses)): 25 | u, w = obstacle_avoidance.braitenberg(robot_poses, i, *(lasers[i].measurements)) 26 | 27 | x = u*np.cos(robot_poses[i][YAW]) - EPSILON*w*np.sin(robot_poses[i][YAW]) 28 | y = u*np.sin(robot_poses[i][YAW]) + EPSILON*w*np.cos(robot_poses[i][YAW]) 29 | 30 | xyoa_velocities.append(np.array([x,y])) 31 | 32 | return np.array(xyoa_velocities) 33 | 34 | def get_noise(): 35 | noise = np.random.uniform(low=-1., high=1., size=[5, 2]) 36 | noise = normalise_velocities(noise) 37 | 38 | return noise 39 | 40 | def normalise_velocities(velocities): 41 | # Accounts for small magnitudes 42 | for i in range(len(velocities)): 43 | n = np.linalg.norm(velocities[i]) 44 | 45 | if n < 1e-2: 46 | velocities[i] = np.zeros_like(velocities[i]) 47 | else: 48 | velocities[i] = velocities[i] / n 49 | 50 | return velocities 51 | 52 | 53 | def get_combined_velocities(robot_poses, leader_rrt_velocity, lasers): 54 | 55 | # get leader and follower poses 56 | leader_pose = robot_poses[LEADER_ID] 57 | follower_poses = np.array([robot_poses[i] for i in range(len(robot_poses)) if i != LEADER_ID]) 58 | 59 | # Velocities 60 | follower_formation_velocities, desired_positions, in_dead_zone = maintain_formation(leader_pose, follower_poses, leader_rrt_velocity, lasers) 61 | obstacle_avoidance_velocities = get_obstacle_avoidance_velocities(robot_poses, lasers) 62 | 63 | # NOTE: for numpy insert, obj is the index of insertion. 64 | formation_velocities = np.insert(arr=follower_formation_velocities, obj=LEADER_ID, values=np.array([0., 0.]), axis=0) 65 | # follower formation velocities is only 4 long 66 | rrt_velocities = np.insert(arr=np.zeros_like(follower_formation_velocities), obj=LEADER_ID, values=leader_rrt_velocity, axis=0) 67 | 68 | # normalized noise and rrt velocities. 69 | noise_velocities = get_noise() 70 | rrt_velocities = normalise_velocities(rrt_velocities) 71 | 72 | combined_velocities = weight_velocities(rrt_velocities, formation_velocities, obstacle_avoidance_velocities, robot_avoidance_weights(robot_poses), noise_velocities, in_dead_zone) 73 | 74 | # Feedback linearization - convert combined_velocities [[x,y], ...] into [[u, w], ...] 75 | us = [] 76 | ws = [] 77 | for i in range(len(combined_velocities)): 78 | u, w = rrt_navigation.feedback_linearized(pose=robot_poses[i], velocity=combined_velocities[i], epsilon=EPSILON) 79 | 80 | us.append(u) 81 | ws.append(w) 82 | 83 | return us, ws, desired_positions 84 | 85 | def robot_avoidance_weights(robot_poses): 86 | # now robots stop if there is a robot infront of it. 87 | # for each robot, if any of the other robot poses are within say pi/2 or pi-delta infront of the robot at a small distance, 88 | # stop and wait for them to move. 89 | v = [] 90 | for i in range(len(robot_poses)): 91 | v.append(1.) 92 | for j in range(len(robot_poses)): 93 | # for robots other than this one 94 | if j != i: 95 | # if the other robot is infront of it, and distance is < avoidance_threshold 96 | vector_to_robot = robot_poses[j] - robot_poses[i] 97 | distance = np.linalg.norm(vector_to_robot[:2]) 98 | 99 | angle_to_robot = np.arctan2(vector_to_robot[Y], vector_to_robot[X]) - robot_poses[i][YAW] 100 | 101 | if -np.pi/2. < angle_to_robot < np.pi/2. and distance < ROBOT_EXTRA_DISTANCE: 102 | # stop dealock (if angle is big, i.e robots are next to each other, let the one with lower id go first.) 103 | if abs(angle_to_robot) > np.pi/3. and i > j: 104 | continue 105 | # if the robots are very close (or quite close but next to each other) stop the lower id robot 106 | elif distance < ROBOT_DISTANCE or abs(angle_to_robot) > np.pi/3.: 107 | v[i] = 0. 108 | break 109 | return v 110 | 111 | def weighting(velocities, weight): 112 | wv = np.array(velocities) 113 | for i in range(len(velocities)): 114 | wv[i] = velocities[i] * weight 115 | return wv 116 | 117 | def weight_velocities(goal_velocities, formation_velocities, obstacle_velocities, robot_avoidance_weights, noise_velocities, in_dead_zone): 118 | 119 | goal_w = .35 120 | formation_w = .2 121 | static_obs_avoid_w = .22 122 | noise_w = .05 123 | overall_weight = 1.5 124 | 125 | goal = weighting(goal_velocities, goal_w) 126 | formation = weighting(formation_velocities, formation_w) 127 | static_obstacle_avoidance = weighting(obstacle_velocities, static_obs_avoid_w) 128 | noise = weighting(noise_velocities, noise_w) 129 | 130 | # only leader has the goal, the rest have formation constraints 131 | objective = goal + formation 132 | 133 | # sum of all velocity components 134 | weighted_sum = objective + static_obstacle_avoidance + noise 135 | 136 | # RULE: For each robot, if it is nearer other robots, let the first robot (by id) pass 137 | for i in range(len(objective)): 138 | if robot_avoidance_weights[i] == 0.: 139 | weighted_sum[i] = np.zeros_like(weighted_sum[i]) 140 | 141 | # RULE: For each robot, if it has reached its objective, stop (ignore obstacle input) 142 | for i in range(len(objective)): 143 | if np.linalg.norm(objective[i]) == 0.: 144 | weighted_sum[i] = np.zeros_like(weighted_sum[i]) 145 | 146 | # RULE: if the robots are outside the deadzone, the leader has to slow down 147 | not_in_dead_zone = len(in_dead_zone) - sum(in_dead_zone) 148 | if not_in_dead_zone > 0: 149 | weighted_sum[LEADER_ID] = weighted_sum[LEADER_ID] * .3 150 | 151 | return weighted_sum * overall_weight 152 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/get_combined_velocity_decentralized.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | from init_formations import LEADER_ID 6 | from maintain_formation_decentralized import maintain_formation, CONTROLLED_ZONE 7 | 8 | import numpy as np 9 | import obstacle_avoidance 10 | import rrt_navigation 11 | 12 | # Feedback linearisation epsilon 13 | EPSILON = .2 14 | 15 | ROBOT_DISTANCE = .15 16 | ROBOT_EXTRA_DISTANCE = .21 17 | 18 | X = 0 19 | Y = 1 20 | YAW = 2 21 | 22 | def get_obstacle_avoidance_velocity(robot_pose, laser, robot_id): 23 | 24 | u, w = obstacle_avoidance.rule_based(*laser.measurements, robot_id=robot_id) 25 | 26 | x = u*np.cos(robot_pose[YAW]) - EPSILON*w*np.sin(robot_pose[YAW]) 27 | y = u*np.sin(robot_pose[YAW]) + EPSILON*w*np.cos(robot_pose[YAW]) 28 | 29 | return np.array([x, y]) 30 | 31 | def get_noise(): 32 | noise = np.random.uniform(low=-1., high=1., size=2) 33 | noise = normalize(noise) 34 | return noise 35 | 36 | def get_combined_velocity(robot_pose, leader_pose, leader_rrt_velocity, laser, robot_id): 37 | 38 | # Velocities 39 | rrt_velocity = normalize(leader_rrt_velocity) 40 | formation_velocity = np.array([0., 0.]) 41 | obstacle_avoidance_velocity = np.array([0., 0.]) 42 | noise = get_noise() 43 | 44 | desired_position = robot_pose[:2] 45 | 46 | if robot_id != LEADER_ID: 47 | rrt_velocity = np.array([0., 0.]) 48 | formation_velocity, desired_position = maintain_formation(leader_pose, robot_pose, leader_rrt_velocity, robot_id, laser) 49 | obstacle_avoidance_velocity = get_obstacle_avoidance_velocity(robot_pose, laser, robot_id) 50 | 51 | min_measurement = min(laser.measurements) 52 | if min < 1.5: 53 | formation_velocity = leader_rrt_velocity * 0.5 54 | 55 | combined_velocity = weight_velocities(rrt_velocity, formation_velocity, obstacle_avoidance_velocity, noise) 56 | 57 | # Feedback linearization - convert combined_velocities [x,y] [u,w] 58 | u, w = rrt_navigation.feedback_linearized(pose=robot_pose, velocity=combined_velocity, epsilon=EPSILON) 59 | 60 | return u, w, desired_position 61 | 62 | def weighting(velocity, weight): 63 | return np.array(velocity * weight) 64 | 65 | def normalize(vec): 66 | mag = np.linalg.norm(vec) 67 | if mag < .01: 68 | return np.zeros_like(vec) 69 | else: 70 | return vec / mag 71 | 72 | def weight_velocities(goal_velocity, formation_velocity, obstacle_velocity, noise_velocity): 73 | 74 | goal_w = .2 75 | formation_w = .5 76 | static_obs_avoid_w = 0.35 77 | noise_w = .05 78 | overall_weight = 1.5 79 | # currently no robot avoidance in decentralized algorithm as we do not keep all robot poses 80 | 81 | goal = weighting(goal_velocity, goal_w) 82 | formation = weighting(formation_velocity, formation_w) 83 | static_obstacle_avoidance = weighting(obstacle_velocity, static_obs_avoid_w) 84 | noise = weighting(noise_velocity, noise_w) 85 | 86 | objective = goal + formation 87 | weighted_sum = objective + static_obstacle_avoidance + noise 88 | 89 | if np.linalg.norm(objective) == 0.: 90 | weighted_sum = np.zeros_like(weighted_sum) 91 | 92 | return weighted_sum * overall_weight 93 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/init_formations.py: -------------------------------------------------------------------------------- 1 | from precomputed_rrt_paths import * 2 | 3 | import numpy as np 4 | 5 | # Formation spacing parameter for the formation 6 | ROBOT_RADIUS = 0.105 / 2. 7 | SPACING_DIST = 0.3 + ROBOT_RADIUS 8 | 9 | # NOTE: 10 | # In leader follower, only the followers need be defined in the formation. 11 | 12 | LEADER_ID = 2 13 | INITIAL_YAW = 1.571 14 | LINE = np.array([[2*SPACING_DIST,0], 15 | [SPACING_DIST, 0], 16 | [-SPACING_DIST, 0], 17 | [-2*SPACING_DIST, 0]]) 18 | 19 | # LEADER_ID = 2 20 | # INITIAL_YAW = 1.571 21 | COLUMN = np.array([[0, 2*SPACING_DIST], 22 | [0, SPACING_DIST], 23 | [0,-SPACING_DIST], 24 | [0, -2*SPACING_DIST]]) 25 | 26 | # DIAMOND 27 | # LEADER (0) not defined 28 | # 1 2 4 29 | # 3 30 | 31 | # LEADER_ID = 0 32 | # INITIAL_YAW = 1.571 33 | DIAMOND = np.array([[-SPACING_DIST, -SPACING_DIST], 34 | [0, -SPACING_DIST], 35 | [0, -2.*SPACING_DIST], 36 | [SPACING_DIST, -SPACING_DIST]]) 37 | 38 | # LEADER_ID = 2 39 | # INITIAL_YAW = 1.571 40 | WEDGE = np.array([[2*SPACING_DIST, -SPACING_DIST], 41 | [SPACING_DIST, 0], 42 | [-SPACING_DIST, 0], 43 | [-2*SPACING_DIST, -SPACING_DIST]]) 44 | 45 | # for switching, leader is in front 46 | SWITCHED_CORRIDOR_FORMATION = np.array([[0, -2.*SPACING_DIST], 47 | [0, -1.*SPACING_DIST], 48 | [0, -4.*SPACING_DIST], 49 | [0, -3.*SPACING_DIST]]) 50 | 51 | # RRT bounds are [[x_min, x_max], [y_min, y_max]] 52 | # Now try setting this pose param to include initial yaw and see if the initial yaw problem goes away... 53 | SIMPLE_MAP = { 54 | "GOAL_POSITION": np.array([1.5, 1.5], dtype=np.float32), 55 | "RRT_BOUNDS": [[-2, 2], [-2, 2]], 56 | "MAP_NAME": "map", 57 | "RRT_ITERATIONS": 500, 58 | "RRT_PATH": SIMPLE_PATH, 59 | "FORMATION_YAW": np.pi/2.} 60 | 61 | CORRIDOR_MAP = { 62 | "GOAL_POSITION": np.array([-2.6, -0.1], dtype=np.float32), 63 | "RRT_BOUNDS": [[-12, -2], [-1, 3]], 64 | "MAP_NAME": "corridor", 65 | "RRT_ITERATIONS": 1500, 66 | "RRT_PATH": CORRIDOR_PATH, 67 | "FORMATION_YAW": 0.} 68 | 69 | CORRIDOR_STRAIGHT_MAP = { 70 | "GOAL_POSITION": np.array([-2.6, -0.1], dtype=np.float32), 71 | "RRT_BOUNDS": [[-12, -2], [-1, 3]], 72 | "MAP_NAME": "corridor", 73 | "RRT_ITERATIONS": 1500, 74 | "RRT_PATH": CORRDIOR_STRAIGHT_PATH, 75 | "FORMATION_YAW": 0.} 76 | 77 | MAZE_MAP = { 78 | "GOAL_POSITION": np.array([-1.0, -3.2], dtype=np.float32), 79 | "RRT_BOUNDS": [[-4.3, -0.3], [-5.8, -2.5]], 80 | "MAP_NAME": "maze", 81 | "RRT_ITERATIONS": 1500, 82 | "RRT_PATH": MAZE_PATH, 83 | "FORMATION_YAW": 0.} 84 | 85 | SQUARE_MAP = { 86 | "GOAL_POSITION": np.array([1.5, 1.5], dtype=np.float32), 87 | "RRT_BOUNDS": [[-2, 2], [-2, 2]], 88 | "MAP_NAME": "map", 89 | "RRT_ITERATIONS": 1000, 90 | "RRT_PATH": SQUARE_PATH, 91 | "FORMATION_YAW": -0.8} 92 | 93 | # Set these params before running code 94 | # NOTE: PATHS DEPEND ON STARTING POSE. IF YOU CHANGE STARTING POSE OF THE ROBOTS, YOU NEED TO RERUN RRT AND GET A NEW PATH 95 | MAP_PARAMS = CORRIDOR_MAP 96 | 97 | # Set to false to use the predefined path 98 | RUN_RRT = False 99 | 100 | ALLOW_FORMATION_ROTATION = True 101 | 102 | FORMATION = LINE 103 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/maintain_formation.py: -------------------------------------------------------------------------------- 1 | from init_formations import LEADER_ID, FORMATION, SPACING_DIST, SWITCHED_CORRIDOR_FORMATION, INITIAL_YAW, MAP_PARAMS, ALLOW_FORMATION_ROTATION 2 | 3 | import numpy as np 4 | 5 | X = 0 6 | Y = 1 7 | YAW = 2 8 | 9 | ROBOT_RADIUS = 0.105 / 2. 10 | FORMATION_YAW = MAP_PARAMS["FORMATION_YAW"] 11 | 12 | # DEAD ZONE (If a robot is within the dead zone of its desired formation postion, it doesnt move) 13 | DEAD_ZONE = 1.5 * ROBOT_RADIUS 14 | # CONTROL_ZONE (if robot is within the controlled zone, velocity towards position linearly increases the further away it is) 15 | CONTROLLED_ZONE = DEAD_ZONE + SPACING_DIST 16 | 17 | def get_desired_positions(formation, formation_pose): 18 | # Take formation and transform (rotate, translate) onto formation_pose 19 | if ALLOW_FORMATION_ROTATION: 20 | theta = formation_pose[YAW] 21 | else: 22 | theta = FORMATION_YAW - INITIAL_YAW 23 | 24 | desired_positions = np.zeros_like(formation) 25 | for i in range(len(formation)): 26 | # Rotate 27 | desired_positions[i] = np.matmul([[np.cos(theta), -np.sin(theta)], 28 | [np.sin(theta), np.cos(theta)]], formation[i]) 29 | # Translate 30 | desired_positions[i] = formation_pose[:2] + desired_positions[i] 31 | return desired_positions 32 | 33 | def detect_corridor(robot_poses, lasers): 34 | # it's a corridor if the majority think it is a corridor, or if the leader thinks it's a corridor 35 | # this approximates waiting until the half the robots leave the corridor to switch back to the normal formation 36 | 37 | believe_in_corridor = [] 38 | for i in range(len(robot_poses)): 39 | sens_inp = np.tanh(lasers[i].measurements) 40 | 41 | # corridor detected if front is big and left and right are small 42 | if sens_inp[0] > np.tanh(1.) and sens_inp[3] < np.tanh(.8) and sens_inp[4] < np.tanh(.8): 43 | believe_in_corridor.append(1.) 44 | else: 45 | believe_in_corridor.append(0.) 46 | 47 | total_in_corridor = np.sum(np.array(believe_in_corridor)) 48 | 49 | if believe_in_corridor[LEADER_ID] == 1. or total_in_corridor > len(robot_poses)/2.: 50 | return SWITCHED_CORRIDOR_FORMATION 51 | else: 52 | return FORMATION 53 | 54 | def maintain_formation(leader_pose, follower_poses, leader_rrt_velocity, lasers): 55 | # Formation orientation is the angle of the formation given the leader's direction. 56 | formation_orientation = leader_pose[YAW] - INITIAL_YAW 57 | formation_pose = np.concatenate((leader_pose[:2], [formation_orientation])) 58 | 59 | # Desired positions of each of the follower robots in the formation 60 | robot_poses = np.insert(arr=follower_poses, obj=LEADER_ID, values=leader_pose, axis=0) 61 | formation = detect_corridor(robot_poses, lasers) 62 | desired_positions = get_desired_positions(formation=formation, formation_pose=formation_pose) 63 | 64 | # velocity directs the follower robots from their current position to their (desired position in the formation) 65 | follower_positions = np.array([pose[:2] for pose in follower_poses]) 66 | velocities = desired_positions - follower_positions 67 | 68 | distances = [] 69 | 70 | # update each velocity (the displacement between the current and desired position) depending on the distance 71 | for i in range(len(velocities)): 72 | 73 | distance = np.linalg.norm(velocities[i]) 74 | distances.append(distance) 75 | 76 | # If a robot is within accepted radius of formation position, velocity should be 0 77 | # DEAD ZONE (If a robot is within the dead zone of its desired formation postion, it doesnt move) 78 | if distance < DEAD_ZONE: 79 | velocities[i] = np.zeros_like(velocities[i]) 80 | elif distance < CONTROLLED_ZONE: 81 | # we do nothing, velocity is directly proportional to distance 82 | pass 83 | else: 84 | # robot is outside the controlled zone, so we normalize the velocity and then multiply 85 | # by the radius of the control zone. 86 | velocities[i] = velocities[i] / distance 87 | velocities[i] = velocities[i] * CONTROLLED_ZONE 88 | 89 | # scale to be between 0 and 1 90 | velocities = velocities / CONTROLLED_ZONE 91 | 92 | # in dead zone vector 93 | distances = np.array(distances) 94 | in_dead_zone = distances < DEAD_ZONE * 3. 95 | 96 | return velocities, desired_positions, in_dead_zone 97 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/maintain_formation_decentralized.py: -------------------------------------------------------------------------------- 1 | from init_formations import LEADER_ID, FORMATION, SPACING_DIST, SWITCHED_CORRIDOR_FORMATION, INITIAL_YAW, MAP_PARAMS, ALLOW_FORMATION_ROTATION 2 | 3 | import numpy as np 4 | 5 | X = 0 6 | Y = 1 7 | YAW = 2 8 | 9 | ROBOT_RADIUS = 0.105 / 2. 10 | FORMATION_YAW = MAP_PARAMS["FORMATION_YAW"] 11 | 12 | # DEAD ZONE (If a robot is within the dead zone of its desired formation postion, it doesnt move) 13 | DEAD_ZONE = 1.5 * ROBOT_RADIUS 14 | # CONTROL_ZONE (if robot is within the controlled zone, velocity towards position linearly increases the further away it is) 15 | CONTROLLED_ZONE = DEAD_ZONE + SPACING_DIST 16 | 17 | def get_desired_positions(formation, formation_pose): 18 | # Take formation and transform (rotate, translate) onto formation_pose 19 | if ALLOW_FORMATION_ROTATION: 20 | theta = formation_pose[YAW] 21 | else: 22 | theta = FORMATION_YAW - INITIAL_YAW 23 | 24 | desired_positions = np.zeros_like(formation) 25 | for i in range(len(formation)): 26 | # Rotate 27 | desired_positions[i] = np.matmul([[np.cos(theta), -np.sin(theta)], 28 | [np.sin(theta), np.cos(theta)]], formation[i]) 29 | # Translate 30 | desired_positions[i] = formation_pose[:2] + desired_positions[i] 31 | return desired_positions 32 | 33 | def detect_corridor(laser): 34 | 35 | sens_inp = np.tanh(laser.leader_measurements) 36 | 37 | # corridor detected if front is big and left and right are small 38 | if sens_inp[0] > np.tanh(1.) and sens_inp[3] < np.tanh(.8) and sens_inp[4] < np.tanh(.8): 39 | return SWITCHED_CORRIDOR_FORMATION 40 | else: 41 | return FORMATION 42 | 43 | 44 | def maintain_formation(leader_pose, follower_pose, leader_rrt_velocity, robot_id, laser): 45 | 46 | # Account for leader in using id as index 47 | if robot_id > LEADER_ID: 48 | robot_id = robot_id - 1 49 | 50 | # Formation orientation is the angle of formation 51 | formation_orientation = leader_pose[YAW] - INITIAL_YAW 52 | formation_pose = np.concatenate((leader_pose[:2], [formation_orientation])) 53 | 54 | # Desired positions of each of the follower robots in the formation (see comment above about replacing formation pose with leader...) 55 | formation = detect_corridor(laser) 56 | desired_positions = get_desired_positions(formation=formation, formation_pose=formation_pose) 57 | 58 | # velocity directs the follower robots from their current position to their (desired position in the formation) 59 | follower_position = follower_pose[:2] 60 | velocity = desired_positions[robot_id] - follower_position 61 | 62 | # update the velocity (the displacement between the current and desired position) depending on the distance 63 | 64 | distance = np.linalg.norm(velocity) 65 | 66 | # If a robot is within accepted radius of formation position, velocity should be 0 67 | # DEAD ZONE (If a robot is within the dead zone of its desired formation postion, it doesnt move) 68 | if distance < DEAD_ZONE: 69 | velocity = np.zeros_like(velocity) 70 | elif distance < CONTROLLED_ZONE: 71 | # we do nothing, velocity is directly proportional to distance 72 | pass 73 | else: 74 | # robot is outside the controlled zone, so we normalize the velocity and then multiply 75 | # by the radius of the control zone. 76 | velocity = velocity / distance 77 | velocity = velocity * CONTROLLED_ZONE 78 | 79 | return velocity / CONTROLLED_ZONE, desired_positions[robot_id] 80 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/obstacle_avoidance.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | from rrt_navigation import feedback_linearized 6 | from init_formations import INITIAL_YAW 7 | 8 | import numpy as np 9 | 10 | SENSOR_ROBOT_TOLERANCE = 0.08 11 | ROBOT_DETECTION_THRESHOLD = 3.5 12 | RIGHT, FRONT_RIGHT, FRONT, FRONT_LEFT, LEFT = 0, 1, 2, 3, 4 13 | MAX_DISTANCE = 3.5 14 | 15 | X = 0 16 | Y = 1 17 | YAW = 2 18 | 19 | def braitenberg(robot_poses, robot_id, front, front_left, front_right, left, right): 20 | u = .1 # [m/s] so when not facing an obstacle the robot accelerates 21 | w = 0. # [rad/s] going counter-clockwise. 22 | 23 | # create an array of sensor input from the robot 24 | sens_inp = np.array([right, front_right, front, front_left, left]) 25 | 26 | # detect the other robots within the sensor range 27 | sens_inp = detect_robot_presence(robot_poses, sens_inp, robot_id) 28 | 29 | # smooth with arctan (could use tanh), but then take the gradient of this, so big inputs give little change, but small imputs give the most change 30 | # this is the gradient of the tanh function 31 | sens_inp = 1. / (1. + 3.*(sens_inp**2.)) 32 | 33 | # rearrange for braitenburg 34 | sens_inp = np.array([sens_inp[FRONT],sens_inp[FRONT_LEFT],sens_inp[FRONT_RIGHT],sens_inp[LEFT],sens_inp[RIGHT]]) 35 | 36 | # Direction to turn from front sensor 37 | # DIRECTION causes the SPLIT AND MERGE behaviour. Robots on LHS of formation go left, robots on RHS of formation go right. 38 | if robot_id < len(robot_poses) // 2: 39 | direction = 1. 40 | else: 41 | direction = -1. 42 | 43 | # this should be a better form of weights, when it is surrounded, acceleration is negative default 44 | # a bias on the front sensor to steer slighty left is given so that it does not get stuck still in reverse (it can manage to turn around, test this with extra objects) 45 | # actually the plus on left and right makes sense, i want it to get smaller when it gets bigger 46 | weights = np.array([ 47 | # front front_left front_right left right 48 | [-.3, -.2, -.2, 0., 0.], 49 | [.5*direction, -5., 5., .3, .3] 50 | ]) 51 | 52 | # multiply sense inputs by the weights. 53 | params = np.matmul(weights, sens_inp) 54 | 55 | # add a function to turn around the robot when it gets very very close 56 | # prevents the non turning issue when it goes head on into something 57 | if front < .5 and front_left > .2 and front_right > .2: 58 | w = w + (40. * (1. / (1. + 100.*(front**2.))) * direction) 59 | 60 | # extract params note this doesn't consider previous step since we set them to 0 / 0.5 at the start.. it does do what you think nice. 61 | u, w = u + params[0], w + params[1] 62 | 63 | # CORRIDOR function, this allows the robot to move through narrow corridors without obstacle avoidance slowing it down 64 | # NOTE: the signs and values are the opposite of what you would expect for tanh , since this uses a different smoothing function (1/3x^2) 65 | if np.tanh(front) < .14 and np.tanh(front_left) < .869 and np.tanh(front_right) < .869: 66 | # print("CORRIDOR RULE TRIGGERED") 67 | w = - .5*(1.-sens_inp[FRONT_LEFT]) + .5*(1.-sens_inp[FRONT_RIGHT]) 68 | u = .1 69 | 70 | return u, w 71 | 72 | def rule_based(front, front_left, front_right, left, right, robot_id): 73 | u = 0. # [m/s] 74 | w = 0. # [rad/s] going counter-clockwise. 75 | 76 | # apply tanh to the input to deal with infinity 77 | sens_inp = np.array([front, front_left, front_right, left, right]) 78 | sens_inp = np.tanh(sens_inp) 79 | 80 | # if the right hand side detects an approaching object , alter w to move left 81 | if sens_inp[2] < np.tanh(.4): 82 | # print("fire front left") 83 | u -= 0.05 84 | w = w + 1.5*(1.3-sens_inp[2]) 85 | 86 | # if the left hand side detects and approaching object, alter w to move to the right 87 | if sens_inp[1] < np.tanh(.4): 88 | u -= 0.05 89 | # print("fire front right") 90 | w = w - 1.5*(1.3-sens_inp[1]) 91 | 92 | # if robot is very close to the right hand slide, adjust left a little 93 | if sens_inp[4] < np.tanh(.2): 94 | # print("fire left") 95 | w = w + 0.3*(1.-sens_inp[4]) 96 | 97 | # if robot is very close to the left hand slide, adjust right a little 98 | if sens_inp[3] < np.tanh(.2): 99 | # print("fire right") 100 | w = w - 0.3*(1.-sens_inp[3]) 101 | 102 | direction = 1. # if robot_id > 2 else -1. 103 | 104 | 105 | # if close to front, move away 106 | if sens_inp[0] < np.tanh(.6): 107 | # print("FRONT firing") 108 | u = -0.2 109 | # if sens_inp[1] < sens_inp[2]: 110 | # w = w -2.5*direction*(1-0.5*sens_inp[0]) 111 | # else: 112 | # w = w +2.5*direction*(1-0.5*sens_inp[0]) 113 | w = w +4.*direction*(1-0.5*sens_inp[0]) 114 | 115 | # print("u, w: ", u, w) 116 | 117 | return u, w 118 | 119 | # if all sensors detect obstacles are far away, adjust slightly using a highest absolute value for direction 120 | if (sens_inp > np.tanh(.6)).all(): 121 | # move in coward motion, away from the closest object 122 | return u, sens_inp[2] + sens_inp[4] - sens_inp[1] - sens_inp[3] 123 | 124 | # breakout! (mentioned in part C only - rest of rules are as described in part a) 125 | # If the front sensor reports a much greater distance than the side sensors, 126 | # don't change direction so much, use the left and right sensors for small adjustments 127 | if sens_inp[0] > np.tanh(.6) and sens_inp[0] > 2.*sens_inp[1] and sens_inp[0] > 2.*sens_inp[2]: 128 | # override w updates and steer using left and right sensors, which should be close enough to detect collisions 129 | # in tight areas 130 | if sens_inp[3] < np.tanh(.3): 131 | w = w + 1.5*(1.-sens_inp[3]) 132 | if sens_inp[4] < np.tanh(.3): 133 | w = w - 1.5*(1.-sens_inp[25]) 134 | if sens_inp[1] < np.tanh(.3): 135 | w = w + 1.5*(1.-sens_inp[1]) 136 | if sens_inp[2] < np.tanh(.3): 137 | w = w - 1.5*(1.-sens_inp[2]) 138 | 139 | return u, w 140 | 141 | # if the right hand side detects an approaching object , alter w to move left 142 | if sens_inp[2] < np.tanh(.3): 143 | w = w + 4.*(1.-sens_inp[2]) 144 | 145 | # if the left hand side detects and approaching object, alter w to move to the right 146 | if sens_inp[1] < np.tanh(.3): 147 | w = w - 4.*(1.-sens_inp[1]) 148 | 149 | # if robot is very close to the right hand slide, adjust left a little 150 | if sens_inp[4] < np.tanh(.3): 151 | w = w + 2.*(1.-sens_inp[4]) 152 | 153 | # if robot is very close to the left hand slide, adjust right a little 154 | if sens_inp[3] < np.tanh(.3): 155 | w = w - 2.*(1.-sens_inp[3]) 156 | 157 | return u, w 158 | 159 | 160 | def detect_robot_presence(robot_poses, raw_sensor_input, robot_id): 161 | # CHECK IF SENSORS ARE BEING TRIGGERED BY OTHER ROBOTS 162 | # compute distances to other robots 163 | robot_pose = robot_poses[robot_id] 164 | distances = [np.linalg.norm(robot_pose - r_p) for r_p in robot_poses] 165 | 166 | # vectors from current robot to all the other robots 167 | vectors = [r_p[:2] - robot_pose[:2] for r_p in robot_poses] 168 | 169 | sensor_angles = [-np.pi/2., -np.pi/4., 0., np.pi/4, np.pi/2.] 170 | 171 | # for each sensor, if there is a robot infront of it, ignore it (by setting the distance it measures to max distance) 172 | for i in range(len(sensor_angles)): 173 | if robot_infront_of_sensor(sensor_angles[i], raw_sensor_input[i], robot_poses, distances, vectors, robot_id): 174 | raw_sensor_input[i] = MAX_DISTANCE 175 | 176 | return raw_sensor_input 177 | 178 | def robot_infront_of_sensor(sensor_angle, raw_sensor_value, robot_poses, robot_distances, robot_displacement_vectors, robot_id): 179 | # check if any of the other robots are infront of the sensor (within a distance of ROBOT_DETECTION_THRESHOLD) 180 | 181 | robot_pose = robot_poses[robot_id] 182 | found_robot = False 183 | 184 | for i in range(len(robot_poses)): 185 | if i != robot_id: 186 | # if robot within range and sensor_value is similar to robot_distance[i] 187 | if robot_distances[i] < ROBOT_DETECTION_THRESHOLD and abs(raw_sensor_value - robot_distances[i]) < SENSOR_ROBOT_TOLERANCE: 188 | # if the angle between the two robots (minus the current robot yaw) is within +- pi/8, there is a robot infront of the sensor 189 | angle_to_robot = np.arctan2(robot_displacement_vectors[i][Y], robot_displacement_vectors[i][X]) 190 | angle_to_robot -= robot_pose[YAW] 191 | 192 | # pi/8. is used since the sensors are pi/4. apart on the robot 193 | if abs(angle_to_robot - sensor_angle) < np.pi/8.: 194 | found_robot = True 195 | break 196 | 197 | return found_robot 198 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/precomputed_rrt_paths.py: -------------------------------------------------------------------------------- 1 | SIMPLE_PATH = [(-1.0000114075275892, -0.99998714913749409), (-0.99961409312041205, -0.94998927456787063), (-0.99840686175330395, -0.90000439772852825), (-0.99639003027561746, -0.85004563762573371), (-0.99356412802395377, -0.8001261064111681), (-0.98992989668323172, -0.75025890594051736), (-0.98548829009202699, -0.70045712433476626), (-0.98024047399222836, -0.65073383254509642), (-0.97418782572307583, -0.60110208092229056), (-0.9673319338596662, -0.55157489579154451), (-0.95967459779601727, -0.50216527603358352), (-0.95121782727279669, -0.45288618967298278), (-0.94196384184985016, -0.40375057047458607), (-0.93191507032365406, -0.35477131454891608), (-0.92107415008985605, -0.30596127696746789), (-0.90944392645106564, -0.25733326838877246), (-0.89702745187007649, -0.20890005169611825), (-0.8838279851687183, -0.16067433864781078), (-0.86984899067255039, -0.11266878654085055), (-0.85509413730161343, -0.064895994888905961), (-0.83956729760748994, -0.017368502115449713), (-0.82327254675691108, 0.029901217737068331), (-0.80621416146219671, 0.076900758281152504), (-0.78839661885878698, 0.12361778404043688), (-0.769824595330177, 0.17004003368724652), (-0.75050296528055682, 0.21615532326070097), (-0.71078307443620781, 0.30340893063348662), (-0.688228646608122, 0.34803100265493836), (-0.66441456499747265, 0.39199372599166193), (-0.63936008666679323, 0.43526155054660665), (-0.61308547171529071, 0.47779948814694451), (-0.58561196689568185, 0.51957314083697026), (-0.55696178843318034, 0.56054872869372674), (-0.52715810406052777, 0.60069311714286466), (-0.49622501428359578, 0.6399738437526441), (-0.46418753289270964, 0.67835914448441614), (-0.43107156673544877, 0.71581797937835168), (-0.39690389476728938, 0.75232005765365284), (-0.36171214639701832, 0.78783586220294444), (-0.32552477914443823, 0.822336673461042), (-0.28837105562842735, 0.85579459262879309), (-0.25028101990396046, 0.8881825642332144), (-0.21128547316723045, 0.91947439800567787), (-0.17141594884851274, 0.94964479006045766), (-0.13070468711291272, 0.97866934335651101), (-0.089184608789621977, 1.0065245874259456), (-0.046889288750757707, 1.0331879973532223), (-0.0038529287613161545, 1.0586380119897438), (0.039889670177804648, 1.0828540513891005), (0.088045407964632449, 1.1083004848877804), (0.13244707739595674, 1.1312891425096598), (0.17703149890768799, 1.1539213316660106), (0.22179581073373766, 1.1761955996513969), (0.26673713956129141, 1.1981105167344772), (0.3118526007152429, 1.2196646762497751), (0.35713929834335412, 1.2408566946879667), (0.40259432560213071, 1.2616852117846902), (0.44821476484340739, 1.2821488906078526), (0.49399768780162523, 1.3022464176434481), (0.539940155781788, 1.321976502879866), (0.58603921984809038, 1.3413378798906983), (0.63229192101320342, 1.3603293059160206), (0.67869529042820576, 1.3789495619421732), (0.79095916834599778, 1.4205901472780684), (0.83861084774220707, 1.4357274660910431), (0.8866857400747048, 1.4494615731731764), (0.93514253427712779, 1.4617806667172959), (0.98393959111236229, 1.4726741608465286), (1.0330349789534106, 1.4821326947108118), (1.082386509815509, 1.4901481405307329), (1.1319517756085371, 1.4967136105817824), (1.1816881845785638, 1.501823463113021), (1.2315529979072173, 1.5054733071950721), (1.2815033664374291, 1.5076600064932755), (1.3314963674939919, 1.5083816819627613), (1.381489041767292, 1.5076377134631243), (1.4314384302285232, 1.5054287402913147), (1.4813016110446575, 1.5017566606322867)] 2 | CORRIDOR_PATH = [(-10.999999488178014, -0.1999974537722253), (-10.995100094505121, -0.15031928895378927), (-10.980571387616546, -0.10256113997127994), (-10.956975469128921, -0.058570723414772985), (-10.925225242305384, -0.020049985951120175), (-10.88654909271134, 0.011510742346383496), (-10.780975174040858, 0.064638196160321293), (-10.735625112518957, 0.08569430425226976), (-10.69011504186922, 0.10640230361184333), (-10.644447634139512, 0.12676097840341072), (-10.598625570615484, 0.14676913330134944), (-10.552651541663138, 0.16642559356023057), (-10.506528246570877, 0.18572920508378576), (-10.460258393391015, 0.20467883449267177), (-10.413844698780782, 0.22327336919101715), (-10.367289887842812, 0.2415117174317416), (-10.320596693965152, 0.25939280838065759), (-10.273767858660776, 0.27691559217934714), (-10.226806131406612, 0.29407904000679519), (-10.179714269482119, 0.31088214413979998), (-10.132495037807399, 0.327323918012139), (-10.030909715988377, 0.36270287765775855), (-9.9840208001726367, 0.38006353088391132), (-9.9373683492519227, 0.39804995715220226), (-9.8909607142803555, 0.41665893679000465), (-9.8448062024886109, 0.4358871386840959), (-9.7989130757968699, 0.45573112087694811), (-9.7532895493359, 0.47618733118285528), (-9.7079437899764898, 0.49725210782379792), (-9.6628839148675372, 0.51892168008491879), (-9.618117989983034, 0.54119216898950473), (-9.5736540286782059, 0.56405958799334499), (-9.5294999902550863, 0.58751984369834398), (-9.4856637785377433, 0.61156873658526534), (-9.4421532404574613, 0.63620196176546795), (-9.3989761646480865, 0.66141510975150553), (-9.3561402800518234, 0.68720366724645032), (-9.313653254535712, 0.71356301795179977), (-9.271522693519028, 0.74048844339381814), (-9.2297561386118723, 0.7679751237681729), (-9.1883610662651858, 0.79601813880270411), (-9.1473448864324158, 0.82461246863818438), (-9.1067149412430943, 0.8537529947268987), (-9.0659203261806596, 0.88265804934859848), (-9.0240130497311046, 0.90992418991572954), (-8.9810421462200214, 0.93548125728049403), (-8.9370772763167885, 0.95928782056118989), (-8.8921897120221427, 0.98130528664018968), (-8.8464522211282404, 1.0014979627277267), (-8.7999389492538889, 1.0198331142239232), (-8.7527252996461442, 1.0362810177852599), (-8.7048878109431733, 1.0508150095094602), (-8.6565040330965068, 1.0634115281606746), (-8.6076524016538425, 1.0740501533648972), (-8.5584121106061932, 1.0827136387136873), (-8.508862984005523, 1.0893879397225394), (-8.4590853465609772, 1.0940622365985722), (-8.409159893423487, 1.09672895178063), (-8.3591675593698547, 1.0973837622233624), (-8.3091893875983711, 1.0960256064053671), (-8.2593063983486736, 1.0926566860500353), (-8.2095994575588254, 1.0872824625563107), (-8.160149145772527, 1.0799116481451465), (-8.1110356275089899, 1.0705561917360167), (-8.0623385213072325, 1.0592312595763711), (-8.0141367706554654, 1.0459552106554433), (-7.9665085160148248, 1.0307495669422635), (-7.9130858058702458, 1.0121427135531951), (-7.8659902229380219, 0.99534996729933312), (-7.8190105704109554, 0.97823557257124527), (-7.772149045021056, 0.96080032962471673), (-7.7254078379767979, 0.94304505371814518), (-7.6787891348606641, 0.92497057507442193), (-7.6322951155269489, 0.90657773884210702), (-7.5859279539998292, 0.88786740505591499), (-7.5396898183717109, 0.86884044859650178), (-7.4935828707018484, 0.84949775914954895), (-7.4476092669152507, 0.82984024116416855), (-7.4017711567018667, 0.8098688138106116), (-7.3560706834160756, 0.78958441093728648), (-7.3105099839764582, 0.76898798102709254), (-7.2650911887658811, 0.74808048715306974), (-7.2198164215318776, 0.72686290693336808), (-7.1746877992873443, 0.70533623248553656), (-7.1297074322115552, 0.68350147038012476), (-7.0848774235514842, 0.66135964159362626), (-7.0401998695234642, 0.63891178146073013), (-6.9956768592151697, 0.61615893962591439), (-6.9513104744879275, 0.59310217999436521), (-6.9071027898793771, 0.56974258068222738), (-6.8630558725064628, 0.5460812339661949), (-6.8191717819687785, 0.52211924623243267), (-6.7754525702522601, 0.497857737924849), (-6.6649953259376948, 0.43760865777565727), (-6.6201869391457686, 0.41542572613707351), (-6.5748465787010337, 0.39435143616757529), (-6.5290016693288058, 0.37439853493419029), (-6.4826799409375901, 0.35557909121692366), (-6.4359094118462696, 0.33790448820879604), (-6.3887183718368545, 0.32138541663056452), (-6.3411353650430238, 0.30603186826429862), (-6.2931891726848264, 0.29185312990971601), (-6.2449087956599723, 0.27885777776693654), (-6.1963234370022482, 0.26705367224905352), (-6.1474624842176766, 0.25644795322765779), (-6.0983554915090821, 0.24704703571418829), (-6.0490321618998406, 0.23885660597972969), (-5.9995223292676041, 0.2318816181155916), (-5.9498559402988791, 0.22612629103676252), (-5.900063036375375, 0.22159410593004125), (-5.8501737354030654, 0.21828780414839555), (-5.8002182135949756, 0.21620938555281555), (-5.7502266872186905, 0.21536010730267385), (-5.7002293943196412, 0.21574048309531424), (-5.6502565764312171, 0.21735028285533575), (-5.6003384602827699, 0.22018853287375517), (-5.5505052395165686, 0.22425351639697011), (-5.5007870564247723, 0.22954277466516038), (-5.4512139837174596, 0.23605310839949878), (-5.4018108373687888, 0.24374987848215834), (-5.3524021144166367, 0.25141412418882192), (-5.3028950970516346, 0.25841539533312297), (-5.253298680769678, 0.26475243391605385), (-5.2036217771299969, 0.27042410128891881), (-5.1538733121539089, 0.27542937835793069), (-5.104062224720983, 0.27976736576732186), (-5.0541974649628854, 0.28343728406094115), (-5.0042879926552066, 0.28643847382230847), (-4.9543427756075582, 0.28877039579310004), (-4.9043707880522165, 0.2904326309700429), (-4.8543810090316279, 0.29142488068020178), (-4.8043824207850321, 0.2917469666346455), (-4.7543840071345276, 0.29139883096048091), (-4.7043947518708373, 0.29038053621125437), (-4.6544236371390957, 0.28869226535570869), (-4.6044796418249145, 0.28633432174490947), (-4.5545717399410508, 0.28330712905773758), (-4.504708899014938, 0.2796112312247625), (-4.4549000784773884, 0.27524729233050671), (-4.4051542280527487, 0.27021609649412293), (-4.2721892843911462, 0.25280861416844225), (-4.2228905885830557, 0.24446906638055577), (-4.1737764409593519, 0.23510370632970723), (-4.1248681843316835, 0.2247166037824555), (-4.0761870720406819, 0.2133122725089458), (-4.0277542587202113, 0.20089566832142713), (-3.9795907911045099, 0.18747218692067591), (-3.9317175988822197, 0.17304766155126794), (-3.8841554856012817, 0.15762836046671014), (-3.8369251196286429, 0.14122098420553941), (-3.7900470251687102, 0.12383266267956472), (-3.7435415733444479, 0.10547095207553125), (-3.6974289733450005, 0.086143831571536378), (-3.6517292636436798, 0.065859699869635691), (-3.6064623032901446, 0.044627371546144712), (-3.5418818320556289, 0.014437960395621285), (-3.4958480624163117, -0.0050734726098868865), (-3.4492375174343088, -0.023163830859928458), (-3.4020940540473568, -0.03981609271882558), (-3.3544620306284219, -0.055014589688993309), (-3.3063862652478817, -0.068745021153787889), (-3.2579119935031695, -0.080994467833289674), (-3.2090848259555553, -0.091751403940350196), (-3.1599507052141167, -0.1010057080254716), (-3.1105558627072769, -0.10874867250031328), (-3.060946775182587, -0.11497301183086117), (-3.01117012097568, -0.11967286939255595), (-2.9612727360895503, -0.12284382298092544), (-2.9113015701254739, -0.12448288897254001), (-2.8613036421070461, -0.12458852513237151), (-2.8113259962388972, -0.12316063206491901), (-2.7614156576417148, -0.12020055330773083), (-2.7116195881052203, -0.1157110740672409), (-2.6619846419007396, -0.10969641859810197), (-2.6125575216949342, -0.10216224622848769)] 3 | MAZE_PATH = [(-6.5001817953151999, -8.0448398557720378), (-6.5013016358772955, -7.9948525512335165), (-6.5028503961811097, -7.9448766970694038), (-6.5048279621732181, -7.8949159735892716), (-6.507234188222192, -7.8449740599884432), (-6.5100688971293215, -7.7950546340770481), (-6.5133318801416724, -7.7451613720091856), (-6.5170228969674504, -7.6952979480121995), (-6.521141675793702, -7.6454680341161092), (-6.5256879133063279, -7.5956752998831893), (-6.530661274712422, -7.545923412137741), (-6.5360613937649266, -7.4962160346960554), (-6.5418878727896015, -7.4465568280966101), (-6.5481402827143089, -7.3969494493304957), (-6.5548181631006139, -7.3473975515721097), (-6.5619210221776907, -7.2979047839101314), (-6.5694483368785352, -7.2484747910787961), (-6.5773995528784877, -7.1991112131894912), (-6.5857740846360544, -7.1498176854626907), (-6.5945713154360233, -7.100597837960251), (-6.6037905974348883, -7.051455295318088), (-6.6134312517085467, -7.0023936764792527), (-6.6234925683023071, -6.9534165944274253), (-6.6339738062831657, -6.9045276559208464), (-6.6448741937943678, -6.8557304612267149), (-6.6561929281122563, -6.8070286038560521), (-6.6679291757053791, -6.7584256702990722), (-6.6800820722958765, -6.7099252397610645), (-6.6926507229231218, -6.6615308838988199), (-6.7056342020096338, -6.6132461665575999), (-6.7180880760930242, -6.5648247322701199), (-6.7287474175986688, -6.5159770831425616), (-6.7375911789128997, -6.4667683209914983), (-6.7446072365634642, -6.4172659036599482), (-6.7497859725861034, -6.3675376915482023), (-6.7531202877093399, -6.3176518545873783), (-6.7546056110865118, -6.2676767787886538), (-6.7551600112064207, -6.2043522161332172), (-6.7558302639853256, -6.1543567485542194), (-6.7567191065566607, -6.1043646894236572), (-6.7578265219281786, -6.0543769944535422), (-6.7591524889291108, -6.0043946192724578), (-6.7606969822105807, -5.9544185194072847), (-6.7624599722460843, -5.9044496502649375), (-6.7644414253320502, -5.8544889671140998), (-6.7666413035884965, -5.804537425066961), (-6.7690595649597398, -5.7545959790609569), (-6.7716961632152088, -5.7046655838405149), (-6.774551047950327, -5.6547471939388014), (-6.7776241645874737, -5.6048417636594747), (-6.7809154543770305, -5.5549502470584384), (-6.7844248543984964, -5.5050735979256071), (-6.7881522975617052, -5.4552127697666668), (-6.7920977126080917, -5.4053687157848529), (-6.795608358851565, -5.3554936150872141), (-6.7978389836709399, -5.305544777361523), (-6.7987838176828195, -5.2555550847647261), (-6.798442235400123, -5.2055576308417866), (-6.79681446295284, -5.1555855142757245), (-6.7939015779383292, -5.1056718169760398), (-6.7897055087079394, -5.0558495821782508), (-6.7842290330904298, -5.0061517925690229), (-6.7774757765530254, -4.9566113484513901), (-6.7694502098013318, -4.9072610459645123), (-6.7601576458196915, -4.8581335553723868), (-6.7496042363539566, -4.8092613994358944), (-6.7377969678389782, -4.760676931882494), (-6.7247436567735424, -4.7124123159878106), (-6.7104529445457901, -4.6644995032833183), (-6.6949342917125492, -4.6169702124041807), (-6.6781979717363793, -4.5698559080912915), (-6.660255064184466, -4.5231877803613756), (-6.6411174473938619, -4.4769967238589725), (-6.6207977906079432, -4.4313133174039505), (-6.5993095455892723, -4.3861678037481084), (-6.5766669377144389, -4.3415900695542442), (-6.55288495655675, -4.2976096256109688), (-6.5279793459630246, -4.2542555872963437), (-6.5019665936310505, -4.211556655303287), (-6.4192043369476117, -4.0973816803955749), (-6.3847448944755749, -4.0611680911928874), (-6.3477219137407372, -4.027579758159936), (-6.3083348708286477, -3.996797651846919), (-6.2667959791533967, -3.9689876231375423), (-6.2233290460731432, -3.9442995096611879), (-6.1781682670386466, -3.9228663284828826), (-6.131556963771807, -3.9048035594207677), (-6.0837462732727641, -3.8902085228524901), (-6.0349937947190808, -3.8791598553628219), (-5.9855622015473555, -3.8717170860576764), (-5.9357178261952424, -3.867920315827289), (-5.8857292251291486, -3.8677900012866666), (-5.8358657318890756, -3.8713268445574087), (-5.7863960059466795, -3.8785117894847474), (-5.7375865851951389, -3.889306124310187), (-5.6897004498698704, -3.9036516902465537), (-5.6429956056375481, -3.9214711948316747), (-5.5977236934876, -3.942668628372366), (-5.5109728988247619, -3.9948819589195672), (-5.4707078733223007, -4.0245123713456437), (-5.4323130371912622, -4.0565290885059628), (-5.3959296047542162, -4.0908143544864419), (-5.3616913924896838, -4.1272420697617926), (-5.329724326861693, -4.1656782549833906), (-5.3001459811684306, -4.2059815437487389), (-5.2730651431134197, -4.2480037025401609), (-5.2485814146896868, -4.2915901759203985), (-5.2267848458485204, -4.3365806549799224), (-5.2077556033001731, -4.3828096669452394), (-5.1915636756646331, -4.4301071837796213), (-5.1782686160569167, -4.4782992475378851), (-5.1679193230536367, -4.5272086101751698), (-5.1605538608464343, -4.5766553854565517), (-5.1561993192437523, -4.6264577105697811), (-5.1548717140358509, -4.6764324150077785), (-5.1565759280895156, -4.7263956942607708), (-5.1613056933891137, -4.7761637858402555), (-5.1690436140900458, -4.8255536451484096), (-5.1797612304998131, -4.8743836187071405), (-5.1934191237513634, -4.9224741122706552), (-5.2099670607837556, -4.9696482513642861), (-5.3000015119688335, -5.0000028610383174), (-5.3000835949922589, -5.0500027764740043), (-5.3003092934682634, -5.1000022498851969), (-5.3006786055347774, -5.1500008687640841), (-5.3011915281448871, -5.1999982206099036), (-5.3018480570668522, -5.2499938929323484), (-5.3026481868841557, -5.2999874732549666), (-5.3035919109955287, -5.3499785491185676), (-5.304679221615018, -5.3999667080846203), (-5.3059101097720465, -5.449951537738662), (-5.3072845653114911, -5.4999326256936945), (-5.3088025768937541, -5.5499095595935923), (-5.3104641319948733, -5.5998819271165008), (-5.3122692169066177, -5.649849315978237), (-5.3142178167365941, -5.6998113139356974), (-5.3163099154083788, -5.7497675087902511), (-5.318545495661656, -5.7997174883911464), (-5.3244062620555326, -5.9290177536453692), (-5.3263135021318053, -5.978981322819207), (-5.327996636278213, -6.0289529434527491), (-5.3294556306379199, -6.0789316103492936), (-5.3306904558627455, -6.1289163181704014), (-5.3317010871137427, -6.1789060614561189), (-5.3324875040617119, -6.2288998346452011), (-5.3330496908875986, -6.2788966320953401), (-5.3333876362828168, -6.3288954481033945), (-5.3335013334494779, -6.3788952769256184), (-5.3333907801005225, -6.4288951127978935), (-5.33305597845977, -6.4788939499559595), (-5.3324969352618723, -6.5288907826556448), (-5.3317136617521825, -6.5788846051930987), (-5.3307061736865187, -6.6288744119250209), (-5.3294744913308598, -6.6788591972888911), (-5.328018639460927, -6.7288379558231952), (-5.3263386473616947, -6.7788096821876493), (-5.3244345488267921, -6.8287733711834271), (-5.3223063821578283, -6.8787280177733745), (-5.3199541901636263, -6.9286726171022295), (-5.3173780201593548, -6.9786061645168331), (-5.3145779239655759, -7.0285276555863421), (-5.3115539579072095, -7.0784360861224274), (-5.3083061828123927, -7.1283304521994797), (-5.3048346640112669, -7.1782097501748003), (-5.3011243103902848, -7.2280726018570007), (-5.296900164042599, -7.2778935305646364), (-5.2920622772385322, -7.3276586107005013), (-5.2866113849522067, -7.3773602819293531), (-5.2805483152858379, -7.4269909935490634), (-5.273873989343925, -7.4765432056377188), (-5.2665894210933208, -7.5260093901990972), (-5.2586957172091857, -7.5753820323063161), (-5.2501940769068618, -7.6246536312435129), (-5.2410857917596925, -7.6738167016453485), (-5.2313722455027953, -7.7228637746341944), (-5.221054913822857, -7.7717873989548041), 4 | (-5.2101353641339374, -7.8205801421063139), (-5.1986152553393481, -7.8692345914713906), (-5.1864963375796371, -7.9177433554423615), (-5.1737804519666994, -7.9660990645441494), (-5.1604695303040771, -8.0142943725538469), (-5.146565594793481, -8.0623219576167582), (-5.1320707577275737, -8.1101745233587401), (-5.1169872211690715, -8.1578447999946739), (-5.1013172766162027, -8.2053255454328831), (-5.085063304654585, -8.2526095463753677), (-5.0682277745955657, -8.2996896194136482), (-5.0508132441010805, -8.3465586121200719), (-5.0328223587950944, -8.3932094041344083), (-5.0137656231396175, -8.4394289702122656), (-4.9913656556646719, -8.4841157295103411), (-4.96546369786968, -8.5268679643271241), (-4.936225676381663, -8.567411806016203), (-4.9038388884823396, -8.6054875327854603), (-4.8685108022912509, -8.6408512334616887), (-4.8304677277355115, -8.6732763699731876), (-4.7899533668199066, -8.7025552285411525), (-4.7472272524842154, -8.7285002502836058), (-4.7025630860483725, -8.7509452327080623), (-4.656246983895703, -8.7697463943962735), (-4.6085756446258914, -8.7847832960606915), (-4.560217073422975, -8.7974891791104941), (-4.511706190225194, -8.8095996284692042), (-4.463050676873797, -8.8211152610098225), (-4.414257826433384, -8.8320343506329557), (-4.3653349525542664, -8.8423552606561309), (-4.316289388376207, -8.8520764440591133), (-4.2671284854292457, -8.8611964437158015), (-4.2178596125317611, -8.8697138926126335), (-4.1684901546859496, -8.8776275140534882), (-4.1190275119708684, -8.8849361218510605), (-4.0694790984332263, -8.8916386205046507), (-4.0198523409760805, -8.8977340053643772), (-4.0000114318608899, -8.8999719190579309), (-3.9501691081565209, -8.8965335112552371), (-3.9012656013115468, -8.8863096715297587), (-3.8542202416467468, -8.8694925964428624), (-3.8099174284288022, -8.8463984278835781), (-3.7691900041697273, -8.8174613099605725), (-3.732803598134665, -8.7832252275865574), (-3.7014422333822936, -8.7443337801805097), (-3.6756954679091245, -8.7015180827304111), (-3.6548438444852653, -8.6560806055722495), (-3.6349385079511918, -8.6102140403157925), (-3.6156358591905824, -8.5640905974501074), (-3.5969392122736545, -8.5177181959054007), (-3.5788517772262205, -8.4711047973555793), (-3.5613766594785523, -8.4242584048513027), (-3.5445168593322158, -8.3771870614459445), (-3.5282752714449459, -8.329898848814679), (-3.5126546843336652, -8.2824018858669444), (-3.4976577798957229, -8.2347043273524996), (-3.4832871329484396, -8.1868143624613445), (-3.4695452107870377, -8.1387402134177087), (-3.4557940053696763, -8.0906731610064924), (-3.4383887926982397, -8.0438181230442023), (-3.416870603331648, -7.9987037845588693), (-3.3914112376088639, -7.9556903365736273), (-3.3622139620587976, -7.9151211966882524), (-3.3295118865286968, -7.8773202672471765), (-3.2935661030447143, -7.8425893493164862), (-3.2546636012639105, -7.8112057331166254), (-3.2131149771606293, -7.7834199841486766), (-3.1692519532410168, -7.7594539426897047), (-3.1234247300841722, -7.7394989526291109), (-3.0759991903551258, -7.7237143337868872), (-3.027353977612691, -7.7122261099107225), (-2.9778774732348592, -7.7051260025075905), (-2.866466834421745, -7.6976667547134854), (-2.8167307523993044, -7.6925524475602423), (-2.7671588361395068, -7.6860363819580346), (-2.7177905847320583, -7.6781237499324932), (-2.6686653349857434, -7.6688208562986375), (-2.6198222300847065, -7.6581351136371758), (-2.5713001883990168, -7.6460750363881349), (-2.5231378724743623, -7.6326502340665199), (-2.4753736582255903, -7.6178714036054158), (-2.4280456043586307, -7.6017503208326351), (-2.3811914220451795, -7.5842998310876881), (-2.3348484448742903, -7.5655338389865729), (-2.2890535991048342, -7.5454672973425225), (-2.2438433742425157, -7.5241161952515476), (-2.1992629269020267, -7.5014800863937658), (-2.1558119766391433, -7.4767552637917669), (-2.1138642993108654, -7.4495579422653631), (-2.073560426668247, -7.419979237400959), (-2.0350353834410813, -7.3881182428183516), (-1.9984182349824224, -7.3540816981904902), (-1.9638316548780137, -7.317983631647679), (-1.9313915139692133, -7.2799449777642264), (-1.9012064921662502, -7.2400931724073478), (-1.8733777143523058, -7.1985617258056456), (-1.847998411598204, -7.1554897752674647), (-1.8251536088226945, -7.1110216190475803), (-1.8049198399447206, -7.065306232923863), (-1.7873648914819624, -7.0184967711034378), (-1.7725475754546378, -6.9707500531304261), (-1.7605175323553763, -6.9222260385141787), (-1.7504802641385375, -6.8732457154003246), (-1.7382039984995035, -6.824783484429501), (-1.7231298523721124, -6.7771173051485931), (-1.7053088621242369, -6.730408560407434), (-1.6848013640831518, -6.6848153914804493), (-1.6616767902555081, -6.6404921626495144), (-1.6360134332521623, -6.5975889385745283), (-1.6078981812137625, -6.5562509762211798), (-1.5774262236345411, -6.5166182330660591), (-1.5447007290803096, -6.478824893244199), (-1.5098324958918008, -6.4429989132433478), (-1.4729395770559646, -6.4092615886831226), (-1.4341468805152928, -6.3777271436457834), (-1.3935857462683943, -6.3485023439490407), (-1.3513935016936325, -6.3216861356702205), (-1.3077129966013565, -6.2973693101456316), (-1.2626921195888969, -6.2756341965793627), (-1.1780085117877337, -6.2371796653778411), (-1.1339669783572999, -6.2135192867639706), (-1.0911741621141942, -6.1876688503839112), (-1.0497388765434166, -6.1596940886740441), (-1.0097664832037703, -6.1296661358008242), (-0.97135862381470572, -6.0976613467809671), (-0.93461296180196651, -6.0637611033260699), (-0.89962293395923154, -6.0280516069053647), (-0.86647751285722852, -5.9906236595527966), (-0.83526098060446397, -5.9515724329758006), (-0.80605271453485194, -5.9109972265528823), (-0.77892698536719218, -5.8690012148353574), (-0.75395276834973979, -5.8256911851953195), (-0.73119356787008605, -5.7811772662869307), (-0.71070725597633222, -5.7355726480115017), (-0.69254592522016467, -5.6889932936984495), (-0.67675575619602268, -5.6415576452339797), (-0.66337690011317663, -5.5933863218873014), (-0.65244337669931685, -5.5446018136001998), (-0.64398298769525975, -5.4953281695198628), (-0.63801724616073852, -5.4456906825669655), (-0.63456132177103919, -5.3958155708410915), (-0.6336240022435814, -5.3458296566736072), (-0.63520767099252895, -5.2958600441440975), (-0.6393083010682491, -5.2460337958803773), (-0.6459154653970316, -5.1964776099639032), (-0.65564280164570787, -5.1474482009592952), (-0.66958511832103573, -5.0994479568345259), (-0.68766072714612381, -5.0528466264252447), (-0.70973186906930674, -5.00799937094044), (-0.73563033401202371, -4.9652479833147334), (-0.76515874284283725, -4.9249182833129614), (-0.79809205165661112, -4.8873176343705671), (-0.83417926689432942, -4.8527326010946084), (-0.87314535823217332, -4.8214267652782086), (-0.91469335466121326, -4.7936387170731782), (-0.9585066077829334, -4.7695802366306266), (-1.0042512050714092, -4.7494346800677576), (-1.0515785147100167, -4.7333555820618383), (-1.1001278426077976, -4.7214654857213398), (-1.1495291813456434, -4.713855008652116), (-1.1994060301018463, -4.7105821523363804), (-1.2493782640656073, -4.7116718600878906), (-1.2992818994800523, -4.7147724494713312), (-1.3492093912785941, -4.7174615766417682), (-1.3991573545334766, -4.7197390333199758), (-1.4491223949282279, -4.7216046647367049), (-1.4991011169858635, -4.7230583441091412), (-1.5490901242996329, -4.7240999726495145), (-1.5990860197638308, -4.7247294795718178), (-1.6490854058046527, -4.7249468220966149), (-1.6990848846110851, -4.7247519854539473), (-1.7490810583658107, -4.7241449828843392), (-1.7990705294761133, -4.7231258556378979), (-1.8490499008047692, -4.7216946729715099), (-1.8990157759009052, -4.7198515321441326), (-1.9489647592308135, -4.7175965584101895), (-1.9988934564087002, -4.7149299050110534), (-2.0487984744273584, -4.7118517531646358), (-2.0986764218887481, -4.7083623120530698), (-2.1485239092344632, -4.7044618188084959), (-2.1983375489760792, -4.7001505384969473), (-2.1999878647401863, -4.7000131124867828), (-2.2496663152888967, -4.705610456917154), (-2.2995879610830952, -4.7082765392098551), (-2.3495799945830824, -4.708002130520077), (-2.3994693645968095, -4.7047881807341829), (-2.4490833753096251, -4.698645815181604), (-2.4982502840838503, -4.6895962961236739), (-2.5467998959588689, -4.6776709491527075), (-2.5945641527938075, -4.6629110547561048), (-2.6413777150134599, -4.6453677054208269), (-2.687078533943688, -4.6251016287729065), (-2.7315084127551215, -4.6021829773641922), (-2.7745135540734074, -4.5766910858339989), (-2.8159450923604128, -4.54871419628628), (-2.8556596092235051, -4.5183491528329274), (-2.8935196298691306, -4.4857010663605825), (-2.9293940989821712, -4.4508829506813798), (-2.9631588343837989, -4.4140153313271151), (-3.0032371683233161, -4.3617245185672946), (-3.0290244325938454, -4.3189101969905455), (-3.0505537500511855, -4.2738043479190955), (-3.0676236108563684, -4.2268291525205983), (-3.0800742447375833, -4.1784242886473129), (-3.0877891164059879, -4.1290428155447882), (-3.0906960163012633, -4.0791469333135444), (-3.0887677364577621, -4.0292036568142944), (-3.0820223251652878, -3.9796804445080367), (-3.070522918040937, -3.9310408231441345), (-3.0543771470931302, -3.8837400492483907), (-3.0337361333088726, -3.8382208480186519), (-3.0087930721934124, -3.7949092695109856), (-2.9797814255013417, -3.7542107009016377), (-2.934015141838588, -3.7009833019871845), (-2.8992925436267152, -3.6650128249892342), (-2.8629082015414671, -3.6307241432842323), (-2.8249434836357143, -3.5981939382990209), (-2.7854832922327275, -3.5674949588944398), (-2.7446158740550342, -3.5386958586735515), (-2.7024326228740292, -3.5118610424482908), (-2.6590278751216943, -3.4870505222078885), (-2.6144986989214991, -3.4643197829111729), (-2.5689446770102928, -3.4437196584028937), (-2.5224676840366431, -3.42529621773155), (-2.4751716587336623, -3.4090906621229613), (-2.4271623714758253, -3.395139232839989), (-2.3785471877395969, -3.3834731301344574), (-2.329434827996856, -3.3741184434725295), (-2.2799351245780795, -3.3670960931895824), (-2.2301587760490231, -3.362421783705055), (-2.1802170996501982, -3.3601059684018977), (-2.1302217823527783, -3.3601538262491744), (-2.0802846310876553, -3.3625652502200802), (-2.0305173227062276, -3.3673348475312945), (-1.9810311542320906, -3.3744519517031186), (-1.9319367939621532, -3.3839006464134389), (-1.8833440339738059, -3.3956598010921617), (-1.8353615445916227, -3.4097031181765241), (-1.7880966313626905, -3.4259991919215942), (-1.7416549950840579, -3.4445115786344536), (-1.6160826804666582, -3.4925275798470383), (-1.5668978478765516, -3.5013703690816578), (-1.5170297127631611, -3.5046125564108137), (-1.4671122846370896, -3.5022347186342619), (-1.4177699526337133, -3.4943136395161849), (-1.3696159103534149, -3.4809496044221908), (-1.3232486162415917, -3.4623087015826304), (-1.279244323037108, -3.4386226001410658), (-1.2381499161031622, -3.4101856709701175), (-1.2004761167359033, -3.3773513282302092), (-1.166691134920077, -3.3405276371383241), (-1.1372148504152597, -3.3001722425334226), (-1.1124135944901354, -3.256786681266306), (-1.0925955971574299, -3.2109101490995529), (-1.0780071564912603, -3.1631127995824935)] 5 | SQUARE_PATH = SIMPLE_PATH 6 | CORRDIOR_STRAIGHT_PATH = CORRIDOR_PATH = [(-10.3, 0.8),(-10., 0.8),(-9., 0.8),(-8., 0.8),(-7., 0.5),(-6., 0.3),(-5., 0.2),(-4., 0.2),(-3., 0.2),(-2.5, 0.2), (-1.98, 0.27)] 7 | -------------------------------------------------------------------------------- /src/ros/velocity_controller/rrt_navigation.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | import numpy as np 6 | import rrt 7 | 8 | SPEED = .5 9 | 10 | X = 0 11 | Y = 1 12 | YAW = 2 13 | 14 | 15 | def feedback_linearized(pose, velocity, epsilon): 16 | u = 0. # [m/s] 17 | w = 0. # [rad/s] going counter-clockwise. 18 | 19 | # check if velocity is near zero: 20 | threshold = 0.01 21 | if np.linalg.norm(velocity) < threshold: 22 | return u, w 23 | 24 | theta = pose[YAW] 25 | 26 | u = velocity[X]*np.cos(theta) + velocity[Y]*np.sin(theta) 27 | w = (velocity[Y]*np.cos(theta) - velocity[X]*np.sin(theta)) / epsilon 28 | 29 | return u, w 30 | 31 | 32 | def get_velocity(position, path_points): 33 | v = np.zeros_like(position) 34 | if len(path_points) == 0: 35 | return v 36 | # Stop moving if the goal is reached. 37 | if np.linalg.norm(position - path_points[-1]) < .2: 38 | return v 39 | 40 | minp = path_points[0] 41 | mind = np.linalg.norm(path_points[0]-position) 42 | nextp = path_points[1] 43 | 44 | for u, v in zip(path_points[1:], path_points[2:]): 45 | if np.linalg.norm(u-position) < mind: 46 | minp = u 47 | mind = np.linalg.norm(u-position) 48 | nextp = v 49 | 50 | vec = nextp - position 51 | vec = vec / np.linalg.norm(vec) 52 | 53 | return SPEED * vec 54 | 55 | 56 | def get_path(final_node): 57 | # Construct path from RRT solution. 58 | if final_node is None: 59 | return [] 60 | path_reversed = [] 61 | path_reversed.append(final_node) 62 | while path_reversed[-1].parent is not None: 63 | path_reversed.append(path_reversed[-1].parent) 64 | path = list(reversed(path_reversed)) 65 | # Put a point every 5 cm. 66 | distance = 0.05 67 | offset = 0. 68 | points_x = [] 69 | points_y = [] 70 | for u, v in zip(path, path[1:]): 71 | center, radius = rrt.find_circle(u, v) 72 | du = u.position - center 73 | theta1 = np.arctan2(du[1], du[0]) 74 | dv = v.position - center 75 | theta2 = np.arctan2(dv[1], dv[0]) 76 | # Check if the arc goes clockwise. 77 | clockwise = np.cross(u.direction, du).item() > 0. 78 | # Generate a point every 5cm apart. 79 | da = distance / radius 80 | offset_a = offset / radius 81 | if clockwise: 82 | da = -da 83 | offset_a = -offset_a 84 | if theta2 > theta1: 85 | theta2 -= 2. * np.pi 86 | else: 87 | if theta2 < theta1: 88 | theta2 += 2. * np.pi 89 | angles = np.arange(theta1 + offset_a, theta2, da) 90 | offset = distance - (theta2 - angles[-1]) * radius 91 | points_x.extend(center[X] + np.cos(angles) * radius) 92 | points_y.extend(center[Y] + np.sin(angles) * radius) 93 | return zip(points_x, points_y) 94 | -------------------------------------------------------------------------------- /src/ros/worlds/maze.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 1 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.001 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | 1 75 | 76 | 77 | EARTH_WGS84 78 | 0 79 | 0 80 | 0 81 | 0 82 | 83 | 84 | -3.66 -5.84 0 0 -0 0 85 | 86 | 87 | 88 | 89 | 7.25 0.15 1 90 | 91 | 92 | 0 0 0.5 0 -0 0 93 | 10 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 0 0 0.5 0 -0 0 109 | 110 | 111 | 7.25 0.15 1 112 | 113 | 114 | 115 | 119 | 1 1 1 1 120 | 121 | 122 | 0 3.55 0 0 -0 3.14159 123 | 0 124 | 0 125 | 1 126 | 127 | 128 | 129 | 130 | 131 | 7.25 0.15 1 132 | 133 | 134 | 0 0 0.5 0 -0 0 135 | 10 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 0 0 0.5 0 -0 0 151 | 152 | 153 | 7.25 0.15 1 154 | 155 | 156 | 157 | 161 | 1 1 1 1 162 | 163 | 164 | -3.55 0 0 0 0 -1.5708 165 | 0 166 | 0 167 | 1 168 | 169 | 170 | 171 | 172 | 173 | 2.25 0.15 1 174 | 175 | 176 | 0 0 0.5 0 -0 0 177 | 10 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 0 0 0.5 0 -0 0 193 | 194 | 195 | 2.25 0.15 1 196 | 197 | 198 | 199 | 203 | 1 1 1 1 204 | 205 | 206 | 2.49237 2.07698 0 0 -0 0 207 | 0 208 | 0 209 | 1 210 | 211 | 212 | 213 | 214 | 215 | 7.25 0.15 1 216 | 217 | 218 | 0 0 0.5 0 -0 0 219 | 10 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 0 0 0.5 0 -0 0 235 | 236 | 237 | 7.25 0.15 1 238 | 239 | 240 | 241 | 245 | 1 1 1 1 246 | 247 | 248 | 0 -3.55 0 0 -0 0 249 | 0 250 | 0 251 | 1 252 | 253 | 254 | 255 | 256 | 257 | 7.25 0.15 1 258 | 259 | 260 | 0 0 0.5 0 -0 0 261 | 10 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 0 0 0.5 0 -0 0 277 | 278 | 279 | 7.25 0.15 1 280 | 281 | 282 | 283 | 287 | 1 1 1 1 288 | 289 | 290 | 3.55 0 0 0 -0 1.5708 291 | 0 292 | 0 293 | 1 294 | 295 | 296 | 297 | 298 | 299 | 6 0.15 1 300 | 301 | 302 | 0 0 0.5 0 -0 0 303 | 10 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 0 0 0.5 0 -0 0 319 | 320 | 321 | 6 0.15 1 322 | 323 | 324 | 325 | 329 | 1 1 1 1 330 | 331 | 332 | -2.24407 -0.501187 0 0 -0 1.5708 333 | 0 334 | 0 335 | 1 336 | 337 | 338 | 339 | 340 | 341 | 6 0.15 1 342 | 343 | 344 | 0 0 0.5 0 -0 0 345 | 10 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 0 0 0.5 0 -0 0 361 | 362 | 363 | 6 0.15 1 364 | 365 | 366 | 367 | 371 | 1 1 1 1 372 | 373 | 374 | -0.881137 0.540777 0 0 0 -1.5708 375 | 0 376 | 0 377 | 1 378 | 379 | 380 | 381 | 382 | 383 | 2 0.15 1 384 | 385 | 386 | 0 0 0.5 0 -0 0 387 | 10 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 0 0 0.5 0 -0 0 403 | 404 | 405 | 2 0.15 1 406 | 407 | 408 | 409 | 413 | 1 1 1 1 414 | 415 | 416 | 0.106122 0.365925 0 0 -0 0 417 | 0 418 | 0 419 | 1 420 | 421 | 1 422 | 423 | 424 | -2.79345 -8.13413 0.5 0 -0 0 425 | 426 | 427 | 1 428 | 429 | 0.145833 430 | 0 431 | 0 432 | 0.145833 433 | 0 434 | 0.125 435 | 436 | 437 | 438 | 439 | 440 | 0.5 441 | 1 442 | 443 | 444 | 10 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 0.5 462 | 1 463 | 464 | 465 | 466 | 470 | 471 | 472 | 0 473 | 0 474 | 1 475 | 476 | 477 | 478 | -3.70137 -6.34108 0.5 0 -0 0 479 | 480 | 481 | 1 482 | 483 | 0.145833 484 | 0 485 | 0 486 | 0.145833 487 | 0 488 | 0.125 489 | 490 | 491 | 492 | 493 | 494 | 0.5 495 | 1 496 | 497 | 498 | 10 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 0.5 516 | 1 517 | 518 | 519 | 520 | 524 | 525 | 526 | 0 527 | 0 528 | 1 529 | 530 | 531 | 532 | -2.34134 -6.70752 0.5 0 -0 0 533 | 534 | 535 | 1 536 | 537 | 0.145833 538 | 0 539 | 0 540 | 0.145833 541 | 0 542 | 0.125 543 | 544 | 545 | 546 | 547 | 548 | 0.5 549 | 1 550 | 551 | 552 | 10 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 0.5 570 | 1 571 | 572 | 573 | 574 | 578 | 579 | 580 | 0 581 | 0 582 | 1 583 | 584 | 585 | 586 | -1.4243 -5.11527 0.5 0 -0 0 587 | 588 | 589 | 1 590 | 591 | 0.145833 592 | 0 593 | 0 594 | 0.145833 595 | 0 596 | 0.125 597 | 598 | 599 | 600 | 601 | 602 | 0.5 603 | 1 604 | 605 | 606 | 10 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 0.5 624 | 1 625 | 626 | 627 | 628 | 632 | 633 | 634 | 0 635 | 0 636 | 1 637 | 638 | 639 | 640 | -1.08972 -7.52534 0.5 0 -0 0 641 | 642 | 643 | 1 644 | 645 | 0.145833 646 | 0 647 | 0 648 | 0.145833 649 | 0 650 | 0.125 651 | 652 | 653 | 654 | 655 | 656 | 0.5 657 | 1 658 | 659 | 660 | 10 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 0.5 678 | 1 679 | 680 | 681 | 682 | 686 | 687 | 688 | 0 689 | 0 690 | 1 691 | 692 | 693 | 694 | 154 754000000 695 | 156 68055943 696 | 1583184130 54672872 697 | 154754 698 | 699 | -3.66 -5.84 0 0 -0 0 700 | 1 1 1 701 | 702 | -3.66 -2.29 0 0 -0 3.14159 703 | 0 0 0 0 -0 0 704 | 0 0 0 0 -0 0 705 | 0 0 0 0 -0 0 706 | 707 | 708 | -7.21 -5.84 0 0 0 -1.5708 709 | 0 0 0 0 -0 0 710 | 0 0 0 0 -0 0 711 | 0 0 0 0 -0 0 712 | 713 | 714 | -1.16763 -3.76302 0 0 -0 0 715 | 0 0 0 0 -0 0 716 | 0 0 0 0 -0 0 717 | 0 0 0 0 -0 0 718 | 719 | 720 | -3.66 -9.39 0 0 -0 0 721 | 0 0 0 0 -0 0 722 | 0 0 0 0 -0 0 723 | 0 0 0 0 -0 0 724 | 725 | 726 | -0.11 -5.84 0 0 -0 1.5708 727 | 0 0 0 0 -0 0 728 | 0 0 0 0 -0 0 729 | 0 0 0 0 -0 0 730 | 731 | 732 | -5.90407 -6.34119 0 0 -0 1.5708 733 | 0 0 0 0 -0 0 734 | 0 0 0 0 -0 0 735 | 0 0 0 0 -0 0 736 | 737 | 738 | -4.54114 -5.29922 0 0 0 -1.5708 739 | 0 0 0 0 -0 0 740 | 0 0 0 0 -0 0 741 | 0 0 0 0 -0 0 742 | 743 | 744 | -3.55388 -5.47408 0 0 -0 0 745 | 0 0 0 0 -0 0 746 | 0 0 0 0 -0 0 747 | 0 0 0 0 -0 0 748 | 749 | 750 | 751 | 0 0 0 0 -0 0 752 | 1 1 1 753 | 754 | 0 0 0 0 -0 0 755 | 0 0 0 0 -0 0 756 | 0 0 0 0 -0 0 757 | 0 0 0 0 -0 0 758 | 759 | 760 | 761 | -2.79345 -8.13413 0.499992 -3e-06 -4e-06 -0 762 | 1 1 1 763 | 764 | -2.79345 -8.13413 0.499992 -3e-06 -4e-06 -0 765 | 0 0 0 0 -0 0 766 | 0 0 -9.8 0 -0 0 767 | 0 0 -9.8 0 -0 0 768 | 769 | 770 | 771 | -3.70137 -6.34108 0.499993 -3e-06 -3e-06 0 772 | 1 1 1 773 | 774 | -3.70137 -6.34108 0.499993 -3e-06 -3e-06 0 775 | 0 0 0 0 -0 0 776 | 0 0 -9.8 0 -0 0 777 | 0 0 -9.8 0 -0 0 778 | 779 | 780 | 781 | -2.34134 -6.70752 0.499997 3e-06 4e-06 -0 782 | 1 1 1 783 | 784 | -2.34134 -6.70752 0.499997 3e-06 4e-06 -0 785 | 0 0 0 0 -0 0 786 | 0 0 -9.8 0 -0 0 787 | 0 0 -9.8 0 -0 0 788 | 789 | 790 | 791 | -1.4243 -5.11527 0.499993 -3e-06 -4e-06 -0 792 | 1 1 1 793 | 794 | -1.4243 -5.11527 0.499993 -3e-06 -4e-06 -0 795 | 0 0 0 0 -0 0 796 | 0 0 -9.8 0 -0 0 797 | 0 0 -9.8 0 -0 0 798 | 799 | 800 | 801 | -1.08972 -7.52534 0.499997 3e-06 4e-06 -0 802 | 1 1 1 803 | 804 | -1.08972 -7.52534 0.499997 3e-06 4e-06 -0 805 | 0 0 0 0 -0 0 806 | 0 0 -9.8 0 -0 0 807 | 0 0 -9.8 0 -0 0 808 | 809 | 810 | 811 | 0 0 10 0 -0 0 812 | 813 | 814 | 815 | 816 | -0.904261 -8.97618 16.475 0 1.3098 2.28404 817 | orbit 818 | perspective 819 | 820 | 821 | 822 | 823 | -------------------------------------------------------------------------------- /src/ros/worlds/simple.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 100.0 15 | 0.01 16 | 0.5 17 | 18 | 19 | quick 20 | 150 21 | 0 22 | 1.400000 23 | 1 24 | 25 | 26 | 0.00001 27 | 0.2 28 | 2000.000000 29 | 0.01000 30 | 31 | 32 | 33 | 34 | 35 | 36 | model://simple_world 37 | 38 | 39 | 40 | 0.4 0.4 0.4 1 41 | 0.7 0.7 0.7 1 42 | false 43 | 44 | 45 | 46 | 47 | 0.8 0.0 12.0 0 1.5708 0 48 | orbit 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /src/ros/worlds/square.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.1 0.1 0.1 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.5 -1 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 10 42 | 43 | 44 | 0 45 | 46 | 47 | 0 0 1 48 | 100 100 49 | 50 | 51 | 52 | 56 | 57 | 58 | 0 59 | 0 60 | 1 61 | 62 | 63 | 0 0 -9.8 64 | 6e-06 2.3e-05 -4.2e-05 65 | 66 | 67 | 0.001 68 | 1 69 | 1000 70 | 71 | 72 | 0.4 0.4 0.4 1 73 | 0.7 0.7 0.7 1 74 | False 75 | 76 | 77 | EARTH_WGS84 78 | 0 79 | 0 80 | 0 81 | 0 82 | 83 | 84 | 1.455 -6.7295 0 0 -0 0 85 | 86 | 87 | 88 | 89 | 12 0.15 0.5 90 | 91 | 92 | 0 0 0.5 0 -0 0 93 | 10 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 0 0 0.5 0 -0 0 109 | 110 | 111 | 12 0.15 0.5 112 | 113 | 114 | 115 | 119 | 0.435294 0.796078 0.67451 1 120 | 121 | 122 | 0 5.9395 0 0 -0 0 123 | 0 124 | 0 125 | 1 126 | 127 | 128 | 129 | 130 | 131 | 12.029 0.15 0.5 132 | 133 | 134 | 0 0 0.5 0 -0 0 135 | 10 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 0 0 0.5 0 -0 0 151 | 152 | 153 | 12.029 0.15 0.5 154 | 155 | 156 | 157 | 161 | 1 0.764706 0.305882 1 162 | 163 | 164 | 5.925 0 0 0 0 -1.5708 165 | 0 166 | 0 167 | 1 168 | 169 | 170 | 171 | 172 | 173 | 12 0.15 0.5 174 | 175 | 176 | 0 0 0.5 0 -0 0 177 | 10 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 0 0 0.5 0 -0 0 193 | 194 | 195 | 12 0.15 0.5 196 | 197 | 198 | 199 | 203 | 0.666667 0.666667 1 1 204 | 205 | 206 | 0 -5.9395 0 0 -0 3.14159 207 | 0 208 | 0 209 | 1 210 | 211 | 212 | 213 | 214 | 215 | 12.029 0.15 0.5 216 | 217 | 218 | 0 0 0.5 0 -0 0 219 | 10 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 0 0 0.5 0 -0 0 235 | 236 | 237 | 12.029 0.15 0.5 238 | 239 | 240 | 241 | 245 | 0.996078 0.47451 0.0196078 1 246 | 247 | 248 | -5.925 0 0 0 -0 1.5708 249 | 0 250 | 0 251 | 1 252 | 253 | 1 254 | 255 | 256 | -1.65396 -4.171 0.5 0 -0 0 257 | 258 | 259 | 1 260 | 261 | 0.145833 262 | 0 263 | 0 264 | 0.145833 265 | 0 266 | 0.125 267 | 268 | 269 | 270 | 271 | 272 | 0.500001 273 | 1 274 | 275 | 276 | 10 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 0.500001 294 | 1 295 | 296 | 297 | 298 | 1 1 1 1 299 | 303 | 304 | 305 | 0 306 | 0 307 | 1 308 | 309 | 310 | 311 | -0.222744 -3.66959 0.5 0 0 -1e-06 312 | 313 | 314 | 1 315 | 316 | 0.145833 317 | 0 318 | 0 319 | 0.145833 320 | 0 321 | 0.125 322 | 323 | 324 | 325 | 326 | 327 | 0.116277 328 | 1 329 | 330 | 331 | 10 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 0.116277 349 | 1 350 | 351 | 352 | 353 | 1 1 1 1 354 | 358 | 359 | 360 | 0 361 | 0 362 | 1 363 | 364 | 365 | 366 | 0.954942 -7.66588 0.5 0 0 -1e-06 367 | 368 | 369 | 1 370 | 371 | 0.145833 372 | 0 373 | 0 374 | 0.145833 375 | 0 376 | 0.125 377 | 378 | 379 | 380 | 381 | 382 | 0.116277 383 | 1 384 | 385 | 386 | 10 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 0.116277 404 | 1 405 | 406 | 407 | 408 | 1 1 1 1 409 | 413 | 414 | 415 | 0 416 | 0 417 | 1 418 | 419 | 420 | 421 | 3.69901 -6.92983 0.5 0 0 -1e-06 422 | 423 | 424 | 1 425 | 426 | 0.145833 427 | 0 428 | 0 429 | 0.145833 430 | 0 431 | 0.125 432 | 433 | 434 | 435 | 436 | 437 | 0.116277 438 | 1 439 | 440 | 441 | 10 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 0.116277 459 | 1 460 | 461 | 462 | 463 | 1 1 1 1 464 | 468 | 469 | 470 | 0 471 | 0 472 | 1 473 | 474 | 475 | 476 | 2.82562 -8.10985 0.5 0 0 -1e-06 477 | 478 | 479 | 1 480 | 481 | 0.145833 482 | 0 483 | 0 484 | 0.145833 485 | 0 486 | 0.125 487 | 488 | 489 | 490 | 491 | 492 | 0.116277 493 | 1 494 | 495 | 496 | 10 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 0.116277 514 | 1 515 | 516 | 517 | 518 | 1 1 1 1 519 | 523 | 524 | 525 | 0 526 | 0 527 | 1 528 | 529 | 530 | 531 | -1.2329 -4.26528 0.5 0 0 -1e-06 532 | 533 | 534 | 1 535 | 536 | 0.145833 537 | 0 538 | 0 539 | 0.145833 540 | 0 541 | 0.125 542 | 543 | 544 | 545 | 546 | 547 | 0.116277 548 | 1 549 | 550 | 551 | 10 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 0.116277 569 | 1 570 | 571 | 572 | 573 | 1 1 1 1 574 | 578 | 579 | 580 | 0 581 | 0 582 | 1 583 | 584 | 585 | 586 | 1.14017 -7.37583 0.5 0 0 -0.000109 587 | 588 | 589 | 1 590 | 591 | 0.145833 592 | 0 593 | 0 594 | 0.145833 595 | 0 596 | 0.125 597 | 598 | 599 | 600 | 601 | 602 | 0.0605414 603 | 1 604 | 605 | 606 | 10 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 0.0605414 624 | 1 625 | 626 | 627 | 628 | 1 1 1 1 629 | 633 | 634 | 635 | 0 636 | 0 637 | 1 638 | 639 | 640 | 641 | 2.40392 -6.77398 0.5 0 0 -0.000118 642 | 643 | 644 | 1 645 | 646 | 0.145833 647 | 0 648 | 0 649 | 0.145833 650 | 0 651 | 0.125 652 | 653 | 654 | 655 | 656 | 657 | 0.0605414 658 | 1 659 | 660 | 661 | 10 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 678 | 0.0605414 679 | 1 680 | 681 | 682 | 683 | 1 1 1 1 684 | 688 | 689 | 690 | 0 691 | 0 692 | 1 693 | 694 | 695 | 696 | 1146 616000000 697 | 1162 623512068 698 | 1583426455 803427008 699 | 1146616 700 | 701 | 1.455 -6.7295 0 0 -0 0 702 | 1 1 1 703 | 704 | 1.455 -0.79 0 0 -0 0 705 | 0 0 0 0 -0 0 706 | 0 0 0 0 -0 0 707 | 0 0 0 0 -0 0 708 | 709 | 710 | 7.38 -6.7295 0 0 0 -1.5708 711 | 0 0 0 0 -0 0 712 | 0 0 0 0 -0 0 713 | 0 0 0 0 -0 0 714 | 715 | 716 | 1.455 -12.669 0 0 -0 3.14159 717 | 0 0 0 0 -0 0 718 | 0 0 0 0 -0 0 719 | 0 0 0 0 -0 0 720 | 721 | 722 | -4.47 -6.7295 0 0 -0 1.5708 723 | 0 0 0 0 -0 0 724 | 0 0 0 0 -0 0 725 | 0 0 0 0 -0 0 726 | 727 | 728 | 729 | 0 0 0 0 -0 0 730 | 1 1 1 731 | 732 | 0 0 0 0 -0 0 733 | 0 0 0 0 -0 0 734 | 0 0 0 0 -0 0 735 | 0 0 0 0 -0 0 736 | 737 | 738 | 739 | -1.65396 -4.171 0.499995 0 0 -0.000314 740 | 0.121082 0.121082 1 741 | 742 | -1.65396 -4.171 0.499995 0 0 -0.000314 743 | 0 0 0 0 -0 0 744 | 0 0 -9.8 0 -0 0 745 | 0 0 -9.8 0 -0 0 746 | 747 | 748 | 749 | -0.222743 -3.66959 0.49999 0 0 -1e-05 750 | 1 1 1 751 | 752 | -0.222743 -3.66959 0.49999 0 0 -1e-05 753 | 0 0 0 0 -0 0 754 | 0 0 -9.8 0 -0 0 755 | 0 0 -9.8 0 -0 0 756 | 757 | 758 | 759 | 0.954943 -7.66588 0.499991 0 0 -1e-05 760 | 1 1 1 761 | 762 | 0.954943 -7.66588 0.499991 0 0 -1e-05 763 | 0 0 0 0 -0 0 764 | 0 0 -9.8 0 -0 0 765 | 0 0 -9.8 0 -0 0 766 | 767 | 768 | 769 | 3.69901 -6.92983 0.499991 0 0 -9e-06 770 | 1 1 1 771 | 772 | 3.69901 -6.92983 0.499991 0 0 -9e-06 773 | 0 0 0 0 -0 0 774 | 0 0 -9.8 0 -0 0 775 | 0 0 -9.8 0 -0 0 776 | 777 | 778 | 779 | 2.82562 -8.10985 0.499991 0 0 -8e-06 780 | 1 1 1 781 | 782 | 2.82562 -8.10985 0.499991 0 0 -8e-06 783 | 0 0 0 0 -0 0 784 | 0 0 -9.8 0 -0 0 785 | 0 0 -9.8 0 -0 0 786 | 787 | 788 | 789 | -1.2329 -4.26528 0.499991 0 0 -7e-06 790 | 1 1 1 791 | 792 | -1.2329 -4.26528 0.499991 0 0 -7e-06 793 | 0 0 0 0 -0 0 794 | 0 0 -9.8 0 -0 0 795 | 0 0 -9.8 0 -0 0 796 | 797 | 798 | 799 | 1.14017 -7.37583 0.499997 0 0 -0.000114 800 | 1 1 1 801 | 802 | 1.14017 -7.37583 0.499997 0 0 -0.000114 803 | 0 0 0 0 -0 0 804 | 0 0 -9.8 0 -0 0 805 | 0 0 -9.8 0 -0 0 806 | 807 | 808 | 809 | 2.40392 -6.77398 0.499991 0 0 -9e-05 810 | 1 1 1 811 | 812 | 2.40392 -6.77398 0.499991 0 0 -9e-05 813 | 0 0 0 0 -0 0 814 | 0 0 -9.8 0 -0 0 815 | 0 0 -9.8 0 -0 0 816 | 817 | 818 | 819 | 0 0 10 0 -0 0 820 | 821 | 822 | 823 | 824 | 1.77811 -11.7189 22.9686 0 1.4058 0.939075 825 | orbit 826 | perspective 827 | 828 | 829 | 830 | 831 | --------------------------------------------------------------------------------