├── README.md ├── halfplaneintersect.py ├── pyorca.py └── test.py /README.md: -------------------------------------------------------------------------------- 1 | # pyorca 2 | 3 | pyorca is a Python implementation of the [ORCA](http://gamma.cs.unc.edu/ORCA/) 4 | local collision avoidance algorithm, made from scratch. 5 | 6 | pyorca.py and halfplaneintersect.py contain copious amounts of comments and 7 | notes regarding the implementations of their respective algorithms. 8 | 9 | test.py contains a simple pygame-powered demo I used to test my implementation. 10 | 11 | 12 | ## Todo 13 | 14 | * Implement the 3D LP fallback described in the paper. 15 | 16 | ## License 17 | 18 | Copyright (c) 2013 Mak Nazecic-Andrlon 19 | 20 | Permission is hereby granted, free of charge, to any person obtaining a copy 21 | of this software and associated documentation files (the "Software"), to deal 22 | in the Software without restriction, including without limitation the rights 23 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 24 | copies of the Software, and to permit persons to whom the Software is 25 | furnished to do so, subject to the following conditions: 26 | 27 | The above copyright notice and this permission notice shall be included in all 28 | copies or substantial portions of the Software. 29 | 30 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 31 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 32 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 33 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 34 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 35 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 36 | SOFTWARE. 37 | -------------------------------------------------------------------------------- /halfplaneintersect.py: -------------------------------------------------------------------------------- 1 | # The MIT License (MIT) 2 | # 3 | # Copyright (c) 2013 Mak Nazecic-Andrlon 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to 7 | # deal in the Software without restriction, including without limitation the 8 | # rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | # sell copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | # IN THE SOFTWARE. 22 | 23 | """This module solves 2D linear programming using half-plane intersection.""" 24 | 25 | from __future__ import division 26 | 27 | import itertools 28 | 29 | from numpy import dot, clip, array, sqrt 30 | from numpy.linalg import det 31 | 32 | 33 | class InfeasibleError(RuntimeError): 34 | """Raised if an LP problem has no solution.""" 35 | pass 36 | 37 | 38 | class Line(object): 39 | """A line in space.""" 40 | def __init__(self, point, direction): 41 | super(Line, self).__init__() 42 | self.point = array(point) 43 | self.direction = normalized(array(direction)) 44 | 45 | def __repr__(self): 46 | return "Line(%s, %s)" % (self.point, self.direction) 47 | 48 | 49 | def halfplane_optimize(lines, optimal_point): 50 | """Find the point closest to optimal_point in the intersection of the 51 | closed half-planes defined by lines which are in Hessian normal form 52 | (point-normal form).""" 53 | # We implement the quadratic time (though linear expected given randomly 54 | # permuted input) incremental half-plane intersection algorithm as laid 55 | # out in http://www.mpi-inf.mpg.de/~kavitha/lecture3.ps 56 | point = optimal_point 57 | for i, line in enumerate(lines): 58 | # If this half-plane already contains the current point, all is well. 59 | if dot(point - line.point, line.direction) >= 0: 60 | # assert False, point 61 | continue 62 | 63 | # Otherwise, the new optimum must lie on the newly added line. Compute 64 | # the feasible interval of the intersection of all the lines added so 65 | # far with the current one. 66 | prev_lines = itertools.islice(lines, i) 67 | left_dist, right_dist = line_halfplane_intersect(line, prev_lines) 68 | 69 | # Now project the optimal point onto the line segment defined by the 70 | # the above bounds. This gives us our new best point. 71 | point = point_line_project(line, optimal_point, left_dist, right_dist) 72 | return point 73 | 74 | def point_line_project(line, point, left_bound, right_bound): 75 | """Project point onto the line segment defined by line, which is in 76 | point-normal form, and the left and right bounds with respect to line's 77 | anchor point.""" 78 | # print("left_bound=%s, right_bound=%s" % (left_bound, right_bound)) 79 | new_dir = perp(line.direction) 80 | # print("new_dir=%s" % new_dir) 81 | proj_len = dot(point - line.point, new_dir) 82 | # print("proj_len=%s" % proj_len) 83 | clamped_len = clip(proj_len, left_bound, right_bound) 84 | # print("clamped_len=%s" % clamped_len) 85 | return line.point + new_dir * clamped_len 86 | 87 | def line_halfplane_intersect(line, other_lines): 88 | """Compute the signed offsets of the interval on the edge of the 89 | half-plane defined by line that is included in the half-planes defined by 90 | other_lines. 91 | 92 | The offsets are relative to line's anchor point, in units of line's 93 | direction. 94 | 95 | """ 96 | # We use the line intersection algorithm presented in 97 | # http://stackoverflow.com/a/565282/126977 to determine the intersection 98 | # point. "Left" is the negative of the canonical direction of the line. 99 | # "Right" is positive. 100 | left_dist = float("-inf") 101 | right_dist = float("inf") 102 | for prev_line in other_lines: 103 | num1 = dot(prev_line.direction, line.point - prev_line.point) 104 | den1 = det((line.direction, prev_line.direction)) 105 | # num2 = det((perp(prev_line.direction), line.point - prev_line.point)) 106 | # den2 = det((perp(line.direction), perp(prev_line.direction))) 107 | 108 | # assert abs(den1 - den2) < 1e-6, (den1, den2) 109 | # assert abs(num1 - num2) < 1e-6, (num1, num2) 110 | 111 | num = num1 112 | den = den1 113 | 114 | # Check for zero denominator, since ZeroDivisionError (or rather 115 | # FloatingPointError) won't necessarily be raised if using numpy. 116 | if den == 0: 117 | # The half-planes are parallel. 118 | if num < 0: 119 | # The intersection of the half-planes is empty; there is no 120 | # solution. 121 | raise InfeasibleError 122 | else: 123 | # The *half-planes* intersect, but their lines don't cross, so 124 | # ignore. 125 | continue 126 | 127 | # Signed offset of the point of intersection, relative to the line's 128 | # anchor point, in units of the line's direction. 129 | offset = num / den 130 | if den > 0: 131 | # Point of intersection is to the right. 132 | right_dist = min((right_dist, offset)) 133 | else: 134 | # Point of intersection is to the left. 135 | left_dist = max((left_dist, offset)) 136 | 137 | if left_dist > right_dist: 138 | # The interval is inconsistent, so the feasible region is empty. 139 | raise InfeasibleError 140 | return left_dist, right_dist 141 | 142 | def perp(a): 143 | return array((a[1], -a[0])) 144 | 145 | def norm_sq(x): 146 | return dot(x, x) 147 | 148 | def norm(x): 149 | return sqrt(norm_sq(x)) 150 | 151 | def normalized(x): 152 | l = norm_sq(x) 153 | assert l > 0, (x, l) 154 | return x / sqrt(l) 155 | 156 | if __name__ == '__main__': 157 | lines = [ 158 | Line((-2, 0), (-1, 1)), 159 | Line((0, -1), (1, 0)) 160 | ] 161 | point = array((1, 0)) 162 | result = halfplane_optimize(lines, point) 163 | print(result, norm(result)) 164 | 165 | # a = point_line_project(lines[0], point, -20, 20) 166 | # print((a - lines[0].point)/(-perp(lines[0].direction))) 167 | # print(a) 168 | 169 | a = point_line_project(lines[1], point, -10000, -3) 170 | # print((a - lines[1].point)/(-perp(lines[1].direction))) 171 | print(a) 172 | -------------------------------------------------------------------------------- /pyorca.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013 Mak Nazecic-Andrlon 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in all 11 | # copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | """Implementation of the 2D ORCA algorithm as described by J. van der Berg, 22 | S. J. Guy, M. Lin and D. Manocha in 'Reciprocal n-body Collision Avoidance'.""" 23 | 24 | from __future__ import division 25 | 26 | import numpy 27 | from numpy import array, sqrt, copysign, dot 28 | from numpy.linalg import det 29 | 30 | from halfplaneintersect import halfplane_optimize, Line, perp 31 | 32 | # Method: 33 | # For each robot A and potentially colliding robot B, compute smallest change 34 | # in relative velocity 'u' that avoids collision. Find normal 'n' to VO at that 35 | # point. 36 | # For each such velocity 'u' and normal 'n', find half-plane as defined in (6). 37 | # Intersect half-planes and pick velocity closest to A's preferred velocity. 38 | 39 | class Agent(object): 40 | """A disk-shaped agent.""" 41 | def __init__(self, position, velocity, radius, max_speed, pref_velocity): 42 | super(Agent, self).__init__() 43 | self.position = array(position) 44 | self.velocity = array(velocity) 45 | self.radius = radius 46 | self.max_speed = max_speed 47 | self.pref_velocity = array(pref_velocity) 48 | 49 | 50 | def orca(agent, colliding_agents, t, dt): 51 | """Compute ORCA solution for agent. NOTE: velocity must be _instantly_ 52 | changed on tick *edge*, like first-order integration, otherwise the method 53 | undercompensates and you will still risk colliding.""" 54 | lines = [] 55 | for collider in colliding_agents: 56 | dv, n = get_avoidance_velocity(agent, collider, t, dt) 57 | line = Line(agent.velocity + dv / 2, n) 58 | lines.append(line) 59 | return halfplane_optimize(lines, agent.pref_velocity), lines 60 | 61 | def get_avoidance_velocity(agent, collider, t, dt): 62 | """Get the smallest relative change in velocity between agent and collider 63 | that will get them onto the boundary of each other's velocity obstacle 64 | (VO), and thus avert collision.""" 65 | 66 | # This is a summary of the explanation from the AVO paper. 67 | # 68 | # The set of all relative velocities that will cause a collision within 69 | # time tau is called the velocity obstacle (VO). If the relative velocity 70 | # is outside of the VO, no collision will happen for at least tau time. 71 | # 72 | # The VO for two moving disks is a circularly truncated triangle 73 | # (spherically truncated cone in 3D), with an imaginary apex at the 74 | # origin. It can be described by a union of disks: 75 | # 76 | # Define an open disk centered at p with radius r: 77 | # D(p, r) := {q | ||q - p|| < r} (1) 78 | # 79 | # Two disks will collide at time t iff ||x + vt|| < r, where x is the 80 | # displacement, v is the relative velocity, and r is the sum of their 81 | # radii. 82 | # 83 | # Divide by t: ||x/t + v|| < r/t, 84 | # Rearrange: ||v - (-x/t)|| < r/t. 85 | # 86 | # By (1), this is a disk D(-x/t, r/t), and it is the set of all velocities 87 | # that will cause a collision at time t. 88 | # 89 | # We can now define the VO for time tau as the union of all such disks 90 | # D(-x/t, r/t) for 0 < t <= tau. 91 | # 92 | # Note that the displacement and radius scale _inversely_ proportionally 93 | # to t, generating a line of disks of increasing radius starting at -x/t. 94 | # This is what gives the VO its cone shape. The _closest_ velocity disk is 95 | # at D(-x/tau, r/tau), and this truncates the VO. 96 | 97 | x = -(agent.position - collider.position) 98 | v = agent.velocity - collider.velocity 99 | r = agent.radius + collider.radius 100 | 101 | x_len_sq = norm_sq(x) 102 | 103 | if x_len_sq >= r * r: 104 | # We need to decide whether to project onto the disk truncating the VO 105 | # or onto the sides. 106 | # 107 | # The center of the truncating disk doesn't mark the line between 108 | # projecting onto the sides or the disk, since the sides are not 109 | # parallel to the displacement. We need to bring it a bit closer. How 110 | # much closer can be worked out by similar triangles. It works out 111 | # that the new point is at x/t cos(theta)^2, where theta is the angle 112 | # of the aperture (so sin^2(theta) = (r/||x||)^2). 113 | adjusted_center = x/t * (1 - (r*r)/x_len_sq) 114 | 115 | if dot(v - adjusted_center, adjusted_center) < 0: 116 | # v lies in the front part of the cone 117 | # print("front") 118 | # print("front", adjusted_center, x_len_sq, r, x, t) 119 | w = v - x/t 120 | u = normalized(w) * r/t - w 121 | n = normalized(w) 122 | else: # v lies in the rest of the cone 123 | # print("sides") 124 | # Rotate x in the direction of v, to make it a side of the cone. 125 | # Then project v onto that, and calculate the difference. 126 | leg_len = sqrt(x_len_sq - r*r) 127 | # The sign of the sine determines which side to project on. 128 | sine = copysign(r, det((v, x))) 129 | rot = array( 130 | ((leg_len, sine), 131 | (-sine, leg_len))) 132 | rotated_x = rot.dot(x) / x_len_sq 133 | n = perp(rotated_x) 134 | if sine < 0: 135 | # Need to flip the direction of the line to make the 136 | # half-plane point out of the cone. 137 | n = -n 138 | # print("rotated_x=%s" % rotated_x) 139 | u = rotated_x * dot(v, rotated_x) - v 140 | # print("u=%s" % u) 141 | else: 142 | # We're already intersecting. Pick the closest velocity to our 143 | # velocity that will get us out of the collision within the next 144 | # timestep. 145 | # print("intersecting") 146 | w = v - x/dt 147 | u = normalized(w) * r/dt - w 148 | n = normalized(w) 149 | return u, n 150 | 151 | def norm_sq(x): 152 | return dot(x, x) 153 | 154 | def normalized(x): 155 | l = norm_sq(x) 156 | assert l > 0, (x, l) 157 | return x / sqrt(l) 158 | 159 | def dist_sq(a, b): 160 | return norm_sq(b - a) 161 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013 Mak Nazecic-Andrlon 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in all 11 | # copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | 22 | from __future__ import division 23 | 24 | from pyorca import Agent, get_avoidance_velocity, orca, normalized, perp 25 | from numpy import array, rint, linspace, pi, cos, sin 26 | import pygame 27 | 28 | import itertools 29 | import random 30 | 31 | N_AGENTS = 8 32 | RADIUS = 8. 33 | SPEED = 10 34 | 35 | agents = [] 36 | for i in range(N_AGENTS): 37 | theta = 2 * pi * i / N_AGENTS 38 | x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1) 39 | vel = normalized(-x) * SPEED 40 | pos = (random.uniform(-20, 20), random.uniform(-20, 20)) 41 | agents.append(Agent(pos, (0., 0.), 1., SPEED, vel)) 42 | 43 | 44 | colors = [ 45 | (255, 0, 0), 46 | (0, 255, 0), 47 | (0, 0, 255), 48 | (255, 255, 0), 49 | (0, 255, 255), 50 | (255, 0, 255), 51 | ] 52 | 53 | pygame.init() 54 | 55 | dim = (640, 480) 56 | screen = pygame.display.set_mode(dim) 57 | 58 | O = array(dim) / 2 # Screen position of origin. 59 | scale = 6 # Drawing scale. 60 | 61 | clock = pygame.time.Clock() 62 | FPS = 20 63 | dt = 1/FPS 64 | tau = 5 65 | 66 | def draw_agent(agent, color): 67 | pygame.draw.circle(screen, color, rint(agent.position * scale + O).astype(int), int(round(agent.radius * scale)), 0) 68 | 69 | def draw_orca_circles(a, b): 70 | for x in linspace(0, tau, 21): 71 | if x == 0: 72 | continue 73 | pygame.draw.circle(screen, pygame.Color(0, 0, 255), rint((-(a.position - b.position) / x + a.position) * scale + O).astype(int), int(round((a.radius + b.radius) * scale / x)), 1) 74 | 75 | def draw_velocity(a): 76 | pygame.draw.line(screen, pygame.Color(0, 255, 255), rint(a.position * scale + O).astype(int), rint((a.position + a.velocity) * scale + O).astype(int), 1) 77 | # pygame.draw.line(screen, pygame.Color(255, 0, 255), rint(a.position * scale + O).astype(int), rint((a.position + a.pref_velocity) * scale + O).astype(int), 1) 78 | 79 | running = True 80 | accum = 0 81 | all_lines = [[]] * len(agents) 82 | while running: 83 | accum += clock.tick(FPS) 84 | 85 | while accum >= dt * 1000: 86 | accum -= dt * 1000 87 | 88 | new_vels = [None] * len(agents) 89 | for i, agent in enumerate(agents): 90 | candidates = agents[:i] + agents[i + 1:] 91 | # print(candidates) 92 | new_vels[i], all_lines[i] = orca(agent, candidates, tau, dt) 93 | # print(i, agent.velocity) 94 | 95 | for i, agent in enumerate(agents): 96 | agent.velocity = new_vels[i] 97 | agent.position += agent.velocity * dt 98 | 99 | screen.fill(pygame.Color(0, 0, 0)) 100 | 101 | for agent in agents[1:]: 102 | draw_orca_circles(agents[0], agent) 103 | 104 | for agent, color in zip(agents, itertools.cycle(colors)): 105 | draw_agent(agent, color) 106 | draw_velocity(agent) 107 | # print(sqrt(norm_sq(agent.velocity))) 108 | 109 | for line in all_lines[0]: 110 | # Draw ORCA line 111 | alpha = agents[0].position + line.point + perp(line.direction) * 100 112 | beta = agents[0].position + line.point + perp(line.direction) * -100 113 | pygame.draw.line(screen, (255, 255, 255), rint(alpha * scale + O).astype(int), rint(beta * scale + O).astype(int), 1) 114 | 115 | # Draw normal to ORCA line 116 | gamma = agents[0].position + line.point 117 | delta = agents[0].position + line.point + line.direction 118 | pygame.draw.line(screen, (255, 255, 255), rint(gamma * scale + O).astype(int), rint(delta * scale + O).astype(int), 1) 119 | 120 | pygame.display.flip() 121 | 122 | for event in pygame.event.get(): 123 | if event.type == pygame.QUIT: 124 | running = False 125 | pygame.quit() 126 | --------------------------------------------------------------------------------