def compute_avoid():
vec1 = Vector2()
direction = Vector2()
for point in obstacle:
vec1.x = robot2[0] - point[0]
vec1.y = robot2[1] - point[1]
distance = vec1.norm() #magnitude of vector = length
vec1 = vec1 / distance #convert vec1 to unit vector
vec1 = vec1 / distance #magnitude in opposite direction is inversely proportional to distance btw robots
direction += vec1
#print("avoid: ", direction)
return direction
def compute_separation(nearest_agents):
#Find distance between neighboring agents
vec1 = Vector2()
direction = Vector2()
for agent in nearest_agents:
vec1.x = robot2[0] - agent[0]
vec1.y = robot2[1] - agent[1]
distance = vec1.norm() #magnitude of vector = length
vec1 = vec1 / distance #convert vec1 to unit vector
vec1 = vec1 / distance #magnitude in opposite direction is inversely proportional to distance btw robots
direction += vec1
#print("separation: ", direction)
return direction
def __init__(self, initial_velocity_x, initial_velocity_y, wait_count,
start_count, frequency):
self.position = Vector2()
self.velocity = Vector2()
self.mass = 1.0 # Mass of robot in kilograms
self.wait_count = wait_count # Waiting time before starting
self.start_count = start_count # Time during which initial velocity is being sent
self.frequency = frequency # Control loop frequency
# Set initial velocity
self.initial_velocity = Twist()
self.initial_velocity.linear.x = 0.0
self.initial_velocity.linear.y = 0.0
def compute_alignment(self, nearest_agents):
mean_velocity = Vector2()
steer = Vector2()
# Find mean direction of neighboring agents.
for agent in nearest_agents:
agent_velocity = get_agent_velocity(agent)
mean_velocity += agent_velocity
print("alignment*: %s", mean_velocity)
# Steer toward calculated mean direction with maximum velocity.
if nearest_agents:
mean_velocity.set_mag(self.max_speed)
steer = mean_velocity - self.velocity
steer.limit(self.max_force)
return steer
def compute_alignment(self, nearest_agents):
"""Return alignment component."""
mean_velocity = Vector2()
steer = Vector2()
# Find mean velocity of neighboring agents
for agent in nearest_agents:
agent_velocity = get_agent_velocity(agent)
mean_velocity += agent_velocity
# Steer toward calculated mean velocity
if nearest_agents:
mean_velocity.set_mag(self.max_speed)
steer = mean_velocity - self.velocity
steer.limit(self.max_force)
return steer
def compute_avoids(self, avoids):
"""
Return avoid component.
This rule consists of two components. The first is active for all
obstacles within range and depends on agent's distance from obstacle as
well as its aproach angle. Force is maximum when agent approaches the
obstacle head-on and minimum if obstacle is to the side of an agent.
Second component depends only on distance and only obstacles closer than
avoid_radius are taken into account. This is to ensure minimal distance
from obstacles at all times.
"""
main_direction = Vector2()
safety_direction = Vector2()
count = 0
# Calculate repulsive force for each obstacle in sight.
for obst in avoids:
obst_position = get_obst_position(obst)
d = obst_position.norm()
obst_position *= -1 # Make vector point away from obstacle.
obst_position.normalize() # Normalize to get only direction.
# Additionally, if obstacle is very close...
if d < self.avoid_radius:
# Scale lineary so that there is no force when agent is on the
# edge of minimum avoiding distance and force is maximum if the
# distance from the obstacle is zero.
safety_scaling = -2 * self.max_force / self.avoid_radius * d + 2 * self.max_force
safety_direction += obst_position * safety_scaling
count += 1
# For all other obstacles: scale with inverse square law.
obst_position = obst_position / (self.avoid_scaling * d**2)
main_direction += obst_position
if avoids:
# Calculate the approach vector.
a = angle_diff(self.old_heading, main_direction.arg() + 180)
# We mustn't allow scaling to be negative.
side_scaling = max(math.cos(math.radians(a)), 0)
# Divicde by number of obstacles to get average.
main_direction = main_direction / len(avoids) * side_scaling
safety_direction /= count
rospy.logdebug("avoids*: %s", main_direction)
# Final force is sum of two componets.
# Force is not limited so this rule has highest priority.
return main_direction + safety_direction
def compute_migration():
#Find vector pointing to migration point
migration_point = Vector2()
migration_point.x = pos_leaderx - pos_robot2x
migration_point.y = pos_leadery - pos_robot2y
print("migration*: %s", migration_point)
return migration_point
def compute_migration():
#Find vector pointing to migration point
migration_point = Vector2()
migration_point.x = leader[0] - robot2[0]
migration_point.y = leader[1] - robot2[1]
#print("migration*: %s", migration_point)
return migration_point
def compute_migration():
#Find vector pointing to migration point
migration_point = Vector2()
migration_point.x = pos_leaderx - pos_robot3x
migration_point.y = pos_leadery - pos_robot3y
print('migration: ', migration_point)
return migration_point
def compute_velocity():
nearest_agents = compute_nearest_agents()
if not nearest_agents:
vel = Twist()
vel.linear.x = 0
vel.linear.y = 0
return vel
# Compute all the components.
alignment = compute_alignment(nearest_agents)
cohesion = compute_cohesion(nearest_agents)
separation = compute_separation(nearest_agents)
migration = compute_migration()
avoid = compute_avoid()
alignment_factor = 0.2
cohesion_factor = 0.3
separation_factor = 0.6
migration_factor = 0.6
avoid_factor = 1.4
# Add components together and limit the output.
velocity = Vector2()
velocity += alignment * alignment_factor
velocity += cohesion * cohesion_factor
velocity += separation * separation_factor
velocity += migration * migration_factor
velocity += avoid * avoid_factor
# Return the the velocity as Twist message.
vel = Twist()
vel.linear.x = velocity.x
vel.linear.y = velocity.y
return vel
def compute_separation(self, nearest_agents):
"""Return separation component."""
direction = Vector2()
count = 0
# Calculate repulsive force for each neighboring agent in sight.
for agent in nearest_agents:
agent_position = get_agent_position(agent)
d = agent_position.norm()
if d < self.crowd_radius:
count += 1
agent_position *= -1 # Make vector point away from other agent.
agent_position.normalize() # Normalize to get only direction.
# Vector's magnitude is proportional to inverse square of the distance between agents.
agent_position = agent_position / (self.separation_scaling *
d**2)
direction += agent_position
if count:
# Devide by number of close-by agents to get average force.
direction /= count
direction.limit(
2 * self.max_force
) # 2 * max_force gives this rule a slight priority.
rospy.logdebug("separation*: %s", direction)
return direction
def compute_velocity():
# Compute all the components.
alignment = compute_alignment()
cohesion = compute_cohesion()
separation = compute_separation()
migration = compute_migration()
print("alignment: ", alignment)
print("cohesion: ", cohesion)
print("separation: ", separation)
print("migration: ", migration)
alignment_factor = 1.5
cohesion_factor = 1.0
separation_factor = 0.0
migration_factor = 1.0
# Add components together and limit the output.
velocity = Vector2()
velocity += alignment * alignment_factor
velocity += cohesion * cohesion_factor
velocity += separation * separation_factor
velocity += migration * migration_factor
print('velocity: ', velocity)
# Return the the velocity as Twist message.
vel = Twist()
vel.linear.x = velocity.x
vel.linear.y = velocity.y
return vel
def compute_cohesion(self, nearest_agents):
mean_position = Vector2()
direction = Vector2()
# Find mean position of neighboring agents.
for agent in nearest_agents:
agent_position = get_agent_position(agent)
mean_position += agent_position
# Apply force in the direction of calculated mean position.
# Force is proportional to agents' distance from the mean.
if nearest_agents:
direction = mean_position / len(nearest_agents)
print("cohesion*: %s", direction)
d = direction.norm()
direction.set_mag((self.max_force * (d / self.search_radius)))
return direction
def compute_alignment():
#Find mean direction of neighboring agents.
mean_velocity = Vector2()
mean_velocity.x = (vel_leaderx + vel_robot2x) / 2
mean_velocity.y = (vel_leadery + vel_robot2y) / 2
print('alignment: ', mean_velocity)
return mean_velocity
def createPolygon(self, points, static=True):
obj = GameObject.GameObject(self)
((x, y), vectors) = Polygon.getPolygonFromPoints(points)
obj.transform.position = Vector2.Vector2(x, y)
obj.collider = Collider.PolygonCollider(obj, 0, 0, vectors)
obj.collider.static = static
return obj
def compute_velocity():
leader[0] = pos_leaderx
leader[1] = pos_leadery
leader[2] = vel_leaderx
leader[3] = vel_leadery
robot2[0] = pos_robot2x
robot2[1] = pos_robot2y
robot2[2] = vel_robot2x
robot2[3] = vel_robot2y
robot3[0] = pos_robot3x
robot3[1] = pos_robot3y
robot3[2] = vel_robot3x
robot3[3] = vel_robot3y
#robot4[0] = pos_robot4x
#robot4[1] = pos_robot4y
#robot4[2] = vel_robot4x
#robot4[3] = vel_robot4y
all_agents[0] = leader
all_agents[1] = robot2
#all_agents[2] = robot4
nearest_agents = compute_nearest_agents()
# Compute all the components.
alignment = compute_alignment(nearest_agents)
cohesion = compute_cohesion(nearest_agents)
separation = compute_separation(nearest_agents)
migration = compute_migration()
#print('alignment: ', alignment)
#print('cohesion: ', cohesion)
#print('separation: ', separation)
#print('migration: ', migration)
alignment_factor = 0.2
cohesion_factor = 0.2
separation_factor = 0.5
migration_factor = 1
# Add components together and limit the output.
velocity = Vector2()
velocity += alignment * alignment_factor
velocity += cohesion * cohesion_factor
velocity += separation * separation_factor
velocity += migration * migration_factor
#print('velocity: ', velocity)
# Return the the velocity as Twist message.
vel = Twist()
vel.linear.x = velocity.x
vel.linear.y = velocity.y
return vel
def compute_alignment(nearest_agents):
#Find mean direction of neighboring agents.
mean_velocity = Vector2()
for agent in nearest_agents:
mean_velocity.x += agent[2]
mean_velocity.y += agent[3]
mean_velocity.x = mean_velocity.x / len(nearest_agents)
mean_velocity.y = mean_velocity.y / len(nearest_agents)
return mean_velocity
def compute_cohesion(self, nearest_agents):
"""Return cohesion component."""
mean_position = Vector2()
steer = Vector2()
# Find mean position of neighboring agents
for agent in nearest_agents:
agent_position = get_agent_position(agent)
mean_position += agent_position
direction = mean_position / len(nearest_agents)
# Steer toward calculated mean position
# Force is proportional to agents distance from mean
d = direction.norm()
if d > 0:
direction.set_mag(self.max_speed * (d / self.search_radius))
steer = direction - self.velocity
steer.limit(self.max_force)
return steer
def createCircGameObject(self, x, y, r, riged=True, trigger=False):
obj = GameObject.GameObject(self)
obj.transform.position = Vector2.Vector2(x, y)
col = Collider.CircleCollider(obj, 0, 0, r)
col.isTrigger = trigger
if riged:
obj.addComponent(Rigid.Rigid)
self.addObject(obj)
return obj
def compute_cohesion():
#Find mean position of neighboring agents.
mean_position = Vector2()
mean_position.x = (pos_leaderx + pos_robot2x) / 2
mean_position.y = (pos_leadery + pos_robot2y) / 2
mean_position.x -= pos_robot3x
mean_position.y -= pos_robot3y
print('cohesion: ', mean_position)
return mean_position
def compute_avoids(self, avoids):
"""Return avoid component."""
direction = Vector2()
steer = Vector2()
# Find mean position of obstacles
for obst in avoids:
obst_position = get_obst_position(obst)
d = obst_position.norm()
obst_position *= -1 # Make vector point away from obstacle
obst_position.normalize() # Normalize to get only direction
# Vector's magnitude is reciprocal to distance between agents
obst_position /= d
direction += obst_position
# Steer away from calculated mean position
if avoids:
direction.set_mag(self.max_speed)
steer = direction - self.velocity
steer.limit(self.max_force)
return steer
def compute_separation():
#Find distance between neighboring agents
vec1 = Vector2()
vec1.x = pos_robot2x - pos_leaderx
vec1.y = pos_robot2y - pos_leadery
distance = vec1.norm() #magnitude of vector = length
vec1 = vec1 / distance #convert vec1 to unit vector
vec1 = vec1 / distance #magnitude in opposite direction is inversely proportional to distance btw robots
vec2 = Vector2()
vec2.x = pos_robot2x - pos_robot3x
vec2.y = pos_robot2y - pos_robot3y
distance = vec2.norm() #magnitude of vector = length
vec2 = vec2 / distance #convert vec1 to unit vector
vec2 = vec2 / distance #magnitude in opposite direction is inversely proportional to distance btw robots
direction = Vector2()
direction = vec1 + vec2
print("separation: ", direction)
return direction
def compute_separation():
#Find distance between neighboring agents
vec1 = Vector2()
vec1.x = pos_robot3x - pos_leaderx
vec1.y = pos_robot3y - pos_leadery
normal = vec1.norm()
#distance = math.sqrt(pow(pos_robot3x-pos_robot3x, 2)+ pow(pos_leadery-pos_leadery, 2))
vec1 = vec1 * normal
vec2 = Vector2()
vec2.x = pos_robot3x - pos_robot2x
vec2.y = pos_robot3y - pos_robot2y
normal = vec2.norm()
#distance = math.sqrt(pow(pos_robot3x-pos_robot3x, 2) + pow(pos_robot2y-pos_robot2y,2))
vec2 = vec2 * normal
direction = Vector2()
direction.x = -vec1.x - vec2.x
direction.y = -vec1.y - vec2.y
print('separation: ', direction)
return direction
def compute_cohesion(nearest_agents):
#Find mean position of neighboring agents.
mean_position = Vector2()
for agent in nearest_agents:
mean_position.x = agent[0]
mean_position.y = agent[1]
mean_position.x = mean_position.x / len(nearest_agents)
mean_position.y = mean_position.x / len(nearest_agents)
mean_position.x -= robot2[0]
mean_position.y -= robot2[1]
#print("cohesion: ", mean_position)
return mean_position
def __init__(self, initial_velocity_x, initial_velocity_y, wait_count, start_count, frequency):
"""Create an empty boid and update parameters."""
self.position = Vector2()
self.velocity = Vector2()
self.mass = 0.18 # Mass of Sphero robot in kilograms
self.wait_count = wait_count # Waiting time before starting
self.start_count = start_count # Time during which initial velocity is being sent
self.frequency = frequency # Control loop frequency
# Set initial velocity
self.initial_velocity = Twist()
self.initial_velocity.linear.x = initial_velocity_x
self.initial_velocity.linear.y = initial_velocity_y
# UNCOMMENT IF USING FILTER
# # Initialize moving average filters.
# self.x_filter = MAFilter(3)
# self.y_filter = MAFilter(3)
# This dictionary holds values of each flocking components and is used
# to pass them to the visualization markers publisher.
self.viz_components = {}
def compute_arrival(self, pose_x, pose_y, my_agent):
#definiram publisher(topic tipa Twist)
#definiram node anonymous=True da nebi imao vise istih nodova
velocity_arrival = Vector2()
radius = self.arrival_radius
target_offset_x = pose_x - get_agent_position(my_agent).x
target_offset_y = pose_y - get_agent_position(my_agent).y
ramped_speed_x = self.max_force * (target_offset_x / radius)
ramped_speed_y = self.max_force * (target_offset_y / radius)
velocity_arrival.x = min(ramped_speed_x, self.max_force)
velocity_arrival.y = min(ramped_speed_y, self.max_force)
return velocity_arrival
def compute_separation(self, nearest_agents):
"""Return separation component."""
direction = Vector2()
steer = Vector2()
count = 0
# Find mean position of neighboring agents
for agent in nearest_agents:
agent_position = get_agent_position(agent)
if agent_position.norm() < self.crowd_radius:
count += 1
d = agent_position.norm()
agent_position *= -1 # Make vector point away from other agent
agent_position.normalize() # Normalize to get only direction
# Vector's magnitude is reciprocal to distance between agents
agent_position /= d
direction += agent_position
# Steer away from calculated mean position
if count:
direction.set_mag(self.max_speed)
steer = direction - self.velocity
steer.limit(self.max_force)
return steer
def __init__(self, main, size, gravity=Vector2.Vector2()):
size = (main.screen.get_width(), main.screen.get_height())
self.terrain = Terrain.Terrain(main.resources, size, self)
self.phyEng = PhysEng.PhysEng()
self.objects = []
self.toRemove = []
self.gravity = gravity
self.main = main
self.reset = False
self.load = False
self.objs = []
self.map_file = []
self.name = "world"
self.music = []
self.size = size
def visualizeColorMap(mapGraph, picSize):
mapSize = mapGraph.getMapSize()
pict = pg.Surface((mapSize.a, mapSize.b))
nodes = mapGraph.getNodes()
for x in range(mapSize.a):
for y in range(mapSize.b):
mapPos = Vector2(x, y)
pict.set_at(
(x, y),
min(mapGraph.getNodes(),
key=lambda n: abs(n.pos - mapPos)).getObject().getColor())
pict = pg.transform.scale(pict, (picSize.a, picSize.b))
return pict
def __init__ (self):
self.fullscreen = True
self.ToggleFullScreen()
Game.setInstance(self)
self.INIT = True
pygame.init()
self.resources = Resources.Resources()
pygame.display.set_caption(self.GAMENAME)
#pygame.display.toggle_fullscreen()
if not hasattr(self, 'World'):
self.world = World.World(self, self.RESOLUTION, Vector2.Vector2(0, 0))
self.fps = 0
self.fps_time = 0
self.delta = 0