def tree(blackboard):
# ----------------------------------------------
wait_for_goal = Sequence(u'wait for goal')
wait_for_goal.add_child(TestBehavior(u'tf'))
wait_for_goal.add_child(TestBehavior(u'js1'))
wait_for_goal.add_child(TestBehavior(u'pybullet updater'))
wait_for_goal.add_child(TestBehavior(u'has goal'))
wait_for_goal.add_child(TestBehavior(u'js2'))
# ----------------------------------------------
planning_3 = PluginBehavior(u'planning III', sleep=0)
planning_3.add_plugin(TestBehavior(u'coll'))
planning_3.add_plugin(TestBehavior(u'controller'))
planning_3.add_plugin(TestBehavior(u'kin sim'))
planning_3.add_plugin(TestBehavior(u'log'))
planning_3.add_plugin(TestBehavior(u'goal reached'))
planning_3.add_plugin(TestBehavior(u'wiggle'))
planning_3.add_plugin(TestBehavior(u'time'))
# ----------------------------------------------
publish_result = failure_is_success(Selector)(u'monitor execution')
publish_result.add_child(TestBehavior(u'goal canceled'))
publish_result.add_child(TestBehavior(u'send traj'))
# ----------------------------------------------
# ----------------------------------------------
planning_2 = failure_is_success(Selector)(u'planning II')
planning_2.add_child(TestBehavior(u'goal canceled'))
planning_2.add_child(success_is_failure(TestBehavior)(u'visualization'))
planning_2.add_child(success_is_failure(TestBehavior)(u'cpi marker'))
planning_2.add_child(planning_3)
# ----------------------------------------------
move_robot = failure_is_success(Sequence)(u'move robot')
move_robot.add_child(TestBehavior(u'execute?'))
move_robot.add_child(publish_result)
# ----------------------------------------------
# ----------------------------------------------
planning_1 = success_is_failure(Sequence)(u'planning I')
planning_1.add_child(TestBehavior(u'update constraints'))
planning_1.add_child(planning_2)
# ----------------------------------------------
# ----------------------------------------------
process_move_goal = failure_is_success(Selector)(u'process move goal')
process_move_goal.add_child(planning_1)
process_move_goal.add_child(TestBehavior(u'set move goal'))
# ----------------------------------------------
#
post_processing = failure_is_success(Sequence)(u'post processing')
post_processing.add_child(TestBehavior(u'wiggle_cancel_final_detection'))
post_processing.add_child(TestBehavior(u'post_processing'))
# ----------------------------------------------
# ----------------------------------------------
root = Sequence(u'root')
root.add_child(wait_for_goal)
root.add_child(TestBehavior(u'cleanup'))
root.add_child(process_move_goal)
root.add_child(TestBehavior(u'plot trajectory'))
root.add_child(post_processing)
root.add_child(move_robot)
root.add_child(TestBehavior(u'send result'))
tree = BehaviourTree(root)
return tree
def __init__(self, name):
"""Init method for the Until sub-tree.
Until sub-tree has following structure
Sequence
- Selector
-p_2
-\\phi_1
- Sequence
-p_1
-\\phi_2
"""
super(UntilNode, self).__init__(name)
self.blackboard = Blackboard()
self.blackboard.shared_content = dict()
# Define a sequence to combine the primitive behavior
root = Sequence('U')
selec = Selector('Se')
p2 = DummyNode('p2')
p1 = DummyNode('p1')
goal1 = DummyNode('g1')
goal2 = DummyNode('g2')
selec.add_children([p2, goal1])
seq = Sequence('S')
seq.add_children([p1, goal2])
root.add_children([selec, seq])
self.bt = BehaviourTree(root)
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 create_until_node(a, b):
root = Sequence('Seq')
selec = Selector('Se')
p2 = ConditionNode(str(b.id), b)
p1 = ConditionNode(str(a.id), a)
goal1 = a
goal2 = b
selec.add_children([p2, goal1])
seq = Sequence('S')
seq.add_children([p1, goal2])
root.add_children([seq, selec])
return root
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]
def find_control_node(operator):
# print(operator, type(operator))
if operator in ['U']:
# sequence
control_node = Sequence(operator)
elif operator == '&':
# parallel
control_node = Sequence('and')
# control_node = Deco
# elif operator == '|':
# # Selector
# control_node = Selector(operator)
# else:
# # decorator
# control_node = Selector(operator)
return control_node
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 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 pre_flight(vehicle):
bt_pf = FailureIsRunning(
Sequence(
name="Pre-flight",
children=[lf.CheckMode(vehicle, "GUIDED"),
lf.IsArmed(vehicle)]))
return (bt_pf)
def move_behaviour(vehicle, dNorth, dEast, dDown):
bt_move = Sequence(name="move",
children=[
lf.MoveDrone(vehicle, dNorth, dEast, dDown),
FailureIsRunning(
Inverter(lf.LatSpeedUnder(vehicle, 1.0))),
FailureIsRunning(lf.LatSpeedUnder(vehicle, 0.1))
])
return (bt_move)
def behaviour_tree(vehicle):
bt = OneShot(
Sequence(name="Flight",
children=[
pre_flight(vehicle),
lf.SimpleTakeoff(vehicle, 20),
FailureIsRunning(lf.AltLocalAbove(vehicle, 18)),
move_behaviour(vehicle, 50, 0, 0),
move_behaviour(vehicle, 0, 50, 0),
move_behaviour(vehicle, -50, 0, 0),
move_behaviour(vehicle, 0, -50, 0),
lf.Land(vehicle),
FailureIsRunning(Inverter(lf.AltLocalAbove(vehicle, 0.3)))
]))
return (bt)
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('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)
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("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 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)
