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 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 __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 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