def testNormalize(self):
self.assertEqual(normalize(Vector(2.0, 2.0)),
Vector(0.5 * 2**0.5, 0.5 * 2**0.5))
self.assertEqual(normalize(Vector(0.01, 0.0)),
Vector(1.0, 0.0))
with self.assertRaises(ZeroDivisionError):
normalize(Vector(0.0, 0.0))
def update_vel(self, delta_time, desired_vel):
dv = desired_vel - self.vel
if length(dv) < self.max_accel * delta_time:
self.vel = desired_vel
else:
self.vel += normalize(dv) * self.max_accel * delta_time
if length(self.vel) > self.max_vel:
self.vel = self.max_vel * normalize(self.vel)
def gradient(distance_potential, dist, direction, thickness):
# use sympy, perhaps?
coeff = distance_potential.derivative(dist - thickness)
try:
return normalize(direction) * coeff
except ZeroDivisionError:
return Vector(0.0, 0.0)
def testLength(self):
self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)
v = normalize(Vector(0.23, 1.45))
self.assertAlmostEqual(length(v * 23.145), 23.145)
self.assertAlmostEqual(length(v / 23.145), 1.0/23.145)
def gradient(distance_potential, dist, direction, thickness):
# use sympy, perhaps?
coeff = distance_potential.derivative(dist - thickness)
try:
return normalize(direction) * coeff
except ZeroDivisionError:
return Vector(0.0, 0.0)
def calc_desired_velocity(self, bots, obstacles, targets):
vel = self.vel
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos),
self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(k=-2.0),
dist(target, self.pos),
target - self.pos,
0)
vel += _FORCE_SENSITIVITY * force
for obstacle in obstacles:
if obstacle.distance(self.pos) <= self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.0),
obstacle.distance(self.pos),
obstacle.repulsion_dir(self.pos),
OBSTACLE_CLEARANCE + self.radius)
vel += _FORCE_SENSITIVITY * force
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
return vel
def testLength(self):
self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)
v = normalize(Vector(0.23, 1.45))
self.assertAlmostEqual(length(v * 23.145), 23.145)
self.assertAlmostEqual(length(v / 23.145), 1.0 / 23.145)
def numerical_gradient(distance_potential, dist_fun, pos, direction, thickness):
eps = 1e-4
here = distance_potential(dist_fun(pos) - thickness)
there = distance_potential(dist_fun(pos + eps * direction) - thickness)
coeff = (there - here) / eps
try:
return normalize(direction) * coeff
except ZeroDivisionError:
return Vector(0.0, 0.0)
def numerical_gradient(distance_potential, dist_fun, pos, direction,
thickness):
eps = 1e-4
here = distance_potential(dist_fun(pos) - thickness)
there = distance_potential(dist_fun(pos + eps * direction) - thickness)
coeff = (there - here) / eps
try:
return normalize(direction) * coeff
except ZeroDivisionError:
return Vector(0.0, 0.0)
def __init__(self, pos, dir, vel = 0.0,
max_vel = BOT_VEL_CAP,
max_accel = BOT_ACCEL_CAP,
radius = BOT_RADIUS):
self.pos = Point(*pos)
self.dir = normalize(dir)
self.lvel = vel
self.rvel = vel
self.max_vel = max_vel
self.max_accel = max_accel
self.radius = radius
self.width = 2.0 * radius
def testNormalize(self):
self.assertEqual(normalize(Vector(2.0, 2.0)),
Vector(0.5 * 2**0.5, 0.5 * 2**0.5))
self.assertEqual(normalize(Vector(0.01, 0.0)),
Vector(1.0, 0.0))
with self.assertRaises(NormalizationError):
normalize(Vector(0.0, 0.0))
normalize(Vector(1.0 - 7.0 * (1.0/7.0), 1.0 - 3.0 * (1.0/3.0)))
def __init__(self, pos, dir, vel = 0.0,
max_vel = config.BOT_VEL_CAP,
max_accel = config.BOT_ACCEL_CAP,
radius = config.BOT_RADIUS):
self.pos = Point(*pos)
self.dir = normalize(dir)
self.lvel = vel
self.rvel = vel
self.max_vel = max_vel
self.max_accel = max_accel
self.radius = radius
self.width = 2.0 * radius
self.max_rot_vel = 2 * max_vel / self.width
self.shape = Circle(self.pos, self.radius)
def extend_trajectory(self, known, last_x, last_y, last_t):
# now adding a circle to the end of known trajectory
# k is signed curvature of the trajectry at t_fn
try:
k = known.curvature(last_t)
except (ValueError, ZeroDivisionError):
k = config.MIN_CIRCLE_CURVATURE
if abs(k) < config.MIN_CIRCLE_CURVATURE:
k = copysign(config.MIN_CIRCLE_CURVATURE, k)
if abs(k) > config.MAX_CURVATURE:
k = copysign(config.MAX_CURVATURE, k)
radius = abs(1.0/k)
# trajectory direction at time t_fn
try:
d = normalize(Vector(known.dx(last_t), known.dy(last_t)))
except NormalizationError:
d = self.real_dir
r = Vector(-d.y, d.x) / k
center = Point(last_x, last_y) + r
phase = atan2(-r.y, -r.x)
#freq = known.dx(last_t) / r.y
freq = k
self.x_approx = lambda time: known.x(time) if time < last_t else \
center.x + radius * cos(freq * (time - last_t) + phase)
self.y_approx = lambda time: known.y(time) if time < last_t else \
center.y + radius * sin(freq * (time - last_t) + phase)
dx = lambda time: known.dx(time) if time < last_t else \
-radius * freq * sin(freq * (time - last_t) + phase)
dy = lambda time: known.dy(time) if time < last_t else \
radius * freq * cos(freq * (time - last_t) + phase)
ddx = lambda time: known.ddx(time) if time < last_t else \
-radius * freq * freq * cos(freq * (time - last_t) + phase)
ddy = lambda time: known.ddy(time) if time < last_t else \
-radius * freq * freq * sin(freq * (time - last_t) + phase)
# FIXME: don't use division by y if y == 0
try:
if isnan(self.x_approx(last_t + 1)):
return known
except Exception:
return known
return Trajectory(x=self.x_approx, y=self.y_approx,
dx=dx, dy=dy, ddx=ddx, ddy=ddy)
def draw(self, screen, field):
if graphics.DRAW_SENSOR_RAYS:
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s, d in zip(self.sensors, self.distances):
r = s.get_ray(self.pos, ang)
graphics.draw_line(screen, field, (115, 115, 200),
r.orig,
r.orig + r.dir * d,
1)
if COLLISION_CHECK and self.collision:
# draw small purple circle indicating collision state
graphics.draw_circle(screen, field, (255, 0, 255),
self.pos,
0.5 * BOT_RADIUS)
# draw projections of virtual velocity onto sensor rays
if graphics.DRAW_SENSOR_RAYS:
vel_ang = signed_angle(self.virtual_vel_before_check, Vector(0.0, 1.0))
abs_vel = length(self.virtual_vel_before_check)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s in self.sensors:
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
proj = c * abs_vel;
if proj < 0:
continue
graphics.draw_line(screen, field, (115, 200, 200),
r.orig,
r.orig + r.dir * proj,
2)
v = Vector(0.0, 0.0)
try:
v = 0.5 * normalize(self.virtual_vel_before_check)
except ZeroDivisionError:
pass
# draw virtual velocity vector that was picked before collision check
graphics.draw_line(screen, field, (0, 200, 0),
self.pos,
self.pos + v,
1)
def update_state(self, delta_time, movement_sigma):
if self.pos_fun is not None:
self.time += delta_time
self.pos = self.pos_fun(self.time)
v = self.vel_fun(self.time, delta_time)
self.vel = length(v)
try:
self.dir = normalize(v)
except ZeroDivisionError:
# keep the previous value of dir
pass
else: # bot is user-controlled
if abs(self.vel) > self.max_vel:
self.vel = copysign(self.max_vel, self.vel)
if self.vel < 0:
self.vel = 0
self.pos += self.dir * self.vel * delta_time
self.shape.center = self.pos
def __init__(self, pos, dir, vel=0.0,
pos_fun=None,
max_vel=config.BOT_VEL_CAP,
max_accel=config.BOT_ACCEL_CAP,
radius=config.BOT_RADIUS,
collidable=False):
self.pos = Point(*pos)
self.dir = normalize(dir)
self.vel = vel
self.max_vel = max_vel
self.max_accel = max_accel
self.radius = radius
self.width = 2.0 * radius
self.max_rot_vel = 2 * max_vel / self.width
self.time = 0
self.pos_fun = pos_fun
if collidable:
self.shape = Circle(self.pos, self.radius)
else:
self.shape = MockShape()
# avoid AttributeErrors in position update code
self.shape.center = self.pos
def draw(self, screen, field):
if graphics.DRAW_SENSOR_RAYS:
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s, d in zip(self.sensors, self.distances):
r = s.get_ray(self.pos, ang)
graphics.draw_line(screen, field, (115, 115, 200), r.orig,
r.orig + r.dir * d, 1)
if COLLISION_CHECK and self.collision:
# draw small purple circle indicating collision state
graphics.draw_circle(screen, field, (255, 0, 255), self.pos,
0.5 * BOT_RADIUS)
# draw projections of virtual velocity onto sensor rays
if graphics.DRAW_SENSOR_RAYS:
vel_ang = signed_angle(self.virtual_vel_before_check,
Vector(0.0, 1.0))
abs_vel = length(self.virtual_vel_before_check)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s in self.sensors:
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
proj = c * abs_vel
if proj < 0:
continue
graphics.draw_line(screen, field, (115, 200, 200), r.orig,
r.orig + r.dir * proj, 2)
v = Vector(0.0, 0.0)
try:
v = 0.5 * normalize(self.virtual_vel_before_check)
except ZeroDivisionError:
pass
# draw virtual velocity vector that was picked before collision check
graphics.draw_line(screen, field, (0, 200, 0), self.pos,
self.pos + v, 1)
def reset(eng, obstacle_map, model=models.DifferentialModel, interactive=False, log_data=False):
log_file = None
if log_data:
log_file = create_log_file()
log_dict = {"num_bots": 1 + config.NUM_FOLLOWERS} # counting the leader
print >> log_file, log_dict
eng.bots = []
eng.obstacles = []
eng.targets = []
if interactive:
pos_fun = None
start_pos = config.DEFAULT_START_POS
start_dir = Vector(0.0, 1.0)
eng.bots.append(
Bot(
models.MockModel(pos=start_pos, dir=start_dir, vel=0.0, pos_fun=pos_fun, collidable=True),
behavior=behaviors.Leader(log_file=log_file),
)
)
else:
# pos_fun = make_lissajous(17.0, 4, 1.2, 1, 3)
pos_fun = make_lissajous(25.5, 7, 1.8, 1, 3)
# pos_fun = make_ellipse()
# pos_fun = make_lissajous(18.0, 4, 4, 1, 1) # circle
# pos_fun = make_figure8()
eng.bots.append(
Bot(
models.MockModel(pos=(0.0, 0.0), dir=(1.0, 0.0), vel=0.0, pos_fun=pos_fun, collidable=True),
behavior=behaviors.Leader(log_file=log_file),
)
)
start_pos = eng.bots[0].real.pos_fun(0.0)
start_dir = normalize(eng.bots[0].real.vel_fun(0.0, 0.01))
eng.bots[0].real.pos = start_pos
eng.bots[0].real.dir = start_dir
displacement = -start_dir * 3.1 * config.BOT_RADIUS
for i in xrange(config.NUM_FOLLOWERS):
eng.bots.append(
Bot(
model(pos=start_pos + (i + 1) * displacement, dir=start_dir, vel=0.0),
behavior=behaviors.Follower(
g=30,
zeta=0.9,
leader=eng.bots[i],
trajectory_delay=config.TRAJECTORY_DELAY,
update_delta_t=config.SENSOR_UPDATE_DELAY,
orig_leader=eng.bots[0],
orig_leader_delay=config.TRAJECTORY_DELAY * (i + 1),
noise_sigma=config.MEASUREMENT_SIGMA,
log_file=log_file,
visibility_fov=config.FOV_ANGLE,
visibility_radius=config.FOV_RADIUS,
id="%02d" % (i + 1),
dt=1.0 / config.SENSOR_UPDATE_DELAY,
),
)
)
eng.obstacles = maps[obstacle_map][:]
eng.time = 0
def get_ray(self, bot_pos, bot_angle):
ang = self.angle + bot_angle
dir = rotate(Vector(0, self.bot_radius), ang)
dir = normalize(dir)
return Ray(bot_pos + dir * self.bot_radius, dir)
def dir(self):
try:
return normalize(self.vel)
except ZeroDivisionError:
return Vector(1.0, 0.0)
def calc_desired_velocity(self, bots, obstacles, targets):
vel = self.real_vel
self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos),
self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(k=-10.0),
dist(target, self.pos),
target - self.pos,
0)
vel += _FORCE_SENSITIVITY * force
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
self.distances = []
for s in self.sensors:
d = s.get_distance(self.pos, ang, obstacles)
self.distances.append(d)
if d < self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.5),
d,
-s.get_ray(self.pos, ang).dir,
OBSTACLE_CLEARANCE)
vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
# TODO: get max_vel from corresponding Model
if length(vel) > BOT_VEL_CAP:
vel = normalize(vel) * BOT_VEL_CAP
self.virtual_vel_before_check = vel
if COLLISION_CHECK:
# check for possible collisions:
# stop moving forward if an obstacle is nearby
collision = False
abs_vel = length(vel)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for cur_d, s in zip(self.distances, self.sensors):
if cur_d == s.max_distance:
continue
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(vel, r.dir))
new_d = cur_d - c * abs_vel * self.collision_delta_time
if new_d < CRITICAL_DIST and new_d < cur_d:
collision = True
break
if not collision:
# separate check for possible collision with another bot
for inter in bots:
# don't count collsions with self
if inter.virtual is self:
continue
new_d = dist(inter.real.pos + self.collision_delta_time * inter.real.vel,
self.pos + self.collision_delta_time * vel)
new_d -= (self.radius + inter.real.radius)
cur_d = dist(inter.real.pos, self.pos)
cur_d -= (self.radius + inter.real.radius)
if d <= CRITICAL_DIST and new_d < cur_d:
collision = True
break
if collision:
vel = normalize(vel) * CRITICAL_VEL
self.collision = True
else:
self.collision = False
return vel
def testNormalize(self):
self.assertEqual(normalize(Vector(2.0, 2.0)),
Vector(0.5 * 2**0.5, 0.5 * 2**0.5))
self.assertEqual(normalize(Vector(0.01, 0.0)), Vector(1.0, 0.0))
with self.assertRaises(ZeroDivisionError):
normalize(Vector(0.0, 0.0))
def get_ray(self, bot_pos, bot_angle):
ang = self.angle + bot_angle
dir = rotate(Vector(0, self.bot_radius), ang)
dir = normalize(dir)
return Ray(bot_pos + dir * self.bot_radius, dir)
def calc_desired_velocity(self, bots, obstacles, targets):
vel = self.real_vel
self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(
inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(
potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos), self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(
k=-10.0), dist(target, self.pos), target - self.pos, 0)
vel += _FORCE_SENSITIVITY * force
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
self.distances = []
for s in self.sensors:
d = s.get_distance(self.pos, ang, obstacles)
self.distances.append(d)
if d < self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.5),
d, -s.get_ray(self.pos, ang).dir,
OBSTACLE_CLEARANCE)
vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
# TODO: get max_vel from corresponding Model
if length(vel) > BOT_VEL_CAP:
vel = normalize(vel) * BOT_VEL_CAP
self.virtual_vel_before_check = vel
if COLLISION_CHECK:
# check for possible collisions:
# stop moving forward if an obstacle is nearby
collision = False
abs_vel = length(vel)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for cur_d, s in zip(self.distances, self.sensors):
if cur_d == s.max_distance:
continue
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(vel, r.dir))
new_d = cur_d - c * abs_vel * self.collision_delta_time
if new_d < CRITICAL_DIST and new_d < cur_d:
collision = True
break
if not collision:
# separate check for possible collision with another bot
for inter in bots:
# don't count collsions with self
if inter.virtual is self:
continue
new_d = dist(
inter.real.pos +
self.collision_delta_time * inter.real.vel,
self.pos + self.collision_delta_time * vel)
new_d -= (self.radius + inter.real.radius)
cur_d = dist(inter.real.pos, self.pos)
cur_d -= (self.radius + inter.real.radius)
if d <= CRITICAL_DIST and new_d < cur_d:
collision = True
break
if collision:
vel = normalize(vel) * CRITICAL_VEL
self.collision = True
else:
self.collision = False
return vel
