def connect(self):
"""
Connect to the MAVLINK interface, either to own SITL process or to
a specified external connection provided via the command line.
Returns
-------
None.
"""
try:
self.vehicle = connect(self._connection_string, wait_ready=True,
vehicle_class=DroneTreeVehicle)
except socket.error as e:
print(e)
return
self._bt = BehaviourTree(self._bt_func(self.vehicle))
self._snapshot_visitor = SnapshotVisitor()
self._bt.visitors.append(self._snapshot_visitor)
self._bt.visitors.append(DebugVisitor())
def setUp(self):
env = init_taxi_s(seed=1234)
target = list(env.decode(env.s))
print(target)
goalspec = 'F P_[PI]['+str(4)+',none,==]'
keys = ['L', 'PI', 'DI']
actions = [0, 1, 2, 3, 4, 5]
root = goalspec2BT(goalspec, planner=None)
self.behaviour_tree = BehaviourTree(root)
child = self.behaviour_tree.root
planner = GenRecPropTaxi(
env, keys, child.name, dict(), actions=actions,
max_trace=5, seed=123)
child.setup(0, planner, True, 5)
# print(child.goalspec, child.planner.goalspec, child.planner.env)
for i in range(5):
self.behaviour_tree.tick(
pre_tick_handler=reset_env_s(env)
)
print(i, self.behaviour_tree.root.status)
def __init__(self):
super(BTAgent, self)
global BLACKBOARD
py_trees.logging.level = py_trees.logging.Level.DEBUG
BLACKBOARD = Blackboard()
self.tree = BehaviourTree(self.create_tree())
_thread.start_new_thread(self.tree.tick_tock,
(50, CONTINUOUS_TICK_TOCK, None, None))
class TestTaxiFullActionsGoal(TestCase):
def setUp(self):
env = init_taxi(seed=1234)
target = list(env.decode(env.s))
print(target)
goalspec = '((((F(P_[L]['+give_loc(target[2])+',none,==])) U (F(P_[PI]['+str(4)+',none,==]))) U (F(P_[L]['+give_loc(target[3])+',none,==]))) U (F(P_[PI]['+str(target[3])+',none,==])))' # noqa: E501
keys = ['L', 'PI', 'DI']
actions = [0, 1, 2, 3, 4, 5]
root = goalspec2BT(goalspec, planner=None)
# print('root', root)
self.behaviour_tree = BehaviourTree(root)
epoch = [80, 50, 80, 50]
j = 0
for child in self.behaviour_tree.root.children:
# print('children', child, child.name, child.id)
planner = GenRecPropTaxi(
env, keys, child.name, dict(), actions=actions,
max_trace=40, seed=1)
child.setup(0, planner, True, epoch[j])
j += 1
# Training
for i in range(200):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
print('Training', self.behaviour_tree.root.status)
# Inference
for child in self.behaviour_tree.root.children:
child.train = False
# print(child, child.name, child.train)
for i in range(2):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
print('inference', self.behaviour_tree.root.status)
def test_full_actions(self):
self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
class TestMDPSequenceGoal(TestCase):
def setUp(self):
goalspec = 'F P_[NC][True,none,==] U F P_[L][03,none,==]'
startpoc = (0, 3)
self.env = init_mdp(startpoc)
keys = ['L', 'NC']
actions = [0, 1, 2, 3]
planner = GenRecPropMDPNear(
self.env, keys, goalspec, dict(),
actions=actions, max_trace=10, seed=123)
root = goalspec2BT(goalspec, planner=planner)
self.behaviour_tree = BehaviourTree(root)
for child in self.behaviour_tree.root.children:
# print(child, child.name)
child.setup(0, planner, True, 20)
# child.planner.env = env
# print(child.goalspec, child.planner.goalspec)
for i in range(20):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(self.env)
)
for child in self.behaviour_tree.root.children:
child.setup(0, planner, True, 10)
child.train = False
# print(child, child.name, child.train)
for i in range(2):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(self.env)
)
# print('inference', behaviour_tree.root.status)
def test_inference(self):
self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
def test_final_loc(self):
self.assertEqual(self.env.curr_loc, (0, 3))
def setUp(self):
env = init_taxi(seed=1234)
target = list(env.decode(env.s))
print(target)
goalspec = 'F P_[L]['+give_loc(target[2])+',none,==] U F P_[PI]['+str(4)+',none,==]' # noqa: E501
keys = ['L', 'PI', 'DI']
actions = [0, 1, 2, 3, 4, 5]
root = goalspec2BT(goalspec, planner=None)
self.behaviour_tree = BehaviourTree(root)
epoch = [50, 10]
j = 0
for child in self.behaviour_tree.root.children:
print('children', child, child.name, child.id)
planner = GenRecPropTaxi(
env, keys, child.name, dict(), actions=actions,
max_trace=40, seed=123)
child.setup(0, planner, True, epoch[j])
j += 1
# print(child.goalspec, child.planner.goalspec, child.planner.env)
# print('rootname', behaviour_tree.root.name)
# behaviour_tree.root.remove_child_by_id(id)
# display_bt(behaviour_tree)
for i in range(50):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
print('Training', self.behaviour_tree.root.status)
for child in self.behaviour_tree.root.children:
# child.setup(0, planner, True, 20)
child.train = False
print(child, child.name, child.train)
for i in range(2):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
print('inference', self.behaviour_tree.root.status)
def skeleton():
main = Sequence('1')
selec = Selector('2')
goal1 = LTLNode('g1')
goal2 = LTLNode('g2')
p2 = CondNode(str(goal2.id), goal2)
p1 = CondNode(str(goal1.id), goal1)
selec.add_children([p2, goal1])
seq = Sequence('3')
seq.add_children([p1, goal2])
main.add_children([seq, selec])
root = BehaviourTree(main)
return [root, p1, p2, goal1, goal2]
class TestTaxiTrainingSimpleGoal(TestCase):
def setUp(self):
env = init_taxi(seed=1234)
target = list(env.decode(env.s))
goalspec = 'F(P_[L]['+give_loc(target[2])+',none,==])'
keys = ['L', 'PI', 'DI']
actions = [0, 1, 2, 3]
root = goalspec2BT(goalspec, planner=None)
self.behaviour_tree = BehaviourTree(root)
child = self.behaviour_tree.root
planner = GenRecPropTaxi(
env, keys, None, dict(), actions=actions,
max_trace=40, seed=1234)
child.setup(0, planner, True, 50)
print(child.goalspec, child.planner.goalspec, child.planner.env)
for i in range(50):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
def test_training(self):
self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
def setup(self, timeout, agent, item):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
self.agent = agent
self.item = item
dropseq = Sequence('CDP_Sequence')
iscarrying = IsInPartialAttached('CDP_IsInPartial')
iscarrying.setup(0, self.agent, self.item)
drop = DropPartial('CDP_DropPartial')
drop.setup(0, self.agent, self.item)
dropseq.add_children([iscarrying, drop])
self.behaviour_tree = BehaviourTree(dropseq)
def setup(self, timeout, agent, item=None):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
self.agent = agent
self.item = item
# Define the root for the BT
root = Sequence("Ex_Sequence")
low = RandomWalk('Ex_RandomWalk')
low.setup(0, self.agent)
high = Move('Ex_Move')
high.setup(0, self.agent)
root.add_children([low, high])
self.behaviour_tree = BehaviourTree(root)
def setup(self, timeout, agent, item=None):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
self.agent = agent
self.item = item
# Define the root for the BT
root = Sequence('CRS_Sequence')
s1 = NeighbourObjects('CRS_NeighbourObjects')
s1.setup(0, self.agent, 'Signal')
s2 = ReceiveSignal('CRS_ReceiveSignal')
s2.setup(0, self.agent, 'Signal')
root.add_children([s1, s2])
self.behaviour_tree = BehaviourTree(root)
def setup(self, timeout, agent, item=None):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
self.agent = agent
self.item = item
# Define the root for the BT
root = Sequence('CPC_Sequence')
c1 = NeighbourObjects('CPS_SearchCue')
c1.setup(0, self.agent, 'Cue')
c2 = PickCue('CPS_PickCue')
c2.setup(0, self.agent, 'Cue')
root.add_children([c1, c2])
self.behaviour_tree = BehaviourTree(root)
class TestMDPTraining(TestCase):
def setUp(self):
goalspec = 'F P_[IC][True,none,==]'
startpoc = (1, 3)
env = init_mdp(startpoc)
keys = ['L', 'IC']
actions = [0, 1, 2, 3]
root = goalspec2BT(goalspec, planner=None)
self.behaviour_tree = BehaviourTree(root)
# # Need to udpate the planner parameters
child = self.behaviour_tree.root
planner = GenRecPropMDP(
env, keys, None, dict(), actions=actions, max_trace=10, seed=123)
child.setup(0, planner, True, 10)
for i in range(10):
self.behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
print(self.behaviour_tree.root.status)
def test_training(self):
self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
def decompose():
# goalspec = 'P_[KE][1,none,==] U P_[KA][1,none,==]'
# goalspec = 'P_[KE][1,none,==] U P_[KA][1,none,==] U P_[KB][1,none,==]'
# goalspec = 'P_[KA][1,none,==] U P_[KB][1,none,==] U P_[KC][1,none,==] U P_[KD][1,none,==], U P_[KE][1,none,==]'
goalspec = '(F(P_[KE][1,none,==]) U G(P_[KA][1,none,==]))'
# goalspec = '(P_[KA][1,none,==] & P_[KB][1,none,==]) U (P_[KC][1,none,==] & P_[KD][1,none,==])'
# goalspec = '((P_[KA][1,none,==] U P_[KB][1,none,==]) & (P_[KC][1,none,==] U P_[KD][1,none,==])) & (P_[KE][1,none,==] & P_[KF][1,none,==])'
root = goalspec2BT(goalspec, planner=None)
# for i in root.iterate():
# print(i.id, i.name)
behaviour_tree = BehaviourTree(root)
# bt = UntilNode('U')
py_trees.logging.level = py_trees.logging.Level.DEBUG
output = py_trees.display.ascii_tree(behaviour_tree.root)
print(output)
recursive_until(root)
output = py_trees.display.ascii_tree(behaviour_tree.root)
print(output)
def find_cheese(seed, max_trace_len=10, epoch=10):
# Define the environment for the experiment
goalspec = 'F P_[IC][True,none,==]'
# startpoc = (3, 0)
startpoc = (9, 0)
env = init_10x10mdp(startpoc)
keys = ['L', 'IC']
actions = [0, 1, 2, 3]
root = goalspec2BT(goalspec, planner=None)
# print(root)
behaviour_tree = BehaviourTree(root)
# display_bt(behaviour_tree, True)
# print(dir(behaviour_tree))
# # Need to udpate the planner parameters
child = behaviour_tree.root
# for child in behaviour_tree.root.children:
# print(child, child.name, env.curr_loc)
planner = GenRecPropMDP(
env, keys, None, dict(), actions=actions,
max_trace=max_trace_len, epoch=epoch)
child.setup(0, planner, True, epoch=epoch)
# Experiment data
# print(planner.trace_len_data)
data = np.zeros(epoch, dtype=np.uint8)
for i in range(epoch):
behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
# print(behaviour_tree.root.status)
data[i] = check_bt_status(behaviour_tree.root.status)
# print(planner.trace_len_data)
# for child in behaviour_tree.root.children:
child.setup(0, planner, True, max_trace_len)
child.train = False
# print(child, child.name, child.train)
for i in range(1):
behaviour_tree.tick(
pre_tick_handler=reset_env(env)
)
# data[-1] = check_bt_status(behaviour_tree.root.status)
# print('inference', behaviour_tree.root.status)
# print(planner.trace_len_data)
# print(data)
# print(env.curr_loc)
return (data, planner.trace_len_data)
def keydoor2():
env_name = 'MiniGrid-DoorKey-8x8-v0'
env = gym.make(env_name)
env.max_steps = min(env.max_steps, 200)
env.seed(12345)
env.reset()
env = env_setup(env)
state = (env.agent_pos, env.agent_dir)
print(state)
# Find the key and carry it
goalspec = '(F(P_[K][1,none,==]) U F(P_[C][1,none,==])) U (F(P_[D][1,none,==]))' # noqa: E501
# goalspec = '(F(P_[K][1,none,==]) U F(P_[C][1,none,==]))' # noqa: E501
keys = ['L', 'F', 'K', 'D', 'C', 'G', 'O']
actions = [[0, 1, 2, 3], [0, 1, 2, 3, 4], [0, 1, 2, 3]]
root = goalspec2BT(goalspec, planner=None)
behaviour_tree = BehaviourTree(root)
# display_bt(behaviour_tree, save=True)
epoch = [70, 40, 70]
j = 0
for child in behaviour_tree.root.children:
planner = GenRecPropKeyDoor(env,
keys,
child.name,
dict(),
actions=actions[j],
max_trace=40,
seed=None)
child.setup(0, planner, True, epoch[j])
j += 1
print(child.goalspec, child.planner.goalspec, type(child.planner.env))
for i in range(150):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Training', behaviour_tree.root.status)
# Inference
for child in behaviour_tree.root.children:
child.train = False
for i in range(2):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Inference', behaviour_tree.root.status)
def keydoor():
env_name = 'MiniGrid-DoorKey-8x8-v0'
env = gym.make(env_name)
env.max_steps = min(env.max_steps, 200)
env.seed(12345)
env.reset()
env = env_setup(env)
state = (env.agent_pos, env.agent_dir)
print(state)
# Find the key
goalspec = 'F P_[K][1,none,==]'
keys = ['L', 'F', 'K', 'D', 'C', 'G', 'O']
actions = [0, 1, 2, 3, 4, 5]
root = goalspec2BT(goalspec, planner=None)
behaviour_tree = BehaviourTree(root)
child = behaviour_tree.root
planner = GenRecPropKeyDoor(env,
keys,
child.name,
dict(),
actions=actions,
max_trace=40,
seed=None)
child.setup(0, planner, True, 100)
print(child.goalspec, child.planner.goalspec, type(child.planner.env))
# Train
for i in range(50):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Training', behaviour_tree.root.status)
child.train = False
# Inference
for i in range(1):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Inference', behaviour_tree.root.status)
class MultiGoalGridUExp():
def __init__(
self, expname='key', goalspecs='F P_[KE][1,none,==]',
keys=['LO', 'FW', 'KE'], actions=list(range(5)),
seed=None, maxtracelen=40, trainc=False, epoch=80):
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
if seed is None:
pass
else:
env = ReseedWrapper(env, seeds=[seed])
env = FullyObsWrapper(env)
self.env = env
self.env.max_steps = min(env.max_steps, maxtracelen-3)
# self.env.agent_view_size = 1
self.env.reset()
# self.env.render()
# import time
# time.sleep(10)
self.expname = expname
self.goalspecs = goalspecs
self.epoch = epoch
self.maxtracelen = maxtracelen
self.trainc = trainc
self.allkeys = [
'LO', 'FW', 'KE', 'DR',
'BOB', 'BOR', 'BAB', 'BAR',
'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR',
'DO', 'RM']
self.keys = keys
self.actions = actions
root = goalspec2BT(goalspecs, planner=None, node=CompetentNode)
self.behaviour_tree = BehaviourTree(root)
self.blackboard = Blackboard()
def run(self):
def fn_c(child):
pass
def fn_eset(child):
planner = GenRecPropMultiGoalU(
self.env, self.keys, child.name, dict(), actions=self.actions,
max_trace=self.maxtracelen, epoch=self.epoch,
seed=None, allkeys=self.allkeys, id=child.nodename)
child.setup(0, planner, True, self.epoch)
def fn_einf(child):
child.train = False
child.planner.epoch = 5
child.planner.tcount = 0
# child.planner.verbose = True
def fn_ecomp(child):
child.planner.compute_competency(self.trainc)
def parallel_hack(node):
if (isinstance(node, Parallel)):
return node
elif node.children:
for c in node.children:
if (isinstance(c, Parallel)):
# Control nodes
return c
parallel_hack(c)
# Save the environment to visualize
# self.save_data(env=True)
# Setup planners
recursive_setup(self.behaviour_tree.root, fn_eset, fn_c)
# py_trees.logging.level = py_trees.logging.Level.DEBUG
# py_trees.display.print_ascii_tree(self.behaviour_tree.root)
# Parallel node hack
def for_parallel_node(root):
node = parallel_hack(root)
if node:
combgoal = [node.name for node in node.children]
othernodes = []
for i in range(len(node.children)):
if i == 0:
node.children[i].planner.list_goalspec = combgoal
node.children[i].planner.parallel_node = True
else:
othernodes.append(node.children[i])
node.remove_child(node.children[i])
return node, othernodes
else:
return None, None
pnode, othernodes = for_parallel_node(self.behaviour_tree.root)
# combgoal = ['F(P_[KE][1,none,==])', 'G(P_[LV][0,none,==])']
# self.behaviour_tree.root.planner.list_goalspec = combgoal
# self.behaviour_tree.root.planner.parallel_node = True
self.blackboard.shared_content['current'] = dict()
for i in range(self.epoch):
self.env.reset()
self.blackboard.shared_content['current']['epoch'] = i
for j in range(self.maxtracelen):
self.behaviour_tree.tick(
# pre_tick_handler=self.reset_env()
)
# print(j, self.behaviour_tree.root.planner.gtable.keys())
if self.behaviour_tree.root.status == Status.SUCCESS:
break
if self.blackboard.shared_content['current']['done']:
break
# print(i, 'Training', self.behaviour_tree.root.status)
# Inference
recursive_setup(self.behaviour_tree.root, fn_einf, fn_c)
for i in range(self.epoch):
self.env.reset()
self.blackboard.shared_content['current']['epoch'] = i
for j in range(self.maxtracelen):
self.behaviour_tree.tick(
# pre_tick_handler=self.reset_env(self.env)
)
if self.behaviour_tree.root.status == Status.SUCCESS:
break
if self.blackboard.shared_content['current']['done']:
break
# print(i, 'Inference', self.behaviour_tree.root.status)
# Recursive compute competency for execution nodes
# print(self.behaviour_tree.root.children[0].planner.idata[0])
# print(self.behaviour_tree.root.children[0].planner.tdata[0])
recursive_setup(self.behaviour_tree.root, fn_ecomp, fn_c)
self.trainc = not self.trainc
# Recursive compute competency for execution nodes
recursive_setup(self.behaviour_tree.root, fn_ecomp, fn_c)
print(self.blackboard.shared_content['curve'])
# Recursive compute competency for control nodes
recursive_com(self.behaviour_tree.root, self.blackboard)
def reset_env(self):
self.env.reset()
def check_env_done(self):
self.reset_env()
def save_data(self, env=False):
# Create folder if not exists
import pathlib
import os
dname = os.path.join('/tmp', 'pygoal', 'data', 'experiments')
pathlib.Path(dname).mkdir(parents=True, exist_ok=True)
if env:
fname = os.path.join(dname, self.expname + '_env.png')
img = self.env.render(mode='exp')
plt.imsave(fname, img)
else:
fname = os.path.join(dname, self.expname + '.pkl')
import pickle
pickle.dump(self.blackboard, open(fname, "wb"))
def draw_plot(self, nodenames, root=False, train=True):
curves = []
datas = []
for nname in nodenames:
if train:
datas.append(np.mean(
self.blackboard.shared_content[
'ctdata'][nname], axis=0))
else:
datas.append(np.mean(
self.blackboard.shared_content[
'cidata'][nname], axis=0))
curves.append(
self.blackboard.shared_content['curve'][nname][str(train)])
compare_curve(curves, datas, name=self.expname+str(train), root=root)
def test_pf():
r = ConditionNode('root', None)
r = BehaviourTree(r)
r.tick()
print(r.root.status)
def find_bt(goalspec):
root = goalspec2BT(goalspec, planner=None)
behaviour_tree = BehaviourTree(root)
# display_bt(behaviour_tree, True)
return behaviour_tree
class ControlAutomaton:
"""
Top-level app class providing functionality for running and testing
behaviour trees for drone control via MAVLINK. Includes:
- optional SITL simulation
- optional visualisation of behaviour tree
- connection to MAVLINK
"""
def __init__(self, bt_func, sitl_lat=51.454531, sitl_lon=-2.629158):
"""
Construct an app embodying a given behaviour tree.
Parameters
----------
bt_func : function
Should take a dronekit.Vehicle as an argument and return the root
node of a behaviour tree, as a py_trees.behaviour.Behaviour object.
Typically built using nodes from leaf_nodes and stitched together
using py_trees.composites and py_trees.decorators or using the
flight_idioms.
sitl_lat : float, optional
Latitude for home location if app launched in SITL mode.
The default is 51.454531.
sitl_lon : float, optional
Longitude for home location if app launched in SITL mode.
The default is -2.629158.
Returns
-------
The ControlAutomaton
"""
super(ControlAutomaton, self).__init__()
self._bt_func = bt_func
self._bt = None
self._snapshot_visitor = None
self._app_name = "App name"
self._sitl = None
self._sitl_lat = sitl_lat
self._sitl_lon = sitl_lon
self._connection_string = None
self.vehicle = None
self._loop_should_exit = False
self._max_ticks = 1000
def __del__(self):
self.cleanup()
def startup(self, override_args=None):
"""
Interpret command line arguments, render the tree (if --render)
and connect to the vehicle if necessary (i.e. not in render mode)
Parameters:
override_args : list of str
override command line arguments from function call
e.g. for use in testing (see test_bridge.py)
"""
self._app_name = sys.argv[0]
if override_args:
my_args = [self._app_name]
my_args = my_args + override_args
else:
my_args = sys.argv[:]
if len(my_args) != 2:
print("""Usage:
{0} sitl
run with built-in SITL simulator
{0} render
just render the behaviour tree
{0} <connection string>
connect as prescribed and fly the mission""".format((self._app_name)))
elif my_args[1] == 'sitl':
self._sitl = dronekit_sitl.start_default(lat=self._sitl_lat,
lon=self._sitl_lon)
self._connection_string = self._sitl.connection_string()
print("Using SITL via {}".format(self._connection_string))
self.connect()
elif my_args[1] == 'render':
print("Rendering only")
self.render()
else:
self._connection_string = my_args[1]
print(f"Attempting to connect via {self._connection_string}")
self.connect()
def render(self, file_name=None):
"""
Print the behaviour tree graphically for visualization. Produces
a .dot file (Graphviz), a .png and a .svg (images)
Parameters
----------
file_name : str, optional
File name stub for graphics files produced.
The default is the name of the executed script.
Returns
-------
None.
"""
if file_name is None:
file_name = self._app_name
render_dot_tree(self._bt_func(self.vehicle), name=file_name)
def connect(self):
"""
Connect to the MAVLINK interface, either to own SITL process or to
a specified external connection provided via the command line.
Returns
-------
None.
"""
try:
self.vehicle = connect(self._connection_string, wait_ready=True,
vehicle_class=DroneTreeVehicle)
except socket.error as e:
print(e)
return
self._bt = BehaviourTree(self._bt_func(self.vehicle))
self._snapshot_visitor = SnapshotVisitor()
self._bt.visitors.append(self._snapshot_visitor)
self._bt.visitors.append(DebugVisitor())
def finished(self):
"""
Check if behaviour tree has completed.
Returns
-------
bool
True if root node has status 'SUCCESS' or 'FAILURE'
"""
if self._bt.root.status == Status.SUCCESS:
return True
if self._bt.root.status == Status.FAILURE:
return True
return False
def tick(self):
"""
Execute one tick of the behaviour tree, printing the tree in ASCII
form, plus some simple status information.
Returns
-------
None.
"""
print("******** {} *********".format(self._bt.count))
self._bt.tick()
# print BT
if self._snapshot_visitor:
print(unicode_tree(self._bt.root,
visited=self._snapshot_visitor.visited,
previously_visited=self._snapshot_visitor.visited))
print("+++++++++++++++++++++")
print(self.vehicle.battery)
if self.vehicle.armed:
arm_status = 'ARMED'
else:
arm_status = 'DISARMED'
print(f'Mode: {self.vehicle.mode.name} {arm_status}')
print(f'Altitude: {self.vehicle.location.global_relative_frame.alt}')
print(f'Connected to {self._connection_string}')
print(f'Next waypoint is {self.vehicle.commands.next}')
# exit after timeout or completion
if self.finished():
print("**** Flight completed in {} steps".format(self._bt.count))
self._loop_should_exit = True
if self._bt.count > self._max_ticks:
print("**** Exiting after {} steps".format(self._bt.count))
self._loop_should_exit = True
else:
print("******** {} *********".format(self._bt.count-1))
def cleanup(self):
"""
Close connection to the MAVLINK stream and, if a SITL simulator was
launched, kill its process. Use at end of each test, pass or fail.
Returns
-------
None.
"""
if self.vehicle:
print(f"Disconnecting from vehicle on {self._connection_string}")
self.vehicle.close()
if self._sitl:
print("Shutting down SITL instance")
self._sitl.stop()
def main(self):
"""
Execute the controller application. Process arguments, connect,
run tree until completion or timeout, and tidy up.
NOTE: exit due to keyboard interrupt is trapped and cleanup() called.
Other exceptions may leave hanging threads, connections or SITL
processes. Cleanup is called in destructor to minimize damage.
Returns
-------
None.
"""
self.startup()
if self.vehicle:
try:
while not self._loop_should_exit:
self.tick()
time.sleep(1)
except KeyboardInterrupt:
self.cleanup()
self.cleanup()
def test_comptency():
# import py_trees
# behaviour_tree = BehaviourTree(root)
# # Remember to comment set_state in GenRecProp before
# # running this test case
one = BehaviourTree(Sequence(name=str(1)))
two = Sequence(name=str(2))
three = Sequence(name=str(3))
four = Selector(name=str(4))
five = Sequence(name=str(5))
six = Sequence(name=str(6))
# seven = Parallel(name=str(7))
seven = Selector(name=str(7))
exenodes = [
CompetentNode(name=chr(ord('A') + i), planner=None)
for i in range(0, 11)
]
three.add_children(exenodes[:3])
four.add_children(exenodes[3:6])
six.add_children(exenodes[6:9])
seven.add_children(exenodes[9:])
two.add_children([three, four])
five.add_children([six, seven])
one.root.add_children([two, five])
# py_trees.logging.level = py_trees.logging.Level.DEBUG
# py_trees.display.print_ascii_tree(one.root)
blackboard = Blackboard()
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
env = ReseedWrapper(env, seeds=[3])
env = FullyObsWrapper(env)
env.max_steps = min(env.max_steps, 200)
env.agent_view_size = 1
env.reset()
# env.render(mode='human')
state, reward, done, _ = env.step(2)
# print(state['image'].shape, reward, done, _)
# Find the key
goalspec = 'F P_[KE][1,none,==]'
# keys = ['L', 'F', 'K', 'D', 'C', 'G', 'O']
allkeys = [
'LO', 'FW', 'KE', 'DR', 'BOB', 'BOR', 'BAB', 'BAR', 'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR', 'DO', 'RM'
]
keys = ['LO', 'FW', 'KE']
actions = [0, 1, 2, 3, 4, 5]
def fn_c(child):
pass
def fn_eset(child):
planner = GenRecPropMultiGoal(env,
keys,
goalspec,
dict(),
actions=actions,
max_trace=40,
seed=None,
allkeys=allkeys,
id=child.name)
child.setup(0, planner, True, 50)
def fn_einf(child):
child.train = False
child.planner.epoch = 5
child.planner.tcount = 0
def fn_ecomp(child):
child.planner.compute_competency()
print(child.name,
child.planner.blackboard.shared_content['curve'][child.name])
recursive_setup(one.root, fn_eset, fn_c)
# Train
for i in range(100):
one.tick(pre_tick_handler=reset_env(env))
print(i, 'Training', one.root.status)
# Inference
recursive_setup(one.root, fn_einf, fn_c)
for i in range(5):
one.tick(pre_tick_handler=reset_env(env))
print(i, 'Inference', one.root.status)
recursive_setup(one.root, fn_ecomp, fn_c)
# Manually setting the competency
ckeys = [chr(ord('A') + i) for i in range(0, 11)]
manval = [
np.array([0.84805786, 4.76735384, 0.20430223]),
np.array([0.54378425, 4.26958399, 3.50727315]),
np.array([0.50952059, 5.54225945, 5.28025611])
]
j = 0
for c in ckeys:
blackboard.shared_content['curve'][c] = manval[j % 3]
j += 1
# Recursively compute competency for control nodes
recursive_com(one.root, blackboard)
# print(exenodes[0].planner.blackboard.shared_content['curve'])
# Manually compare the recursively computed competency values
# for the control
# First sub-tree
a = exenodes[0].planner.blackboard.shared_content['curve']['A']
b = exenodes[0].planner.blackboard.shared_content['curve']['B']
c = exenodes[0].planner.blackboard.shared_content['curve']['C']
threec = sequence([a, b, c])
# print('three', threec)
# print(
# 'three', exenodes[0].planner.blackboard.shared_content['curve']['3'])
assert threec == exenodes[0].planner.blackboard.shared_content['curve'][
'3']
# Second sub-tree
d = exenodes[0].planner.blackboard.shared_content['curve']['D']
e = exenodes[0].planner.blackboard.shared_content['curve']['E']
f = exenodes[0].planner.blackboard.shared_content['curve']['F']
fourc = selector([d, e, f])
# print(
# 'four', exenodes[0].planner.blackboard.shared_content['curve']['4'])
assert fourc == exenodes[0].planner.blackboard.shared_content['curve']['4']
# Third sub-tree
g = exenodes[0].planner.blackboard.shared_content['curve']['G']
h = exenodes[0].planner.blackboard.shared_content['curve']['H']
i = exenodes[0].planner.blackboard.shared_content['curve']['I']
sixc = sequence([g, h, i])
# print(
# 'six', exenodes[0].planner.blackboard.shared_content['curve']['6'])
assert sixc == exenodes[0].planner.blackboard.shared_content['curve']['6']
# Fourth sub-tree
j = exenodes[0].planner.blackboard.shared_content['curve']['J']
k = exenodes[0].planner.blackboard.shared_content['curve']['K']
sevenc = selector([j, k])
# print(
# 'seven', exenodes[0].planner.blackboard.shared_content['curve']['7'])
assert sevenc == exenodes[0].planner.blackboard.shared_content['curve'][
'7']
twoc = sequence([threec, fourc])
assert twoc == exenodes[0].planner.blackboard.shared_content['curve']['2']
fivec = sequence([sixc, sevenc])
assert fivec == exenodes[0].planner.blackboard.shared_content['curve']['5']
onec = sequence([twoc, fivec])
assert onec == exenodes[0].planner.blackboard.shared_content['curve']['1']
print(onec)
def find_key():
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
# env = ReseedWrapper(env, seeds=[3]) # Easy
env = ReseedWrapper(env, seeds=[5]) # Medium
# env = ReseedWrapper(env, seeds=[7]) # Hard
env = FullyObsWrapper(env)
env.max_steps = min(env.max_steps, 200)
env.agent_view_size = 1
env.reset()
# env.render(mode='human')
# time.sleep(10)
# state, reward, done, _ = env.step(2)
# print(state['image'].shape, reward, done, _)
# Find the key
goalspec = 'F P_[KE][1,none,==]'
# keys = ['L', 'F', 'K', 'D', 'C', 'G', 'O']
allkeys = [
'LO', 'FW', 'KE', 'DR', 'BOB', 'BOR', 'BAB', 'BAR', 'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR', 'DO', 'RM'
]
keys = ['LO', 'FW', 'KE']
actions = [0, 1, 2]
root = goalspec2BT(goalspec, planner=None, node=CompetentNode)
behaviour_tree = BehaviourTree(root)
child = behaviour_tree.root
planner = GenRecPropMultiGoal(env,
keys,
child.name,
dict(),
actions=actions,
max_trace=50,
seed=None,
allkeys=allkeys)
def run(pepoch=50, iepoch=10):
# pepoch = 50
child.setup(0, planner, True, pepoch)
# Train
for i in range(pepoch):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
# Inference
child.train = False
child.planner.epoch = iepoch
child.planner.tcount = 0
for i in range(iepoch):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
competency = []
epochs = [(80, 10)] * 2
datas = []
for i in range(2):
run(epochs[i][0], epochs[i][1])
datas.append(
np.mean(
planner.blackboard.shared_content['ctdata'][planner.goalspec],
axis=0))
competency.append(planner.compute_competency())
print(competency)
compare_curve(competency, datas)
def carry_key():
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
env = ReseedWrapper(env, seeds=[3])
env = FullyObsWrapper(env)
env.max_steps = min(env.max_steps, 200)
env.agent_view_size = 1
env.reset()
# env.render(mode='human')
state, reward, done, _ = env.step(2)
# Find the key
goalspec = 'F P_[KE][1,none,==] U F P_[CK][1,none,==]'
allkeys = [
'LO', 'FW', 'KE', 'DR', 'BOB', 'BOR', 'BAB', 'BAR', 'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR', 'DO', 'RM'
]
keys = ['LO', 'FW', 'KE', 'CK']
actions = [0, 1, 2, 3, 4, 5]
root = goalspec2BT(goalspec, planner=None, node=CompetentNode)
behaviour_tree = BehaviourTree(root)
epoch = 80
def fn_c(child):
pass
def fn_eset(child):
planner = GenRecPropMultiGoal(env,
keys,
child.name,
dict(),
actions=actions,
max_trace=40,
seed=None,
allkeys=allkeys)
child.setup(0, planner, True, epoch)
def fn_einf(child):
child.train = False
child.planner.epoch = 5
child.planner.tcount = 0
def fn_ecomp(child):
child.planner.compute_competency()
recursive_setup(behaviour_tree.root, fn_eset, fn_c)
# recursive_setup(behaviour_tree.root, fn_c, fn_c)
# py_trees.logging.level = py_trees.logging.Level.DEBUG
# py_trees.display.print_ascii_tree(behaviour_tree.root)
# Train
for i in range(100):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Training', behaviour_tree.root.status)
# Inference
recursive_setup(behaviour_tree.root, fn_einf, fn_c)
for i in range(5):
behaviour_tree.tick(pre_tick_handler=reset_env(env))
print(i, 'Inference', behaviour_tree.root.status)
recursive_setup(behaviour_tree.root, fn_ecomp, fn_c)
# recursive_setup(behaviour_tree.root, fn_c, fn_c)
blackboard = Blackboard()
print(recursive_com(behaviour_tree.root, blackboard))
class MultiGoalGridExp():
def __init__(
self, expname='key', goalspecs='F P_[KE][1,none,==]',
keys=['LO', 'FW', 'KE'], actions=list(range(5)),
seed=None, maxtracelen=40, trainc=False, epoch=80):
env_name = 'MiniGrid-Goals-v0'
env = gym.make(env_name)
if seed is None:
pass
else:
env = ReseedWrapper(env, seeds=[seed])
env = FullyObsWrapper(env)
self.env = env
self.env.max_steps = min(env.max_steps, 200)
# self.env.agent_view_size = 1
self.env.reset()
self.expname = expname
self.goalspecs = goalspecs
self.epoch = epoch
self.maxtracelen = maxtracelen
self.trainc = trainc
self.allkeys = [
'LO', 'FW', 'KE', 'DR',
'BOB', 'BOR', 'BAB', 'BAR',
'LV', 'GO', 'CK',
'CBB', 'CBR', 'CAB', 'CAR',
'DO', 'RM']
self.keys = keys
self.actions = actions
root = goalspec2BT(goalspecs, planner=None, node=CompetentNode)
self.behaviour_tree = BehaviourTree(root)
self.blackboard = Blackboard()
def run(self):
def fn_c(child):
pass
def fn_eset(child):
planner = GenRecPropMultiGoal(
self.env, self.keys, child.name, dict(), actions=self.actions,
max_trace=self.maxtracelen, seed=None, allkeys=self.allkeys)
child.setup(0, planner, True, self.epoch)
def fn_einf(child):
child.train = False
child.planner.epoch = 5
child.planner.tcount = 0
def fn_ecomp(child):
child.planner.compute_competency(self.trainc)
# Save the environment to visualize
# self.save_data(env=True)
# Setup planners
recursive_setup(self.behaviour_tree.root, fn_eset, fn_c)
# import py_trees
# py_trees.logging.level = py_trees.logging.Level.DEBUG
# py_trees.display.print_ascii_tree(self.behaviour_tree.root)
# exit()
# print(dir(self.behaviour_tree.root.children[0]))
# print(self.behaviour_tree.root.children[0].parent.children)
# Train
for i in range(150):
self.behaviour_tree.tick(
pre_tick_handler=self.reset_env(self.env)
)
# print(i, 'Training', self.behaviour_tree.root.status)
# Inference
recursive_setup(self.behaviour_tree.root, fn_einf, fn_c)
for i in range(10):
self.behaviour_tree.tick(
pre_tick_handler=self.reset_env(self.env)
)
# print(i, 'Inference', self.behaviour_tree.root.status)
# Recursive compute competency for execution nodes
recursive_setup(self.behaviour_tree.root, fn_ecomp, fn_c)
# Recursive compute competency for control nodes
recursive_com(self.behaviour_tree.root, self.blackboard)
def reset_env(self, seed=12345):
self.env.reset()
def save_data(self, env=False):
# Create folder if not exists
import pathlib
import os
dname = os.path.join('/tmp', 'pygoal', 'data', 'experiments')
pathlib.Path(dname).mkdir(parents=True, exist_ok=True)
if env:
fname = os.path.join(dname, self.expname + '_env.png')
img = self.env.render(mode='exp')
plt.imsave(fname, img)
else:
fname = os.path.join(dname, self.expname + '.pkl')
import pickle
pickle.dump(self.blackboard, open(fname, "wb"))
def draw_plot(self, nodenames, root=False, train=True):
curves = []
datas = []
for nname in nodenames:
if train:
datas.append(np.mean(
self.blackboard.shared_content[
'ctdata'][nname], axis=0))
else:
datas.append(np.mean(
self.blackboard.shared_content[
'cidata'][nname], axis=0))
curves.append(self.blackboard.shared_content['curve'][nname])
compare_curve(curves, datas, name=self.expname, root=root)
class CompositeMultipleCarry(Behaviour):
"""CompositeMultipleCarry behavior for the agents.
Inherits the Behaviors class from py_trees. This
behavior combines the privitive behaviors to succesfully carry any heavy
carryable object. It combines IsCarrable, IsMultipleCarry
and InitiateMultipleCarry primitive behaviors.
"""
def __init__(self, name):
"""Init method for the CompositeSingleCarry behavior."""
super(CompositeMultipleCarry, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
def setup(self, timeout, agent, item):
"""Have defined the setup method.
This method defines the other objects required for the
behavior. Agent is the actor in the environment,
item is the name of the item we are trying to find in the
environment and timeout defines the execution time for the
behavior.
"""
self.agent = agent
self.item = item
# Root node from the multiple carry behavior tree
root = Sequence("MC_Sequence")
# Conditional behavior to check if the sensed object is carrable or not
carryable = IsCarryable('MC_IsCarryable')
carryable.setup(0, self.agent, self.item)
# Conditional behavior to check if the object is too heavy
# for single carry
is_mc = IsMultipleCarry('MC_IsMultipleCarry')
is_mc.setup(0, self.agent, self.item)
# Check if the object is alread attached to the object
partial_attached = IsInPartialAttached('MC_IsPartialAttached')
partial_attached.setup(0, self.agent, self.item)
# Initiate multiple carry process
initiate_mc_b = InitiateMultipleCarry('MC_InitiateMultipleCarry')
initiate_mc_b.setup(0, self.agent, self.item)
# Selector to select between intiate
# multiple carry and checking strength
initial_mc_sel = Selector("MC_Selector")
initial_mc_sel.add_children([partial_attached, initiate_mc_b])
strength = IsEnoughStrengthToCarry('MC_EnoughStrength')
strength.setup(0, self.agent, self.item)
strength_seq = Sequence("MC_StrenghtSeq")
strength_seq.add_children([strength])
# Main sequence branch where all the multiple carry logic takes place
sequence_branch = Sequence("MC_Sequence_branch")
sequence_branch.add_children([is_mc, initial_mc_sel, strength_seq])
# Main logic behind this composite multiple carry BT
"""
First check if the object is carryable or not. If the object is
carryable then execute the sequence branch. In the sequence branch,
check is the object needs multiple agents to carry. If yes, execute
the initiate multiple carry sequence branch only if it has not been
attached before. Finally, check if there are enought agents/strenght
to lift the object up.
"""
root.add_children([carryable, sequence_branch])
self.behaviour_tree = BehaviourTree(root)
def initialise(self):
"""Everytime initialization. Not required for now."""
pass
def update(self):
"""Just call the tick method for the sequence.
This will execute the primitive behaviors defined in the sequence
"""
self.behaviour_tree.tick()
return self.behaviour_tree.root.status