def run(self):
global s
#global gui
while 1:
cmd = base_control.base_control(self.bot)
cmd += base_control.obstacle_avoidance(self.bot)
cmd += surround_attractor(self.bot)
cmd += exp.explore(self.bot)
cmd.exec(self.bot)
def disp_exp_area_cvg(bot, use_base_control=True,\
disp_weight_function = hyperbolic,\
exp_weight_function = linear_trunc,\
disp_weight_params = [],\
exp_weight_params = []):
""""
Weighted combination of dispersion and random motion
Begins with a low weight for random motion and high for dispersion
Weights change as per their weight functions
Ends with high random motion and low dispersion
"""
"""
Args:
<task>_weight_function: decides how task weights vary with iteration_
<task>_weight_params are unpacked and passed to the weight function
Returns: Cmd
"""
if bot.sim == None:
print("ERROR: Bot is not linked to a simulation")
return
t = bot.sim.time_elapsed
#Calculate weights and create command
weight_disp = disp_weight_function(t, *disp_weight_params)
weight_exp = exp_weight_function(t, *exp_weight_params)
cmd = disp_field(bot,
neighbourhood_radius=NEIGHBOURHOOD_RADIUS) * weight_disp
cmd += exp.explore(bot) * weight_exp
#Add base control if needed (Reccomended)
if (use_base_control):
cmd += base_control.base_control(bot)
obstacle_field_weights = {
'bots': 1,
'obstacles': 0.3,
'borders': 0.3,
'goal': 0.0,
'items': 0.1
}
cmd += base_control.obstacle_avoidance(bot, obstacle_field_weights)
return cmd
swarm_tasks.utils.robot.MAX_SPEED = 1.5
swarm_tasks.utils.robot.MAX_ANGULAR: 0.3
np.random.seed(42)
random.seed(42)
from swarm_tasks.simulation import simulation as sim
from swarm_tasks.simulation import visualizer as viz
#Required modules
import swarm_tasks.controllers.base_control as base_control
from swarm_tasks.modules.aggregation import aggr_centroid
#Initialize Simulation and GUI
s = sim.Simulation(env_name='rectangles')
gui = viz.Gui(s)
gui.show_env()
while (1):
for b in s.swarm:
#Base control
cmd = base_control.base_control(b)
cmd += base_control.obstacle_avoidance(b)
#Behaviour
cmd += aggr_centroid(b)
#Execute
cmd.exec(b)
gui.update()
def gather_resources(bot, use_base_control=True,\
thresh_dist=0.1):
num_contact = 0
item_visible = False
nest_visible = False
nest_pt = None
nest_contact = False
for item in bot.sim.contents.items:
if item.subtype in ['resource', 'nest']:
pos = Point(bot.get_position())
p1, p2 = nearest_points(pos, item.polygon)
r = p2.distance(p1)
if r <= bot.size + thresh_dist:
if item.subtype == 'nest':
nest_pt = np.array(p2)
nest_visible = True
nest_contact = True
bot.state = STATE_ENDPOINT
num_contact += 1
item_visible = True
continue
if r < PERIMETER_NEIGHBOURHOOD_RADIUS:
if item.subtype == 'nest':
nest_pt = np.array(p2)
nest_visible = True
item_visible = True
continue
neighbours_line = bot.neighbours(PERIMETER_NEIGHBOURHOOD_RADIUS,
single_state=True,
state=STATE_LINE)
neighbours_deployed = bot.neighbours(PERIMETER_NEIGHBOURHOOD_RADIUS,
single_state=True,
state=STATE_DEPLOY)
neighbours_waiting = bot.neighbours(PERIMETER_NEIGHBOURHOOD_RADIUS,
single_state=True,
state=STATE_ENDPOINT)
#SEARCH
if bot.state == STATE_SEARCH:
# cmd = cvg.disp_exp_area_cvg(bot, use_base_control=False, \
# exp_weight_params=[1.0,1.5],\
# disp_weight_params=[2.0, 1.0])
cmd = disp(bot, item_types=['all']) * 2
cmd += exp.explore(bot) * 1.5
if nest_contact:
bot.unstuck(bot.size)
print("UNSTUCK!!")
if (len(neighbours_line) or neighbours_waiting):
switch(bot, STATE_LINE, 0.1)
pass
if item_visible:
switch(bot, STATE_DEPLOY, 0.005)
#DEPLOYED
if bot.state == STATE_DEPLOY:
cmd = surround_attractor(bot)
switch(bot, STATE_SEARCH, 0.002)
if num_contact:
#bot.state = STATE_ENDPOINT #
if nest_visible:
cmd = follow.follow_point(bot, nest_pt) * 0.01
else:
cmd = aggr_centroid(bot, single_state=True,
state=STATE_LINE) * 0.01
if (len(neighbours_line) or nest_visible):
switch(bot, STATE_ENGAGE,\
0.3*nest_visible + 0.3*(len(neighbours_line)>0)*(not nest_visible))
switch(bot, STATE_ENGAGE, 0.05)
switch(bot, STATE_SEARCH, 0.01 * len(bot.neighbours(bot.size * 3)))
#HOME
if bot.state == STATE_ENDPOINT:
cmd = surround_attractor(bot)
if not num_contact:
bot.state = STATE_DEPLOY
if num_contact > 1:
bot.state = STATE_SEARCH
switch(bot, STATE_SEARCH, 0.02 * len(neighbours_waiting))
#LINE
if bot.state == STATE_LINE:
cmd = line(bot, LINE_NEIGHBOURHOOD_RADIUS, True,
[STATE_LINE, STATE_ENDPOINT, STATE_DEPLOY]) * 2.5
#cmd = surround_attractor(bot)*0.2
#cmd += aggr_centroid(bot, single_state=True, state=STATE_LINE)*0.2
#cmd += exp.explore(bot)
cmd += base_control.base_control(bot)
if item_visible:
switch(bot, STATE_DEPLOY, 0.003 * (not nest_visible) + 0.001)
if not nest_visible:
cmd += exp.explore(
bot) * 0.75 #(Replace with linearly increasing truncated wt)
cmd += aggr_centroid(bot) * 0.1
cmd += surround_attractor(bot) * 0.05
else:
cmd += disp(bot) * 0.2
if num_contact:
bot.state = STATE_SEARCH
if not len(neighbours_line) + len(neighbours_waiting):
switch(bot, STATE_SEARCH, 0.075) #Tune
switch(bot, STATE_SEARCH, 0.0025)
#ENGAGED
if bot.state == STATE_ENGAGE:
if nest_visible:
cmd = follow.follow_point(bot, nest_pt)
else:
cmd = aggr_centroid(bot, single_state=True, state=STATE_LINE)
if not num_contact:
bot.state = STATE_DEPLOY
if num_contact > 1:
bot.state = STATE_SEARCH
if not len(neighbours_line):
switch(bot, STATE_DEPLOY, 0.2)
switch(bot, STATE_DEPLOY, 0.005)
#Add base control if needed -- IS ALWAYS NEEDED!!
if (use_base_control and bot.state != STATE_ENGAGE):
cmd += base_control.base_control(bot) * 0.5
field_weights = {
'bots': 1,
'obstacles': 1,
'borders': 2,
'goal': -3,
'items': 1
}
if bot.state == STATE_DEPLOY:
field_weights['items'] = 0.00
elif bot.state == STATE_ENDPOINT:
field_weights['bots'] = 3.0
field_weights['items'] = 0.00
cmd += base_control.obstacle_avoidance(bot, field_weights) * 0.3
return cmd
def remove_contamination(bot, use_base_control=True,\
surround_weight = 2.5,\
search_weight = 2.0,\
thresh_dist = 0.25,\
use_follow_state=True):
#If item is in contact or item is seen, change state
flag=False
for item in bot.sim.contents.items:
if item.subtype == 'contamination':
pos = Point(bot.get_position())
p1,p2 = nearest_points(pos, item.polygon)
r = p2.distance(p1)
if r<= bot.size+thresh_dist:
flag=True
bot.set_state(STATE_PERIMETER)
break
if r < PERIMETER_NEIGHBOURHOOD_RADIUS:
flag=True
bot.set_state(STATE_RUSH)
continue #continue searching for STATE_PERIM
if not flag:
bot.set_state(STATE_SEARCH)
#Task
if bot.state ==STATE_SEARCH:
cmd = cvg.disp_exp_area_cvg(bot, use_base_control=False, \
exp_weight_params=[1.0,1.5],\
disp_weight_params=[2.0, 1.0]) * search_weight
#If a RUSH bot is seen, follow it
#STATE_FOLLOW are only the robots that see a rushing robot
#If a STATE_FOLLOW neighbour is seen, it is followed without changing state
neighbours_rush = bot.neighbours(PERIMETER_NEIGHBOURHOOD_RADIUS,single_state=True, state=STATE_RUSH)
neighbours_follow = bot.neighbours(PERIMETER_NEIGHBOURHOOD_RADIUS,single_state=True, state=STATE_FOLLOW)
if len(neighbours_rush)>0:
cmd+= follow_leader(bot, neighbours_rush[0])*3
if use_follow_state:
bot.set_state(STATE_FOLLOW)
elif len(neighbours_follow)>0:
cmd+= follow_leader(bot, neighbours_follow[0])*3
if bot.state==STATE_RUSH:
cmd =surround_attractor(bot) #Goes slow in hopes of being seen
cmd += base_control.base_control(bot) #Extra obst. avoidance (tuning)
if bot.state ==STATE_PERIMETER:
cmd =surround_attractor(bot)*surround_weight
cmd += cvg.disp_exp_area_cvg(bot, use_base_control=False, exp_weight_params=[0.5,1])
#Add base control if needed
if(use_base_control):
cmd += base_control.base_control(bot)*0.5
#ADD FIELD WEIGHT FOR ITEMS: REDUCE MIN DIST,INCREASE FIELD
field_weights={'bots':1.0, 'obstacles':0.5, 'borders':0.5, 'goal':-3, 'items':0.25}
cmd += base_control.obstacle_avoidance(bot, field_weights)
return cmd