def update_agents(agents):
for i in xrange(len(X)):
agents[i].pref_velocity = (numpy.array(goal[i]) - numpy.array(X[i]))
if numpy.linalg.norm(agents[i].pref_velocity) > V_max[i]:
agents[i].pref_velocity = normalized(
agents[i].pref_velocity) * V_max[i]
return agents
from pyorca import Agent, get_avoidance_velocity, orca, normalized, perp
from numpy import array, rint, linspace, pi, cos, sin
import pygame
import itertools
import random
N_AGENTS = 10
RADIUS = 8.
SPEED = 10
agents = []
for i in range(N_AGENTS):
theta = 2 * pi * i / N_AGENTS
x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1)
vel = normalized(-x) * SPEED
pos = (random.uniform(-20, 20), random.uniform(-20, 20))
print(type(pos))
print(type((0., 0.)))
print(type(1.))
print(type(SPEED))
print(type(vel))
print(normalized(-x))
agents.append(Agent(pos, (0., 0.), 1., SPEED, vel))
colors = [
(255, 0, 0),
(0, 255, 0),
(0, 0, 255),
(255, 255, 0),
(0, 255, 255),
agents = []
X = [(-15, 0), (15, 0.5), (0.00, 15), (-0.5, -15)]
V = [(0.0, 0.0) for _ in xrange(len(X))]
V_max = [5.0 for _ in xrange(len(X))]
goal = [(15.0, 0.0), (-15.0, 0.0), (0.0, -15.0), (0.0, 15.0)]
for i in range(N_AGENTS):
#theta = 2 * pi * i / N_AGENTS
#x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1)
#vel = normalized(-x) * SPEED
#pos = (random.uniform(-20, 20), random.uniform(-20, 20))
pos = X[i]
vel = (array(goal[i]) - array(X[i]))
if norm(vel) > V_max[i]:
vel = normalized(vel) * V_max[i]
agents.append(Agent(pos, (0., 0.), RADIUS, V_max[i], vel))
colors = [
(255, 0, 0),
(0, 255, 0),
(0, 0, 255),
(255, 255, 0),
(0, 255, 255),
(255, 0, 255),
]
pygame.init()
dim = (640, 480)
screen = pygame.display.set_mode(dim)
N_AGENTS = 4
RADIUS = 0.7
tau = 5
Ts = 0.1
X = [(-5, 0.25), (5, 0), (0.00, 5), (-0.25, -5)]
V = [(0, 0) for _ in xrange(len(X))]
V_max = [1.0 for _ in xrange(len(X))]
goal = [(5.0, 0.0), (-5.0, 0.0), (0.0, -5.0), (0.0, 5.0)]
agents = []
for i in xrange(len(X)):
vel = (numpy.array(goal[i]) - numpy.array(X[i]))
if numpy.linalg.norm(vel) > V_max[i]:
vel = normalized(vel) * V_max[i]
agents.append(Agent(X[i], (0., 0.), RADIUS, V_max[i], vel))
def update_agents(agents):
for i in xrange(len(X)):
agents[i].pref_velocity = (numpy.array(goal[i]) - numpy.array(X[i]))
if numpy.linalg.norm(agents[i].pref_velocity) > V_max[i]:
agents[i].pref_velocity = normalized(
agents[i].pref_velocity) * V_max[i]
return agents
def callback_0(msg):
X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y))
from pyorca import Agent, get_avoidance_velocity, orca, normalized, perp
from numpy import array, rint, linspace, pi, cos, sin
import pygame
import itertools
import random
N_AGENTS = 8
RADIUS = 8.
SPEED = 10
agents = []
for i in range(N_AGENTS):
theta = 2 * pi * i / N_AGENTS
x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1)
vel = normalized(-x) * SPEED
pos = (random.uniform(-20, 20), random.uniform(-20, 20))
agents.append(Agent(pos, (0., 0.), 1., SPEED, vel))
colors = [
(255, 0, 0),
(0, 255, 0),
(0, 0, 255),
(255, 255, 0),
(0, 255, 255),
(255, 0, 255),
]
pygame.init()
from pyorca import Agent, get_avoidance_velocity, orca, normalized, perp
from numpy import array, rint, linspace, pi, cos, sin
import pygame
import itertools
import random
N_AGENTS = 8
RADIUS = 8.
SPEED = 10
agents = []
for i in range(N_AGENTS):
theta = 2 * pi * i / N_AGENTS
x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1)
vel = normalized(-x) * SPEED
pos = (random.uniform(-20, 20), random.uniform(-20, 20))
agents.append(Agent(pos, (0., 0.), 1., SPEED, vel))
colors = [
(255, 0, 0),
(0, 255, 0),
(0, 0, 255),
(255, 255, 0),
(0, 255, 255),
(255, 0, 255),
]
pygame.init()
dim = (640, 480)
