def cozmomain(): goal1 = 'F(P_[DC][True,none,==])' goal2 = 'F(P_[FC][True,none,==])' goal3 = 'F(P_[CC][True,none,==])' goal4 = 'G(P_[CC][True,none,==]) & F(P_[DD][True,none,==])' goal5 = 'F(P_[FD][True,none,==])' goal6 = 'F(P_[CC][False,none,==])' # goal1 = 'F(P_[P][2,none,==])' # goal1 = 'F(P_[P][2,none,==])' goalspec = '((((('+goal1+' U '+goal2+') U '+goal3+') U '+goal4+') U '+goal5+') U '+goal6+')' # noqa: # goalspec = goal+' U '+goal print(goalspec) keys = ['P', 'DC', 'FC', 'CC', 'DD', 'FD', 'D', 'A'] # Pose, Detected Cube, Found Cube, Carried Cube, Detected Desk, Found Desk, Drop Desk # actions = [0, 1, 2, 3, 5] root = goalspec2BT(goalspec, planner=None) behaviour_tree = BehaviourTree(root) # display_bt(behaviour_tree) policies = [ 'detect_cube', 'find_cube', 'carry_cube', 'find_charger', 'move_to_charger', 'drop_cube'] # policies = [detect_cube, find_cube] j = 0 for child in behaviour_tree.root.children: # planner = planners[j] planner = CozmoPlanner(ComplexGoal, keys, child.name, policy=policies[j]) j += 1 child.setup(0, planner, False, 5) for i in range(1): behaviour_tree.tick( pre_tick_handler=reset_env(ComplexGoal) ) print(i, behaviour_tree.root.status)
def taxid(): env = init_taxi_d(seed=1234) target = list(env.decode(env.s)) print(target) goalspec = 'F P_[PI]['+str(3)+',none,==]' keys = ['L', 'PI', 'DI'] actions = [0, 1, 2, 3, 5] root = goalspec2BT(goalspec, planner=None) behaviour_tree = BehaviourTree(root) child = behaviour_tree.root planner = GenRecPropTaxi( env, keys, child.name, dict(), actions=actions, max_trace=5, seed=123) child.setup(0, planner, True, 5) # 4,3,4,3 # print(child.goalspec, child.planner.goalspec, child.planner.env) for i in range(5): behaviour_tree.tick( pre_tick_handler=reset_env_d(env) ) print(i, behaviour_tree.root.status) child.train = False for i in range(1): behaviour_tree.tick( pre_tick_handler=reset_env_d(env) ) print(i, behaviour_tree.root.status)
class TestTaxiInferenceSimpleGoal(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) ) # child.setup(0, planner, True, 40) child.train = False print(child, child.name, child.train) for i in range(1): self.behaviour_tree.tick( pre_tick_handler=reset_env(env) ) print('inference', self.behaviour_tree.root.status) # print(env.curr_loc) def test_inference(self): self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
class TestTaxiInferenceDropP(TestCase): def setUp(self): env = init_taxi_d(seed=1234) target = list(env.decode(env.s)) print(target) goalspec = 'F P_[PI]['+str(3)+',none,==]' keys = ['L', 'PI', 'DI'] actions = [0, 1, 2, 3, 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) # 4,3,4,3 # print(child.goalspec, child.planner.goalspec, child.planner.env) for i in range(5): self.behaviour_tree.tick( pre_tick_handler=reset_env_d(env) ) print(i, self.behaviour_tree.root.status) child.train = False for i in range(1): self.behaviour_tree.tick( pre_tick_handler=reset_env_d(env) ) print(i, self.behaviour_tree.root.status) def test_inference(self): self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
class TestMDPInference(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) child.train = False for i in range(1): self.behaviour_tree.tick( pre_tick_handler=reset_env(env) ) def test_inference(self): self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
def cozmomain2(): goal1 = 'F(P_[DC][True,none,==])' goal2 = 'F(P_[FC][True,none,==])' # goalspec = '((((('+goal+' U '+goal+') U '+goal+') U '+goal+') U '+goal+') U '+goal+')' goalspec = goal1+' U '+goal2 print(goalspec) keys = ['P', 'DC', 'FC', 'CC', 'DD', 'FD', 'D', 'A'] # actions = [0, 1, 2, 3, 5] root = goalspec2BT(goalspec, planner=None) behaviour_tree = BehaviourTree(root) # display_bt(behaviour_tree) # policies = [detect_cube, find_cube, carry_cube, find_charger, move_to_charger, drop_cube] policies = ['detect_cube', 'find_cube'] j = 0 child = behaviour_tree.root for child in behaviour_tree.root.children: # planner = planners[j] planner = CozmoPlanner(ComplexGoal, keys, child.name, policy=policies[j]) j += 1 child.setup(0, planner, False, 5) for i in range(1): behaviour_tree.tick( pre_tick_handler=reset_env(ComplexGoal) ) print(i, behaviour_tree.root.status)
class CompositeSingleCarry(Behaviour): """CompositeSingleCarry behavior for the agents. Inherits the Behaviors class from py_trees. This behavior combines the privitive behaviors to succesfully carry any carryable object. It combines IsCarrable, IsSingleCarry and SingleCarry primitive behaviors. """ def __init__(self, name): """Init method for the CompositeSingleCarry behavior.""" super(CompositeSingleCarry, 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 # First check if the item is carrable? carryable = IsCarryable('SC_IsCarryable_1') carryable.setup(0, self.agent, self.item) # Then check if the item can be carried by a single agent issinglecarry = IsSingleCarry('SC_IsSingleCarry_2') issinglecarry.setup(0, self.agent, self.item) # Finally, carry the object singlecarry = SingleCarry('SC_SingleCarry_3') singlecarry.setup(0, self.agent, self.item) # Define a sequence to combine the primitive behavior sc_sequence = Sequence('SC_SEQUENCE') sc_sequence.add_children([carryable, issinglecarry, singlecarry]) self.behaviour_tree = BehaviourTree(sc_sequence) 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
class MoveAway(Behaviour): """MoveAway behavior for the agents. Inherits the Behaviors class from py_trees. This behavior combines the privitive behaviors GoTo, Away and Move. This allows agents actually to move away the object of interest. """ def __init__(self, name): """Init method for the MoveAway behavior.""" super(MoveAway, 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 # Define goto primitive behavior goto = GoTo('MA_GOTO_1') goto.setup(0, self.agent, self.item) # Define away behavior away = Away('MA_Away_2') away.setup(0, self.agent) # Define move behavior move = Move('MA_MOVE_3') move.setup(0, self.agent) # Define a sequence to combine the primitive behavior mt_sequence = Sequence('MA_SEQUENCE') mt_sequence.add_children([goto, away, move]) self.behaviour_tree = BehaviourTree(mt_sequence) 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
class CompositeSendSignal(Behaviour): """Send signal behavior for the agents. Inherits the Behaviors class from py_trees. This behavior combines the privitive behaviors to succesfully send signals in the environment. It combines SignalDoesNotExists and SendSignal behaviors. """ def __init__(self, name): """Init method for the SendSignal behavior.""" super(CompositeSendSignal, self).__init__(name) self.blackboard = Blackboard() self.blackboard.shared_content = dict() 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('CSS_Sequence') s1 = SignalDoesNotExists('CSS_SignalDoesNotExists') s1.setup(0, self.agent, self.item) s2 = SendSignal('CSS_SendSignal') s2.setup(0, self.agent, self.item) root.add_children([s1, s2]) 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
class CompositeDropPartial(Behaviour): """CompositeDropPartial behavior for the agents. Inherits the Behaviors class from py_trees. This behavior combines the privitive behaviors to succesfully drop any objects carried with cooperation to a dropable surface. It combines IsDropable, IsCarrying and DropPartial primitive behaviors. """ def __init__(self, name): """Init method for the CompositeDrop behavior.""" super(CompositeDropPartial, 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 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 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
class Explore(Behaviour): """Explore behavior for the agents. Inherits the Behaviors class from py_trees. This behavior combines the privitive behaviors to succesfully explore the environment. It combines Randomwalk and Move behaviors. """ def __init__(self, name): """Init method for the Explore behavior.""" super(Explore, self).__init__(name) self.blackboard = Blackboard() self.blackboard.shared_content = dict() 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 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
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 taxi(): 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 # 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], [0, 1, 2, 3], [5]] actions = [0, 1, 2, 3, 4, 5] root = goalspec2BT(goalspec, planner=None) print('root', root) behaviour_tree = BehaviourTree(root) # display_bt(behaviour_tree) epoch = [80, 50, 80, 50] j = 0 for child in 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 # planner.env = env # 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(200): behaviour_tree.tick( pre_tick_handler=reset_env(env) ) print('Training', behaviour_tree.root.status) for child in behaviour_tree.root.children: # child.setup(0, planner, True, 20) child.train = False # print(child, child.name, child.train) for i in range(2): behaviour_tree.tick( pre_tick_handler=reset_env(env) ) print('Inference', behaviour_tree.root.status)
class TestTaxiCompositeGoal1(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,==]' # 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 test_inference(self): self.assertEqual(self.behaviour_tree.root.status, Status.SUCCESS)
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)
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 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)
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)
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 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()
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
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 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)