def stay_in_border(position):
v = PVector(0, 0)
Xmin = 13
Xmax = 13
Ymin = 13
Ymax = 13
constant = 100
if position.x <= Xmin:
v.x = constant
elif position.x >= Xmax:
v.x = -constant
if position.y <= Ymin:
v.y = constant
elif position.y >= Ymax:
v.y = -constant
return v.return_as_vector()
def separation (uav_name,blockListDict):
alt_d=8
pos=get_position(uav_name,blockListDict)
close_drones=blockListDict
position=PVector(pos[0],pos[1])
#close_drones=find_neighbours_in_radius(current,1000)
if len(close_drones)==0:
empty=PVector(0,0)
#velocity=PVector(current.v_ned_d[0],current.v_ned_d[1])
#current.set_v_2D_alt_lya(velocity.return_as_vector(),-alt_d)
return empty.return_as_vector()
dx=sub_all_x(uav_name,blockListDict)
dx=(dx/len(blockListDict))
dy=sub_all_y(uav_name,blockListDict)
dy=(dy/len(blockListDict))
sep_vector=numpy.array([dx,dy])
sep_vector=normalize(sep_vector)
return sep_vector
def cohesion (uav_name,blockListDict):
alt_d=8
pos=get_position(uav_name,blockListDict)
position=PVector(pos[0],pos[1])
close_drones=blockListDict
#close_drones=find_neighbours_in_radius(current,1000)
if len(close_drones)==0:
empty=PVector(0,0)
#velocity=PVector(current.v_ned_d[0],current.v_ned_d[1])
#current.set_v_2D_alt_lya(velocity.return_as_vector(),-alt_d)
return empty.return_as_vector()
sx=sum_all_x(uav_name,blockListDict)
sx=(sx/len(blockListDict))
sx=sx - pos[0]
sy=sum_all_y(uav_name,blockListDict)
sy=(sy/len(blockListDict))
sy=sy - pos[1]
cohesion_vec=numpy.array([(sx-pos[0]),(sy-pos[1])])
cohesion_vec=normalize(cohesion_vec)
return cohesion_vec
def getAvoidAvoids(position, avoid_radius):
relative_position = PVector(0, 0)
diff = PVector(0, 0)
steer = PVector(0, 0)
obstacle_position = PVector(0.0, 0.0)
count = 0
obstacle_position.subVector(position)
relative_position.addVector(obstacle_position)
d = math.sqrt(
pow((position.x - obstacle_position.x), 2) +
pow((position.y - obstacle_position.y), 2))
#for obstacle : obstacles :
# If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
if relative_position.normalize() < avoid_radius:
#Calculate vector pointing away from neighbor
position.subVector(obstacle_position)
diff.addVector(position)
diff.normalize()
diff.divScalar(d)
#Weight by distance
steer.addVector(diff)
#count++; // Keep track of how many
return steer.return_as_vector()
def separation(boids, current_drone):
alt_d = 4
desiredseparation = 50
steer = PVector(0.0, 0.0)
count = 0
position = current_drone.xyz
c_vector = PVector(position[0], position[1])
for other in boids:
if other.tag != current_drone.tag:
other_position = other.xyz
o_vector = PVector(other_position[0], other_position[1])
distance = c_vector.distance(o_vector)
if distance < desiredseparation:
c_vector.subVector(o_vector)
c_vector.normalize()
c_vector.divScalar(distance)
steer.addVector(c_vector)
count = count + 1
if count > 0:
steer.divScalar(count)
vres = steer.return_as_vector()
#current_drone.set_a_2D_alt_lya(vres[0:2],-alt_d)
return vres
def normalize(vector): vector=PVector(vector[0],vector[1]) vector.normalize() return vector.return_as_vector()
def avg(first,second): ########average point##### one=PVector(first[0],first[1]) second=PVector(second[0],second[1]) one.mean(second) return one.return_as_vector()
def flocking(agents, current, radius, kva, ks, kc, ke):
neighbor_count = 0
velAvg = PVector(0, 0)
centroid = PVector(0, 0)
separation = PVector(0, 0)
cohesion = PVector(0, 0)
obstacle = PVector(0, 0)
desired_velocity = PVector(0, 0)
position = PVector(current.xyz[0], current.xyz[1])
theta = 0
alt_d = 10
limitX = 15
limitY = 15
#We check all the agents on the screen.
#Any agent closer than radius units is a neighbor.
for it in agents:
neighbor = PVector(it.xyz[0], it.xyz[1])
relative_position = PVector(0, 0)
neighbor.subVector(position)
#relative_position.addVector(neighbor)
#relative_position=PVector(relativePosition[0],relativePosition[1])
d = math.sqrt(
pow((position.x - neighbor.x), 2) +
pow((position.y - neighbor.y), 2))
if d / 10 < radius:
#We have found a neighbor
neighbor_count = neighbor_count + 1
#We add all the positions
#centroid += it->getPosition();
it_position = PVector(it.xyz[0], it.xyz[1])
centroid.addVector(it_position)
it_velocity = PVector(it.v_ned[0], it.v_ned[1])
#We add all the velocities
velAvg.addVector(it_velocity)
#Vector pointing at the opposite direction w.r.t. your
#neighbor
#separation -= relativePosition;
separation.subVector(relative_position)
if neighbor_count == 0:
velocity = PVector(current.v_ned_d[0], current.v_ned_d[1])
return velocity.return_as_vector()
centroid.divScalar(
neighbor_count) # All the positions over the num of neighbors
velAvg.divScalar(
neighbor_count) # All the velocities over the numb of neighbors
#Relative position of the agent w.r.t. centroid
centroid.subVector(position)
cohesion.addVector(centroid)
#In order to compare the following vectors we normalize all of them,
# so they have the same magnitude. Later on with the gains
#kva, ks and kc we assing which vectors are more important.
velAvg.normalize()
cohesion.normalize()
separation.normalize()
if neighbor_count == 1:
#desired_velocity = velocity
velocity = PVector(current.v_ned_d[0], current.v_ned_d[1])
desired_velocity = velocity.return_as_vector()
else:
vel_avg = velAvg.return_as_vector()
v_separation = separation.return_as_vector()
v_cohesion = cohesion.return_as_vector()
v_target = tend_to_place(agents, current)
random_walk = randomWalkb(position.x, position.y)
v_bound_position = bound_position(17, -17, 17, -17, position.x,
position.y, 0.1)
#avoid_vector=getAvoidAvoids(position,1)
avoid_vector = apply_force(position, obstacle, current)
desired_velocity = kva * vel_avg + ks * v_separation + 5 * kc * v_cohesion + 3 * ke * v_bound_position + ke * v_target
#kva*vel_avg + ks*v_separation + kc*v_cohesion +ke*v_bound_position+ks*avoid_vector
desiredVel = PVector(desired_velocity[0], desired_velocity[1])
#desired_velocity +=kva*velAvg + ks*separation + kc*cohesion;
error_theta = math.atan2(desired_velocity[1], desired_velocity[0]) - theta
error_theta = ke * error_theta
#updateUnicycle(0, ke*error_theta);
current.set_v_2D_alt_lya(error_theta, -alt_d)
def flocking(current, radius, kva, ks, kc, ke):
agents = current.group.neibourgh_list
print len(agents), "length agents"
print current.tag
neighbor_count = 0
velAvg = PVector(0, 0)
centroid = PVector(0, 0)
separation = PVector(0, 0)
cohesion = PVector(0, 0)
obstacle = PVector(0, 0)
desired_velocity = PVector(0, 0)
position = PVector(current.xyz[0], current.xyz[1])
theta = 0
alt_d = 10
limitX = 15
limitY = 15
avoid_vector = numpy.array([0.0, 0.0])
avoid_coefficient = 0
is_fire = locate_fire(current, position)
if is_fire == True:
#avoid_vector=is_fire
avoid_coefficient = -1
if len(agents) == 0:
velocity = PVector(current.v_ned_d[0], current.v_ned_d[1])
return velocity.return_as_vector()
#We check all the agents on the screen.
#Any agent closer than radius units is a neighbor.
for it in agents:
neighbor = PVector(it.xyz[0], it.xyz[1])
relative_position = PVector(0, 0)
neighbor.subVector(position)
#relative_position.addVector(neighbor)
#relative_position=PVector(relativePosition[0],relativePosition[1])
d = math.sqrt(
pow((position.x - neighbor.x), 2) +
pow((position.y - neighbor.y), 2))
if d < 10000:
#We have found a neighbor
neighbor_count = neighbor_count + 1
#We add all the positions
#centroid += it->getPosition();
it_position = PVector(it.xyz[0], it.xyz[1])
centroid.addVector(it_position)
it_velocity = PVector(it.v_ned[0], it.v_ned[1])
#We add all the velocities
velAvg.addVector(it_velocity)
#Vector pointing at the opposite direction w.r.t. your
#neighbor
#separation -= relativePosition;
separation.subVector(relative_position)
if neighbor_count == 0:
velocity = PVector(current.v_ned_d[0], current.v_ned_d[1])
return velocity.return_as_vector()
centroid.divScalar(
neighbor_count) # All the positions over the num of neighbors
velAvg.divScalar(
neighbor_count) # All the velocities over the numb of neighbors
#Relative position of the agent w.r.t. centroid
centroid.subVector(position)
cohesion.addVector(centroid)
#In order to compare the following vectors we normalize all of them,
# so they have the same magnitude. Later on with the gains
#kva, ks and kc we assing which vectors are more important.
velAvg.normalize()
cohesion.normalize()
separation.normalize()
if neighbor_count == 7:
print "I am here"
#desired_velocity = velocity
#desiredVel=PVector(current.v_ned_d[0],current.v_ned_d[1])
#desired_velocity=desiredVel.return_as_vector()
else:
vel_avg = velAvg.return_as_vector()
v_separation = separation.return_as_vector()
v_cohesion = cohesion.return_as_vector()
v_target = tend_to_place(agents, current)
random_walk = randomWalkb(position.x, position.y)
v_bound_position = bound_position(17, -17, 17, -17, position.x,
position.y, 3)
desired_velocity = kva * vel_avg + ks * v_separation + kc * v_cohesion + ke * v_target #+avoid_coefficient*position.return_as_vector()
desiredVel = PVector(desired_velocity[0], desired_velocity[1])
if (desiredVel.magnitude() > 2):
desiredVel.normalize()
desiredVel.mulScalar(2)
desired_vel = desiredVel.return_as_vector()
current.set_v_2D_alt_lya(desired_vel, -alt_d)
