def createTree():
global cannonAttackingTree
cannonAttackingTree = b3.BehaviorTree()
cannonAttackingTree.id = 'cannonAttackingTree'
node1 = b3.Sequence([
Attack()
])
cannonAttackingTree.root = node1
global cannonDefendingTree
cannonDefendingTree = b3.BehaviorTree()
cannonDefendingTree.id = 'cannonDefendingTree'
node2 = b3.Sequence([
Defend()
])
cannonDefendingTree.root = node2
# global cannonMovingTree
# cannonMovingTree = b3.BehaviorTree()
# cannonMovingTree.id = 'cannonMovingTree'
# node3 = b3.Sequence([
# Move()
# ])
# cannonMovingTree.root = node3
resetBlackboard()
def createTree():
global siegeTowerAttackingTree
siegeTowerAttackingTree = b3.BehaviorTree()
siegeTowerAttackingTree.id = 'siegeTowerAttackingTree'
node1 = b3.Sequence([
IsNotDefeated(),
IsAttacking(),
IsInBattlefield(),
ReadyToShoot(),
SelectPosAndClosestConstruction(),
Attack()
])
siegeTowerAttackingTree.root = node1
# global siegeTowerDefendingTree
# siegeTowerDefendingTree = b3.BehaviorTree()
# siegeTowerDefendingTree.id = 'siegeTowerDefendingTree'
# node2 = b3.Sequence([
# IsDefeated(),
# IsCorrectCommand(),
# IsDefendingLine(),
# IsReloading(),
# AcquireTargetForDefense(),
# Defend()
# ])
# siegeTowerDefendingTree.root = node2
global siegeTowerMovingTree
siegeTowerMovingTree = b3.BehaviorTree()
siegeTowerMovingTree.id = 'siegeTowerMovingTree'
node3 = b3.Sequence([
IsNotDefeated(),
HasValidPath(),
UpdateConstructionStatus(),
b3.Priority([b3.Inverter(HasMoat()), # if not (HasMoat is False):
b3.Sequence([ #
b3.Priority([HasTurtle(), # if (hasTurtle is False):
CreateTurtle()]), # createTurtle
b3.Priority([b3.Inverter(HasTurtle()), # if not (hasTurtle is False):
HandleTurtle()]), # HandleTurtle
]),
]),
b3.Sequence([ReadyToMove(), # if (ReadyToMove is True):
SelectNextCell(), # SelectNextCell
Move() # Move !
])
])
siegeTowerMovingTree.root = node3
resetBlackboard()
def test_populateBlackboard(self):
tree = b3.BehaviorTree()
blackboard = mock.Mock()
target = {}
node = mock.Mock()
def side_effect(tick):
tick._enter_node('node1')
tick._enter_node('node2')
node._execute.side_effect = side_effect
blackboard.get.return_value = []
tree.id = 'tree1'
tree.root = node
tree.tick(target, blackboard)
expected = [mock.call.get('open_nodes', 'tree1')]
result = blackboard.get.mock_calls
self.assertListEqual(result, expected)
expected = [
mock.call.set('open_nodes', ['node1', 'node2'], 'tree1'),
mock.call.set('node_count', 2, 'tree1')
]
result = blackboard.set.mock_calls
self.assertListEqual(result, expected)
def createTree():
global archerAttackingTree
archerAttackingTree = b3.BehaviorTree()
archerAttackingTree.id = 'archerAttackingTree'
node1 = b3.Sequence([
IsDefeated(),
IsCorrectCommand(),
IsInBattlefield(),
IsReloading(),
Attack()
])
archerAttackingTree.root = node1
global archerDefendingTree
archerDefendingTree = b3.BehaviorTree()
archerDefendingTree.id = 'archerDefendingTree'
node2 = b3.Sequence([
IsDefeated(),
IsCorrectCommand(),
IsDefendingLine(),
IsReloading(),
AcquireTargetForDefense(),
Defend()
])
archerDefendingTree.root = node2
global archerMovingTree
archerMovingTree = b3.BehaviorTree()
archerMovingTree.id = 'archerMovingTree'
node3 = b3.Sequence([
IsDefeated(),
IsMoveCommand(),
IsInBattlefield(),
AcquireCandidateCells(),
b3.Sequence([
selectDestinationTarget(),
b3.Priority([
b3.Inverter(doesDestinationNeedCorrection()),
correctDestinationTarget(),
]),
]),
selectFinalDestinationCell(),
Move()
])
archerMovingTree.root = node3
resetBlackboard()
def test_initialization(self):
tree = b3.BehaviorTree()
self.assertIsNotNone(tree.id)
self.assertIsNotNone(tree.title)
self.assertIsNotNone(tree.description)
self.assertIsNone(tree.root)
self.assertDictEqual(tree.properties, {})
def test_sinANDcos(self):
ik_tester = b3.BehaviorTree()
bb = b3.Blackboard()
sc_setup = test_sinandcos_id()
sc_setup.BHdebug = self.DB
sc_id = sinandcos_id()
sc_id.BHdebug = self.DB
sc_sl = sinandcos_solve()
sc_sl.BHdebug = self.DB
asg = assigner()
subtree = b3.Repeater(b3.Sequence([asg, sc_id, sc_sl]), 6)
test = b3.Sequence([sc_setup, subtree])
#test = b3.Sequence([sc_setup, sc_id, sc_sl])
ik_tester.root = test
# run the test BT
ik_tester.tick("test sin and cos solver", bb)
unkns = bb.get("unknowns")
fs = ' sin AND cos solver FAIL'
ntests = 0
for u in unkns:
print u.symbol
print u.solutions
if(u.symbol == th_1):
ntests += 1
self.assertTrue(u.solved, fs)
self.assertTrue(u.solutions[0] == sp.atan2(
l_1, l_2) + sp.atan2(sp.sqrt(l_1**2 + l_2**2 - l_6**2), l_6), fs)
self.assertTrue(u.solutions[1] == sp.atan2(
l_1, l_2) + sp.atan2(-sp.sqrt(l_1**2 + l_2**2 - l_6**2), l_6), fs)
if(u.symbol == th_3):
ntests += 1
self.assertTrue(u.solved, fs)
self.assertTrue(u.solutions[0] == sp.atan2(
l_3, l_3) + sp.atan2(sp.sqrt(2 * l_3**2 - (l_2 - l_4 + 5)**2), l_2 - l_4 + 5), fs)
self.assertTrue(u.solutions[1] == sp.atan2(
l_3, l_3) + sp.atan2(-sp.sqrt(2 * l_3**2 - (l_2 - l_4 + 5)**2), l_2 - l_4 + 5), fs)
if (u.symbol == th_4):
ntests += 1
self.assertTrue(not u.solved, fs + ' [th_4]')
if (u.symbol == th_6):
ntests += 1
self.assertTrue(u.solutions[0] == sp.atan2(l_1 + l_4, l_3) + sp.atan2(
sp.sqrt(-l_2**2 + l_3**2 + (l_1 + l_4)**2), l_2), fs + ' [th_6]')
self.assertTrue(u.solutions[1] == sp.atan2(
l_1 + l_4, l_3) + sp.atan2(-sp.sqrt(-l_2**2 + l_3**2 + (l_1 + l_4)**2), l_2), fs + ' [th_6a]')
self.assertTrue(
ntests == 4, 'sinANDcos_solver: Assert count fail FAIL')
def createTree():
#global infantryAttackingTree
#infantryAttackingTree = b3.BehaviorTree()
#infantryAttackingTree.id = 'infantryAttackingTree'
#node1 = b3.Sequence([
# Attack()
# ])
#infantryAttackingTree.root = node1
# global siegeTowerDefendingTree
# siegeTowerDefendingTree = b3.BehaviorTree()
# siegeTowerDefendingTree.id = 'siegeTowerDefendingTree'
# node2 = b3.Sequence([
# IsDefeated(),
# IsCorrectCommand(),
# IsDefendingLine(),
# IsReloading(),
# AcquireTargetForDefense(),
# Defend()
# ])
# siegeTowerDefendingTree.root = node2
global infantryMovingTree
infantryMovingTree = b3.BehaviorTree()
infantryMovingTree.id = 'infantryMovingTree'
node3 = b3.Sequence([
IsNotDefeated(),
IsOnMoveCommand(),
IfThenElse( IsWaitingForClimbing(), # I should have written this node looooooong before! ;-)
WaitingClimbers(),
HandleClimbing()
),
IfThenElse( IsInBattlefield(), # definitely! ;-)
b3.Sequence([
TraverseCurrCell(),
SelectNextCells(),
IfThenElse(IsCoveringMoat(),
MoveOnMoat(),
b3.Sequence([
Move(),
IfThenElse(ArrivedToWalls(),
IfThenElse(HasGateways(),
ProceedToGateway(),
StartClimbing()
)
)
])
),
]),
),
])
infantryMovingTree.root = node3
resetBlackboard()
def test_callRoot(self):
tree = b3.BehaviorTree()
node = mock.Mock()
blackboard = mock.Mock()
blackboard.get.return_value = []
target = {}
tree.id = 'tree1'
tree.root = node
tree.tick(target, blackboard)
self.assertEqual(node._execute.call_count, 1)
def test_algebra(self):
algebra_tester = b3.BehaviorTree()
bb = b3.Blackboard()
bb.set('Robot', Robot())
setup = test_algebra_id() # see top of this file
aid = algebra_id()
aid.Name = 'Algebra ID'
aid.BHdebug = self.DB
ais = algebra_solve()
ais.Name = 'Algebra Solver'
ais.BHdebug = self.DB
compdet = cpd.comp_det()
compdet.Name = 'Completion Checker'
compdet.BHdebug = self.DB
asgn = assigner()
subtree = b3.Sequence([asgn, aid, ais,compdet])
test = b3.Sequence([setup, b3.Repeater(subtree, max_loop = 5)])
algebra_tester.root = test
# Run the testing BT
algebra_tester.tick("Test the algebra ID/Solver", bb)
# check the results
Tm = bb.get('Tm')
unk = bb.get('unknowns')
fs = ' algebra solver FAIL'
sp.var(' r_13 r_12') # elements of the rotation matrix portion of Td
print '\n\n Results: \n\n'
ntests = 0
for u in unk:
if(u.symbol == d_1):
ntests += 1
self.assertTrue(u.solved, fs)
self.assertTrue(u.nsolutions == 1, fs)
print 'Soln: ', u.solutions[0]
self.assertTrue(u.solutions[0] == (r_13-l_1)/(l_3))
if(u.symbol == th_2):
ntests += 1
self.assertTrue(u.solved, fs)
self.assertTrue(u.nsolutions == 1, fs)
self.assertTrue(u.solutions[0] == r_12-l_1*l_2, fs)
if(u.symbol == th_3):
ntests += 1
self.assertFalse(u.solved, fs)
self.assertTrue(ntests == 3, ' Algebra solver: assertion count error --- FAIL')
print 'Algebra solver PASSED ', ntests, ' assertions.'
def test_load(self):
tree = b3.BehaviorTree()
tree.load(data, names)
self.assertEqual(tree.title, 'Custom Name BT')
self.assertEqual(tree.description, 'description =)')
self.assertEqual(tree.properties['a'], 2)
self.assertNotEqual(tree.root, None)
self.assertEqual(tree.root.name, 'Sequence')
self.assertEqual(tree.root.title, 'Sequence2')
self.assertEqual(tree.root.description, '')
self.assertEqual(len(tree.root.properties), 1)
self.assertEqual(tree.root.properties['b'], 'cv')
self.assertEqual(len(tree.root.children), 2)
def test_closeOpenedNodes(self):
tree = b3.BehaviorTree()
blackboard = mock.Mock()
node1 = mock.Mock()
node2 = mock.Mock()
node3 = mock.Mock()
node4 = mock.Mock()
node5 = mock.Mock()
node6 = mock.Mock()
node7 = mock.Mock()
root = mock.Mock()
def root_side_effect(tick):
tick._enter_node(node1)
tick._enter_node(node2)
tick._enter_node(node3)
root._execute = mock.Mock()
root._execute.side_effect = root_side_effect
def blackboard_side_effect(key, tree_scope=None, node_scope=None):
if key == 'open_nodes':
return [node1, node2, node3, node4, node5, node6, node7]
else:
return 7
blackboard.get.side_effect = blackboard_side_effect
tree.id = 'tree1'
tree.root = root
tree.tick(None, blackboard)
self.assertEqual(node7._close.call_count, 1)
self.assertEqual(node6._close.call_count, 1)
self.assertEqual(node5._close.call_count, 1)
self.assertEqual(node4._close.call_count, 1)
self.assertEqual(node3._close.call_count, 0)
self.assertEqual(node2._close.call_count, 0)
self.assertEqual(node1._close.call_count, 0)
def test_m7(self):
sp.var('Px Py Pz th_1 th_23 th_3 a_3 a_2 d_4')
exp1 = Pz * sp.sin(th_23) + a_2 * sp.cos(th_3) + a_3 + (
-Px * sp.cos(th_1) - Py * sp.sin(th_1)) * sp.cos(th_23)
exp2 = Pz * sp.cos(th_23) - a_2 * sp.sin(th_3) + d_4 + (
Px * sp.cos(th_1) + Py * sp.sin(th_1)) * sp.sin(th_23)
keq1 = kequation(0, exp1)
keq2 = kequation(0, exp2)
one_ls = [keq1, keq2]
uth1 = unknown(th_1)
uth2 = unknown(th_3)
uth23 = unknown(th_23)
unknowns = [uth1, uth2, uth23]
Rob = Robot()
ik_tester = b3.BehaviorTree()
# two equations one unknown,
SimuEqnID = simu_id()
SimuEqnID.Name = 'Simultaneous Eqn ID'
SimuEqnSolve = simu_solver()
SimuEqnSolve.Name = 'Simultaneous Eqn solver'
Simu_Eqn_Sol = b3.Sequence([SimuEqnID, SimuEqnSolve])
ik_tester.root = Simu_Eqn_Sol
bb = b3.Blackboard()
bb.set('curr_unk', uth23)
bb.set('eqns_1u', one_ls)
bb.set('Robot', Rob)
bb.set('unknowns', unknowns)
ik_tester.tick("testing two_eqn_m7", bb)
curr = bb.get('curr_unk')
print curr.solutions
def createTree():
# global throwerAttackingTree
# throwerAttackingTree = b3.BehaviorTree()
# throwerAttackingTree.id = 'throwerAttackingTree'
# node1 = b3.Sequence([
# Attack()
# ])
# throwerAttackingTree.root = node1
global throwerDefendingTree
throwerDefendingTree = b3.BehaviorTree()
throwerDefendingTree.id = 'throwerDefendingTree'
node2 = b3.Sequence([Defend()])
throwerDefendingTree.root = node2
# global cannonMovingTree
# cannonMovingTree = b3.BehaviorTree()
# cannonMovingTree.id = 'cannonMovingTree'
# node3 = b3.Sequence([
# Move()
# ])
# cannonMovingTree.root = node3
resetBlackboard()
def test_subber(self):
sub_tester = b3.BehaviorTree()
bb = b3.Blackboard()
bb.set('Robot', Robot())
setup = test_sub_transform()
trans = sub_transform()
trans.Name = 'Substitution Transf'
trans.BHdebug = True
test = b3.Sequence([setup, trans])
sub_tester.root = test
sub_tester.tick("Test the substitution test tree", bb)
# now examine results
R = bb.get('Robot')
Tm = R.mequation_list[0] # for a single test as above
sp.var('a b c d r_23 r_31 r_33 r_43')
fs = " sub_transform FAIL"
self.assertTrue(Tm.Ts[1, 1] == r_23 + sp.sin(th_5), fs)
self.assertTrue(Tm.Ts[1, 2] == sp.sin(th_1) * sp.cos(th_2), fs)
self.assertTrue(Tm.Ts[2, 1] == d + r_33, fs)
self.assertTrue(Tm.Ts[2, 3] == b * r_31 + c, fs)
self.assertTrue(Tm.Ts[2, 0] == a, fs)
def Mario_agent(sensor, state):
aug_leaf.sensor = sensor
aug_leaf.fire = state
obstacle = 100
enemy = 1
control = {'right': 3, 'fire': 5, 'jump': 4}
tree = b3.BehaviorTree()
#--------------------------C1----------------------#
n0 = mario_state_condition()
n1 = mario_leaf_condition(2, 4, enemy)
n2 = mario_leaf_action(control['fire'])
c1 = b3.Sequence([n0, n1, n2])
#--------------------------C4-----------------------#
n5 = mario_leaf_condition(3, 1, obstacle)
n6 = mario_leaf_condition(3, 2, obstacle)
i1 = b3.Inverter(n5)
i2 = b3.Inverter(n6)
c4 = b3.Sequence([i1, i2])
#--------------------------C3------------------------#
n3 = mario_leaf_condition(2, 2, obstacle)
n4 = mario_leaf_condition(1, 3, obstacle)
c3 = b3.Priority([n3, n4, c4])
#--------------------------C2-----------------------#
n7 = mario_leaf_action(control['jump'])
c2 = b3.Sequence([c3, n7])
#--------------------------Root----------------------#
n8 = mario_leaf_action(control['right'])
root = b3.Priority([c1, c2, n8])
###################################################################
tree.root = root
return tree
def createTeam(firstIndex, secondIndex, isRed,
first='MyAgent', second='MyAgent'):
"""
This function should return a list of two agents that will form the
team, initialized using firstIndex and secondIndex as their agent
index numbers. isRed is True if the red team is being created, and
will be False if the blue team is being created.
As a potentially helpful development aid, this function can take
additional string-valued keyword arguments ("first" and "second" are
such arguments in the case of this function), which will come from
the --redOpts and --blueOpts command-line arguments to capture.py.
For the nightly contest, however, your team will be created without
any extra arguments, so you should make sure that the default
behavior is what you want for the nightly contest.
"""
names = {'Collect Food': CollectFood,
'Return Food': ReturnFood,
'Should Return Food': ShouldReturnFood,
'Ghost Nearby': GhostNearby,
'Capsule Close Enough': CapsuleCloseEnough,
'Collect Capsule': CollectCapsule,
'Be Offensive': BeOffensive,
'Pacman Nearby': PacmanNearby,
'Hunt Pacman': HuntPacman,
'Patrol': Patrol,
}
data = {
"id": "2a6f4e50-f239-431c-8805-5bff0db74283",
"title": "A behavior tree",
"description": "",
"root": "1df39656-d347-4146-8057-b01cfdcb841b",
"properties": {},
"nodes": {
"d0ff0db6-56d3-4845-8bcd-cecf0e17d845": {
"id": "d0ff0db6-56d3-4845-8bcd-cecf0e17d845",
"name": "Ghost Nearby",
"title": "Ghost Nearby",
"description": "",
"properties": {},
"display": {
"x": -1188,
"y": 216
}
},
"5de96d10-0eb2-4c9a-b353-486dbb03a229": {
"id": "5de96d10-0eb2-4c9a-b353-486dbb03a229",
"name": "Return Food",
"title": "Return Food",
"description": "",
"properties": {},
"display": {
"x": -564,
"y": 348
}
},
"ca27a2f4-0c5f-4009-ada4-e6bcc24da81e": {
"id": "ca27a2f4-0c5f-4009-ada4-e6bcc24da81e",
"name": "Collect Food",
"title": "Collect Food",
"description": "",
"properties": {},
"display": {
"x": 60,
"y": 216
}
},
"83425c8e-c0cb-475e-91f1-d1346ddcb9b1": {
"id": "83425c8e-c0cb-475e-91f1-d1346ddcb9b1",
"name": "Should Return Food",
"title": "Should Return Food",
"description": "",
"properties": {},
"display": {
"x": -360,
"y": 348
}
},
"381f7e98-091d-481d-9bbd-2a1809f8fe31": {
"id": "381f7e98-091d-481d-9bbd-2a1809f8fe31",
"name": "Priority",
"title": "Priority",
"description": "Offensive sub-tree",
"properties": {},
"display": {
"x": -528,
"y": -48
},
"children": [
"232f8e3a-ff3c-46cf-9f2e-a7b83a655da8",
"8ccd1db2-a6bf-4a89-851b-2715b025af33"
]
},
"232f8e3a-ff3c-46cf-9f2e-a7b83a655da8": {
"id": "232f8e3a-ff3c-46cf-9f2e-a7b83a655da8",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": -960,
"y": 84
},
"children": [
"d0ff0db6-56d3-4845-8bcd-cecf0e17d845",
"e4dfc945-a0e9-4263-816f-618f4df94cdb"
]
},
"8ccd1db2-a6bf-4a89-851b-2715b025af33": {
"id": "8ccd1db2-a6bf-4a89-851b-2715b025af33",
"name": "Priority",
"title": "Priority",
"description": "",
"properties": {},
"display": {
"x": -96,
"y": 84
},
"children": [
"127e4bb2-e2ac-41be-8a67-d14ced0d7a8c",
"ca27a2f4-0c5f-4009-ada4-e6bcc24da81e"
]
},
"127e4bb2-e2ac-41be-8a67-d14ced0d7a8c": {
"id": "127e4bb2-e2ac-41be-8a67-d14ced0d7a8c",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": -252,
"y": 216
},
"children": [
"83425c8e-c0cb-475e-91f1-d1346ddcb9b1",
"040dad1e-4468-4b6e-92a5-c3a6f620acfc"
]
},
"040dad1e-4468-4b6e-92a5-c3a6f620acfc": {
"id": "040dad1e-4468-4b6e-92a5-c3a6f620acfc",
"name": "Return Food",
"title": "Return Food",
"description": "",
"properties": {},
"display": {
"x": -144,
"y": 348
}
},
"32057ac9-6304-41ec-94f7-9c7babfbdaea": {
"id": "32057ac9-6304-41ec-94f7-9c7babfbdaea",
"name": "Capsule Close Enough",
"title": "Capsule Close Enough",
"description": "",
"properties": {},
"display": {
"x": -984,
"y": 468
}
},
"e4dfc945-a0e9-4263-816f-618f4df94cdb": {
"id": "e4dfc945-a0e9-4263-816f-618f4df94cdb",
"name": "Priority",
"title": "Priority",
"description": "",
"properties": {},
"display": {
"x": -720,
"y": 216
},
"children": [
"bb4ed532-4ce7-4e58-8457-fb65c4c3ee22",
"5de96d10-0eb2-4c9a-b353-486dbb03a229"
]
},
"bb4ed532-4ce7-4e58-8457-fb65c4c3ee22": {
"id": "bb4ed532-4ce7-4e58-8457-fb65c4c3ee22",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": -876,
"y": 348
},
"children": [
"32057ac9-6304-41ec-94f7-9c7babfbdaea",
"bd2fd080-37dd-4a61-b0bb-04ee622535b2"
]
},
"bd2fd080-37dd-4a61-b0bb-04ee622535b2": {
"id": "bd2fd080-37dd-4a61-b0bb-04ee622535b2",
"name": "Collect Capsule",
"title": "Collect Capsule",
"description": "",
"properties": {},
"display": {
"x": -768,
"y": 468
}
},
"1df39656-d347-4146-8057-b01cfdcb841b": {
"id": "1df39656-d347-4146-8057-b01cfdcb841b",
"name": "Priority",
"title": "Priority",
"description": "",
"properties": {},
"display": {
"x": -108,
"y": -312
},
"children": [
"44e4ca83-46ae-4597-8ee9-17f84fe3dc26",
"c27acbb2-4902-4575-823b-01addeffa020"
]
},
"44e4ca83-46ae-4597-8ee9-17f84fe3dc26": {
"id": "44e4ca83-46ae-4597-8ee9-17f84fe3dc26",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": -960,
"y": -180
},
"children": [
"9e4ba8ed-35fb-421c-a1d1-4341bde5f69b",
"381f7e98-091d-481d-9bbd-2a1809f8fe31"
]
},
"9e4ba8ed-35fb-421c-a1d1-4341bde5f69b": {
"id": "9e4ba8ed-35fb-421c-a1d1-4341bde5f69b",
"name": "Be Offensive",
"title": "Be Offensive",
"description": "",
"properties": {},
"display": {
"x": -1392,
"y": -48
}
},
"c27acbb2-4902-4575-823b-01addeffa020": {
"id": "c27acbb2-4902-4575-823b-01addeffa020",
"name": "Priority",
"title": "Priority",
"description": "Defensive sub-tree",
"properties": {},
"display": {
"x": 732,
"y": -180
},
"children": [
"f761a81a-b5a0-432a-bde3-bc047237dda8",
"1b0f4524-f329-4860-84a8-730e921f93b5",
"aeedbe6f-4817-41a5-88df-178017f76171"
]
},
"f761a81a-b5a0-432a-bde3-bc047237dda8": {
"id": "f761a81a-b5a0-432a-bde3-bc047237dda8",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": 372,
"y": -48
},
"children": [
"3c039fca-feb1-474d-8c0f-7bfbcc7ecf4b",
"8cd2f0b6-e6ed-4155-900d-69963aedcd4d"
]
},
"3c039fca-feb1-474d-8c0f-7bfbcc7ecf4b": {
"id": "3c039fca-feb1-474d-8c0f-7bfbcc7ecf4b",
"name": "Pacman Nearby",
"title": "Pacman Nearby",
"description": "",
"properties": {},
"display": {
"x": 264,
"y": 84
}
},
"8cd2f0b6-e6ed-4155-900d-69963aedcd4d": {
"id": "8cd2f0b6-e6ed-4155-900d-69963aedcd4d",
"name": "Hunt Pacman",
"title": "Hunt Pacman",
"description": "",
"properties": {},
"display": {
"x": 480,
"y": 84
}
},
"1b0f4524-f329-4860-84a8-730e921f93b5": {
"id": "1b0f4524-f329-4860-84a8-730e921f93b5",
"name": "Patrol",
"title": "Patrol",
"description": "",
"properties": {},
"display": {
"x": 684,
"y": -48
}
},
"3ab5d788-061d-4c1f-8670-dcbc2bb8e094": {
"id": "3ab5d788-061d-4c1f-8670-dcbc2bb8e094",
"name": "Collect Food",
"title": "Collect Food",
"description": "",
"properties": {},
"display": {
"x": 1308,
"y": 84
}
},
"b4de14f8-7884-4b2d-9924-4e60b0a004ef": {
"id": "b4de14f8-7884-4b2d-9924-4e60b0a004ef",
"name": "Should Return Food",
"title": "Should Return Food",
"description": "",
"properties": {},
"display": {
"x": 888,
"y": 216
}
},
"aeedbe6f-4817-41a5-88df-178017f76171": {
"id": "aeedbe6f-4817-41a5-88df-178017f76171",
"name": "Priority",
"title": "Priority",
"description": "",
"properties": {},
"display": {
"x": 1152,
"y": -48
},
"children": [
"eb8d75b8-2093-4440-8a0f-182eaa2e1428",
"3ab5d788-061d-4c1f-8670-dcbc2bb8e094"
]
},
"eb8d75b8-2093-4440-8a0f-182eaa2e1428": {
"id": "eb8d75b8-2093-4440-8a0f-182eaa2e1428",
"name": "Sequence",
"title": "Sequence",
"description": "",
"properties": {},
"display": {
"x": 996,
"y": 84
},
"children": [
"b4de14f8-7884-4b2d-9924-4e60b0a004ef",
"7afbb7fd-8af9-40c2-8e6f-f0003151de1c"
]
},
"7afbb7fd-8af9-40c2-8e6f-f0003151de1c": {
"id": "7afbb7fd-8af9-40c2-8e6f-f0003151de1c",
"name": "Return Food",
"title": "Return Food",
"description": "",
"properties": {},
"display": {
"x": 1104,
"y": 216
}
}
},
"display": {
"camera_x": 194,
"camera_y": 319,
"camera_z": 0.75,
"x": -108,
"y": -432
},
"custom_nodes": [
{
"name": "Ghost Nearby",
"category": "condition",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Return Food",
"category": "action",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Collect Food",
"category": "action",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Should Return Food",
"category": "condition",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Capsule Close Enough",
"category": "condition",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Collect Capsule",
"category": "action",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Be Offensive",
"category": "condition",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Pacman Nearby",
"category": "condition",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Hunt Pacman",
"category": "action",
"title": None,
"description": None,
"properties": {}
},
{
"name": "Patrol",
"category": "action",
"title": None,
"description": None,
"properties": {}
}
]
}
firstTree = b3.BehaviorTree()
secondTree = b3.BehaviorTree()
firstTree.load(data, names)
secondTree.load(data, names)
# Create agents
firstAgent = eval(first)(firstIndex)
secondAgent = eval(second)(secondIndex)
firstAgent.otherAgent = secondAgent
secondAgent.otherAgent = firstAgent
firstAgent.isOffensive = False
secondAgent.isOffensive = False
firstAgent.tree = firstTree
secondAgent.tree = secondTree
# Blackboard
blackboard = b3.Blackboard()
firstAgent.blackboard = blackboard
secondAgent.blackboard = blackboard
# Debug
debug = False
firstTree.debug = debug
secondTree.debug = debug
# Debugger
if debug:
debugger = Debugger()
firstAgent.debugger = debugger
secondAgent.debugger = debugger
return [firstAgent, secondAgent]
def _load_project(self, data, names=None):
names = names or {}
trees = {}
rospy.loginfo('Creating BehaviorTree objects')
# Create the BehaviorTree objects
for tree in data['trees']:
tmptree = b3.BehaviorTree()
tmptree.id = tree['id'] or tree.id
tmptree.title = tree['title'] or tree.title
tmptree.description = tree['description'] or tmptree.description
tmptree.properties = tree['properties'] or tmptree.properties
trees[tree['id']] = tmptree
names[tree['id']] = tmptree
rospy.loginfo('Populating BehaviorTrees with nodes')
# Create the nodes in each tree
for tree in data['trees']:
rospy.loginfo('Creating nodes for: %s' % tree['title'])
nodes = tree['nodes']
tmpnodes = {}
for key in nodes:
spec = nodes[key]
name = spec['name']
rospy.loginfo('Creating node: %s' % name)
if name in trees:
# Node is a tree
tmpnodes[key] = trees[name]
else:
if name in names:
cls = names[name]
elif hasattr(b3, name):
cls = getattr(b3, name)
else:
rospy.logerror('Invalid node name: %s' % name)
AttributeError(
'BehaviorTree.load_project: Invalid node name "%s"'
% name)
params = spec['properties']
node = cls(**params)
node.id = spec['id'] or node.id
node.title = spec['title'] or node.title
node.description = spec['description'] or node.description
node.properties = spec['properties'] or node.properties
tmpnodes[key] = node
rospy.loginfo('Connecting nodes')
# Connect the nodes
for key in nodes:
spec = nodes[key]
node = tmpnodes[key]
if node.category == b3.COMPOSITE and 'children' in spec:
for cid in spec['children']:
node.children.append(tmpnodes[cid])
elif node.category == b3.DECORATOR and 'child' in spec:
node.child = tmpnodes[spec['child']]
rospy.loginfo('Connecting root node')
# Connect the root node for the tree
trees[tree['id']].root = tmpnodes[tree['root']]
root_tree = trees[data['selectedTree']]
return root_tree
elif arg == 2:
test = "Smart Selector S1: P(s)"
else:
test = "Smart Selector S2: P(s|F)"
print "Starting " + test
#############################################################################
logfile = 'learnExp.csv'
#############################################################################
###### Set up tree
tr1 = b3.BehaviorTree()
if (test == "Dumb Selector"): # argv = 1
A = Rn3()
B = Rn3()
C = Rn3()
# Vanilla selector node
extinguish = b3.Priority([A, B, C])
vict = Rn2() # based on fixed P(s)
save = b3.Succeeder()
saver = b3.Priority([vict, save])
changer = b3.Succeeder()
tr1.root = b3.Sequence([saver, extinguish, changer])
test_name = test
elif (test == "Smart Selector S1: P(s)"): # argv = 2
A = Rn3()
def test_tansolver(self):
ik_tester = b3.BehaviorTree()
tan_setup = test_tan_id()
tanID = tan_id()
tanID.Name = "tan ID"
tanID.BHdebug = False
tanSOL = tan_solve()
tanSOL.BHdebug = False
tanSOL.Name = 'Tangent Solver'
asgn = assigner()
subtree = b3.Sequence([asgn, tanID, tanSOL])
repeats = b3.Repeater(subtree, max_loop=15)
#updateL01.name = 'update Transform and eqn lists'
bb = b3.Blackboard()
ik_tester.root = b3.Sequence([tan_setup, repeats])
sp.var('r_11 r_12 r_13 r_21 r_22 r_23 r_31 r_32 r_33 Px Py Pz'
) # needed for test results
print '\n\n ---------- tan solver TEST 1 --------------\n\n'
bb.set('test_number', 1) # go to test 1
ik_tester.tick("tan_id test", bb)
# Test the results
variables = bb.get('unknowns')
fs = 'tan_solver test 1 FAIL'
ntests = 0
for v in variables:
if (v.symbol == th_5):
self.assertTrue(v.solved == False, fs)
if (self.DB):
print '\n-------------------- ', v.symbol
if (self.DB and v.solved):
sp.pprint(v.solutions[0])
if (v.nsolutions == 2):
sp.pprint(v.solutions[1])
if (v.symbol == th_1):
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_1]')
if (v.symbol == th_2):
ntests += 1
#self.assertTrue(v.solved, fs+' [th_2]')
self.assertTrue(
v.solutions[0] - sp.atan2((r_22 - 15) / l_1,
(r_23 - 99) / l_3) == 0,
fs + ' [th_2]')
if (v.symbol == th_3):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_3]')
sp.pprint(v.solutions[0])
self.assertTrue(
v.solutions[0] == sp.atan2((r_31 - l_2) / l_1,
(r_32 - l_4) / l_3),
fs + ' [th_3]')
if (v.symbol == th_4):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_4]')
self.assertTrue(
v.solutions[0] == sp.atan2(r_13 / (l_1 + l_2), Px / l_3),
fs + ' [th_4]')
if (v.symbol == th_23):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_4]')
self.assertTrue(
v.solutions[0] == sp.atan2(r_33 / (l_5), (Pz - 50) / l_1),
fs + ' [th_23]')
self.assertTrue(ntests == 5, fs + ' assertion count failure ')
print 'Passed: ', ntests, ' asserts'
print '\n\n ---------- tan solver TEST 2 --------------\n\n'
bb2 = b3.Blackboard()
bb2.set('test_number', 2) # go to test 2
ik_tester.tick("tan_id test", bb2)
# Test the results
variables = bb2.get('unknowns')
print '>> Test 2 Asserts'
fs = 'tan_solver test 2 FAIL'
fs2 = 'wrong assumption'
ntests = 0
for v in variables:
if v.solved:
print '\n-------------------- ', v.symbol
sp.pprint(v.solutions[0])
if (v.nsolutions == 2):
sp.pprint(v.solutions[1])
if (v.symbol == th_1):
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_1]')
if (v.symbol == th_2):
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_2]')
self.assertTrue(v.nsolutions == 1, fs + ' [th_2]')
self.assertTrue(
v.solutions[0] == sp.atan2(
(r_22 - 15) / l_1, (r_23 - 99) / l_3), fs + ' [th_2]')
if v.symbol == th_4:
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_4]')
if v.symbol == th_5:
ntests += 1
self.assertTrue(not v.solved, fs + ' [th_5]')
if v.symbol == th_6:
ntests += 1
#self.assertTrue(v.solved, fs + ' [th_6]')
self.assertTrue(v.solutions[0] == sp.atan2(-r_33, r_31), fs)
self.assertTrue(v.solutions[1] == sp.atan2(r_33, -r_31), fs)
print 'Assumptions for ', v.symbol # should set assumptions if canceling an unk.
print ' ', sp.pprint(v.assumption[0])
print ' ', sp.pprint(v.assumption[1])
self.assertTrue(ntests == 5, 'tan_solver: Assert count FAIL')
print 'Passed: ', ntests, ' asserts'
print "global assumptions"
print global_assumptions
def test_updateL(self):
#
# Set up robot equations for further solution by BT
#
# Check for a pickle file of pre-computed Mech object. If the pickle
# file is not there, compute the kinematic equations
#### Using PUMA 560 also tests scan_for_equations() and sum_of_angles_transform() in ik_classes.py
#
# The famous Puma 560 (solved in Craig)
#
import os as os
print '------------'
print os.getcwd()
print '------------'
robot = 'Puma'
[dh, vv, params, unknowns] = robot_params(robot) # see ik_robots.py
[M, R, unk_Puma] = kinematics_pickle(robot, dh, params, vv, unknowns, False)
print 'GOT HERE: updateL robot name: ', R.name
R.name = 'test: '+ robot # ??? TODO: get rid of this (but fix report)
## check the pickle in case DH params were changed
check_the_pickle(M.DH, dh) # check that two mechanisms have identical DH params
testerbt = b3.BehaviorTree()
setup = updateL()
setup.BHdebug = True
bb = b3.Blackboard()
testerbt.root= b3.Sequence([setup]) # this just runs updateL - not real solver
bb.set('Robot',R)
bb.set('unknowns', unk_Puma)
testerbt.tick('test', bb)
L1 = bb.get('eqns_1u')
L2 = bb.get('eqns_2u')
# print them all out(!)
sp.var('Px Py Pz')
fs = 'updateL: equation list building FAIL'
# these self.assertTrues are not conditional - no self.assertTrueion counting needed
self.assertTrue(L1[0].RHS == d_3, fs)
self.assertTrue(L1[0].LHS == -Px*sp.sin(th_1)+Py*sp.cos(th_1), fs)
self.assertTrue(L2[0].RHS == -a_2*sp.sin(th_2)-a_3*sp.sin(th_23)-d_4*(sp.cos(th_23)), fs)
self.assertTrue(L2[0].LHS == Pz, fs)
#########################################
# test R.set_solved
u = unk_Puma[2] # here's what should happen when we set up two solutions
sp.var('Y X B')
u.solutions.append(sp.atan2(Y,X)) # # make up some equations
u.solutions.append(sp.atan2(-Y, X)) #
# assumptions are used when a common denominator is factored out
u.assumption.append(sp.Q.positive(B)) # right way to say "non-zero"?
u.assumption.append(sp.Q.negative(B))
u.nsolutions = 2
u.set_solved(R, unk_Puma) # test the set solved function
fs = 'updateL: testing R.set_solved FAIL '
self.assertTrue(not u.readytosolve, fs)
self.assertTrue( u.solved , fs)
self.assertTrue(R.solveN == 1, fs) # when initialized solveN=0 set_solved should increment it
testing) print 'GOT HERE: robot name: ', R.name R.name = robot R.params = params ## check the pickle in case DH params were changed dhp = M.DH check_the_pickle(dhp, dh) # check that two mechanisms have identical DH params #################################################################################### ## # Set up the BT Leaves # # ikbt = b3.BehaviorTree() LeafDebug = False SolverDebug = False ###add in new nodes:assigner and rank node############# asgn = assigner() asgn.Name = "Assigner" rk = rank() rk.Name = "Rank Node" ####################################################### tanID = tan_id() tanID.Name = 'Tangent ID' tanID.BHdebug = LeafDebug tanSolver = tan_solve()
for unk in unknowns:
if unk.n == 0:
unk.solved = True
th_1_obj = find_obj(th_1, unknowns)
th_1_obj.set_solved(R, unknowns)
th_5_obj = find_obj(th_5, unknowns)
th_5_obj.set_solved(R, unknowns)
tick.blackboard.set('Robot', R)
tick.blackboard.set('unknowns', unknowns)
return b3.SUCCESS
if __name__ == "__main__":
ik_tester = b3.BehaviorTree()
s1 = sum_id()
st1 = test_sum_id()
test = b3.Sequence([st1, s1])
testNum = 1
if testNum == 1:
print '\n\n\n - - - Sum of Angles Test 1 - - -\n\n'
ik_tester.root = test
bb = b3.Blackboard()
bb.set('test_id', 1)
return False
return True
def validsleep(self):
if self.m_MP <= 0:
return True
return False
def sleep(self):
print "sleep"
if not self.m_SleepBeginTime:
self.m_SleepBeginTime = int(time.clock())
iNowTime = int(time.clock())
if iNowTime - self.m_SleepBeginTime >= 4:
self.m_MP = 100
self.m_SleepBeginTime = 0
return False
return True
tree = b3.BehaviorTree()
bb = b3.Blackboard()
aitarget = CAITarget(1001)
tree.root = CSelector()
iBegin = int(time.clock())
iLastTime = int(time.clock())
while iLastTime - iBegin < 100:
tree.tick(aitarget, bb)
iLastTime = int(time.clock())
time.sleep(0.5)
def test_sincos(self):
Rob = Robot()
# test the sincos ID and solver
ik_tester = b3.BehaviorTree()
ik_tester.log_flag = False # log this one
#ik_tester.log_file = open('BT_nodeSUCCESS_log.txt', 'w')
st1 = test_sincos_id()
st1.Name = 'sincos test setup'
sid = sincos_id()
sid.BHdebug = self.DB
sid.Name = "sincos ID"
sis = sincos_solve()
sis.Name = 'sincos Solver'
sis.BHdebug = self.DB
asgn = assigner()
subtree = b3.Sequence([asgn, sid, sis])
repeats = b3.Repeater(subtree, max_loop=5)
test_sincos = b3.Sequence([st1, repeats])
test_sincos.Name = 'overall sincos test'
bb = b3.Blackboard()
bb.set('Robot', Rob)
ik_tester.root = test_sincos
# Off we go: tick the BT ik_tester.root = test_sincos
ik_tester.tick("Test the sincos solver", bb)
# check the results
unks = bb.get('unknowns')
fs = 'acos() solution fail'
ntests = 0
for u in unks:
if (u.symbol == th_1):
self.assertTrue(u.nsolutions == 2, fs + '[th_1 n]')
self.assertTrue(
u.solutions[0] == sp.acos((l_1 - l_5) / (l_2 + l_3 + l_4)),
fs + '[th_1]')
self.assertTrue(
u.solutions[1] == -sp.acos(
(l_1 - l_5) / (l_2 + l_3 + l_4)), fs + '[th_1]')
ntests += 3
if (u.symbol == th_2):
self.assertTrue(u.solutions[0] == sp.acos(l_2),
'acos() solution fail')
self.assertTrue(u.solutions[1] == -sp.acos(l_2),
'acos() solution fail')
ntests += 2
if (u.symbol == th_3):
self.assertTrue(u.solutions[0] == sp.asin(l_1),
'asin() solution fail')
self.assertTrue(u.solutions[1] == -sp.asin(l_1) + sp.pi,
'asin() solution fail')
ntests += 2
if (u.symbol == th_4):
self.assertTrue(u.solutions[0] == sp.acos((l_2 + 5) / l_1),
'acos((a+b)/c) solution fail')
self.assertTrue(u.solutions[1] == -sp.acos((l_2 + 5) / l_1),
'acos((a+b)/c) solution fail')
ntests += 2
self.assertTrue(ntests == 9,
' sincos_solver.py: Assertion count FAIL ')
#test equation lists
L1 = bb.get('eqns_1u')
L2 = bb.get('eqns_2u')
fs = 'sincos: Equation Counts FAIL'
self.assertTrue(len(L1) == 5, fs)
self.assertTrue(len(L2) == 1, fs)
ntests += 2
print '\nPassed all ', ntests, ' asserts.'
time.sleep(2) # for easier reading/ stopping
#
# Look for sum-of-angle equations which can now be solved
# for example: th2 = th23-th3 (where th23 and th3 are known)
#
# this should be algebric solver's work,
# also the set solve here causes problem
# we can set up to succeed when all are done or succeed when more to do.
if (self.FailAllDone):
DONEComplete = b3.FAILURE
DONEIncomplete = b3.SUCCESS
else:
DONEComplete = b3.SUCCESS
DONEIncomplete = b3.FAILURE
if (n == ns):
print ""
print " Solution Complete!!"
print ""
return DONEComplete # we have solved all vars
else:
return DONEIncomplete # we still have unsolved vars
# Self Tests
if __name__ == "__main__":
bb = b3.Blackboard()
up_tester = b3.BehaviorTree()
def test_updateL(self):
#
# Set up robot equations for further solution by BT
#
# Check for a pickle file of pre-computed Mech object. If the pickle
# file is not there, compute the kinematic equations
#### Using PUMA 560 also tests scan_for_equations() and sum_of_angles_transform() in ik_classes.py
#
# The famous Puma 560 (solved in Craig)
#
import os as os
PickleFK = True # True: compute/retrieve FK False: use hard coded equations (not yet workign)
if PickleFK:
print '\n------------'
print 'Current dir: ', os.getcwd()
pickname = 'IKBT/fk_eqns/Puma_pickle.p'
if (os.path.isfile(pickname)):
print 'a pickle file will be used to speed up'
else:
print 'There was no pickle file'
print '------------'
#return [dh, vv, params, pvals, variables]
robot = 'Puma'
[dh, vv, params, pvals,
unknowns] = robot_params(robot) # see ik_robots.py
#def kinematics_pickle(rname, dh, constants, pvals, vv, unks, test):
Test = True
[M, R, unk_Puma] = kinematics_pickle(robot, dh, params, pvals, vv,
unknowns, Test)
print 'Starting Sum of Angle scan/transform'
R.sum_of_angles_transform(unknowns)
print 'Completed Sum of Angles scan/transform'
print 'GOT HERE: updateL robot name: ', R.name
R.name = 'test: ' + robot # ??? TODO: get rid of this (but fix report)
## check the pickle in case DH params were changed
check_the_pickle(
M.DH, dh) # check that two mechanisms have identical DH params
testerbt = b3.BehaviorTree()
setup = updateL()
setup.BHdebug = True
bb = b3.Blackboard()
testerbt.root = b3.Sequence(
[setup]) # this just runs updateL - not real solver
bb.set('Robot', R)
bb.set('unknowns', unk_Puma)
testerbt.tick('test', bb)
L1 = bb.get('eqns_1u')
L2 = bb.get('eqns_2u')
print L2[0].RHS
# print them all out(!)
sp.var('Px Py Pz')
fs = 'updateL: equation list building FAIL'
# these self.assertTrues are not conditional - no self.assertTrueion counting needed
self.assertTrue(L1[0].RHS == d_3, fs)
self.assertTrue(
L1[0].LHS == -Px * sp.sin(th_1) + Py * sp.cos(th_1), fs)
print '-----'
##########################################################################################
# Print out lists L1 and L2 in form of python code to make a new version that will
# not require the painful/slow Puma FK
print 'Code excerpt: (insert at line 124!)'
print 'L1 = []'
print 'l2 = []'
print 'unk_Puma =', unk_Puma
def syconv(s):
a = s
s = s.replace('sin(', 'sp.sin(') # for correct code generation
s = s.replace('cos(', 'sp.cos(')
#print '--->',a , '/', s
return s
for eqn in L1:
s1 = str(eqn.LHS)
s2 = str(eqn.RHS)
s1 = syconv(s1)
s2 = syconv(s2)
print 'L1.append(kequation(' + s1 + ', ' + s2 + '))'
for eqn in L2:
s1 = str(eqn.LHS)
s2 = str(eqn.RHS)
s1 = syconv(s1)
s2 = syconv(s2)
print 'L2.append(kequation(' + s1 + ', ' + s2 + '))'
print '\n End of code generation \n'
if not PickleFK: # generate same equation lists as real FK for Puma
L1 = []
L2 = []
sp.var('Px Py Pz d_3 d_4')
unk_Puma = [th_1, th_2, th_3, th_4, th_5, th_6, th_23]
for i in range(len(unk_Puma)):
unk_Puma[i] = unknown(unk_Puma[i]) # convert these to unknowns
L1.append(kequation(-Px * sp.sin(th_1) + Py * sp.cos(th_1), d_3))
L1.append(
kequation(-Px * sp.sin(th_1) + Py * sp.cos(th_1) - d_3, 0))
L2.append(
kequation(
Pz, -a_2 * sp.sin(th_2) - a_3 * sp.sin(th_23) + d_1 -
d_4 * sp.cos(th_23)))
L2.append(
kequation(
Pz - d_1, -a_2 * sp.sin(th_2) - a_3 * sp.sin(th_23) -
d_4 * sp.cos(th_23)))
fs = 'Sum of Angles Transform (2-way) FAIL'
self.assertTrue(
L2[0].RHS == -a_2 * sp.sin(th_2) - a_3 * sp.sin(th_23) + d_1 -
d_4 * (sp.cos(th_23)), fs)
self.assertTrue(
L2[1].RHS == -a_2 * sp.sin(th_2) - a_3 * sp.sin(th_23) -
d_4 * sp.cos(th_23), fs)
self.assertTrue(L2[0].LHS == Pz, fs)
#########################################
# test R.set_solved
u = unk_Puma[
2] # here's what should happen when we set up two solutions
sp.var('Y X B')
u.solutions.append(sp.atan2(Y, X)) # # make up some equations
u.solutions.append(sp.atan2(-Y, X)) #
# assumptions are used when a common denominator is factored out
u.assumption.append(sp.Q.positive(B)) # right way to say "non-zero"?
u.assumption.append(sp.Q.negative(B))
u.nsolutions = 2
u.set_solved(R, unk_Puma) # test the set solved function
fs = 'updateL: testing R.set_solved FAIL '
self.assertTrue(not u.readytosolve, fs)
self.assertTrue(u.solved, fs)
self.assertTrue(
R.solveN == 1,
fs) # when initialized solveN=0 set_solved should increment it
def test_dump(self):
tree = b3.BehaviorTree()
class Custom(b3.Condition):
title = 'custom'
tree.properties = {
'prop': 'value',
'comp': {
'val1': 234,
'val2': 'value'
}
}
node5 = Custom()
node5.id = 'node-5'
node5.title = 'Node5'
node5.description = 'Node 5 Description'
node4 = b3.Wait()
node4.id = 'node-4'
node4.title = 'Node4'
node4.description = 'Node 4 Description'
node3 = b3.MemSequence([node5])
node3.id = 'node-3'
node3.title = 'Node3'
node3.description = 'Node 3 Description'
node2 = b3.Inverter(node4)
node2.id = 'node-2'
node2.title = 'Node2'
node2.description = 'Node 2 Description'
node1 = b3.Priority([node2, node3])
node1.id = 'node-1'
node1.title = 'Node1'
node1.description = 'Node 1 Description'
node1.properties = {'key': 'value'}
tree.root = node1
tree.title = 'Title in Tree'
tree.description = 'Tree Description'
data = tree.dump()
self.assertEqual(data['title'], 'Title in Tree')
self.assertEqual(data['description'], 'Tree Description')
self.assertEqual(data['root'], 'node-1')
self.assertEqual(data['properties']['prop'], 'value')
self.assertEqual(data['properties']['comp']['val1'], 234)
self.assertEqual(data['properties']['comp']['val2'], 'value')
self.assertNotEqual(data['custom_nodes'], None)
self.assertEqual(len(data['custom_nodes']), 1)
self.assertEqual(data['custom_nodes'][0]['name'], 'Custom')
self.assertEqual(data['custom_nodes'][0]['title'], 'Node5')
self.assertEqual(data['custom_nodes'][0]['category'], b3.CONDITION)
self.assertNotEqual(data['nodes']['node-1'], None)
self.assertNotEqual(data['nodes']['node-2'], None)
self.assertNotEqual(data['nodes']['node-3'], None)
self.assertNotEqual(data['nodes']['node-4'], None)
self.assertNotEqual(data['nodes']['node-5'], None)
self.assertEqual(data['nodes']['node-1']['id'], 'node-1')
self.assertEqual(data['nodes']['node-1']['name'], 'Priority')
self.assertEqual(data['nodes']['node-1']['title'], 'Node1')
self.assertEqual(data['nodes']['node-1']['description'],
'Node 1 Description')
self.assertEqual(data['nodes']['node-1']['children'][0], 'node-3')
self.assertEqual(data['nodes']['node-1']['children'][1], 'node-2')
self.assertEqual(data['nodes']['node-1']['properties']['key'], 'value')
self.assertEqual(data['nodes']['node-2']['name'], 'Inverter')
self.assertEqual(data['nodes']['node-2']['title'], 'Node2')
self.assertEqual(data['nodes']['node-2']['description'],
'Node 2 Description')
self.assertNotEqual(data['nodes']['node-2']['child'], None)
self.assertEqual(data['nodes']['node-3']['name'], 'MemSequence')
self.assertEqual(data['nodes']['node-3']['title'], 'Node3')
self.assertEqual(data['nodes']['node-3']['description'],
'Node 3 Description')
self.assertEqual(len(data['nodes']['node-3']['children']), 1)
self.assertEqual(data['nodes']['node-4']['name'], 'Wait')
self.assertEqual(data['nodes']['node-4']['title'], 'Node4')
self.assertEqual(data['nodes']['node-4']['description'],
'Node 4 Description')
self.assertEqual(data['nodes']['node-5']['name'], 'Custom')
self.assertEqual(data['nodes']['node-5']['title'], 'Node5')
self.assertEqual(data['nodes']['node-5']['description'],
'Node 5 Description')
def test_tick(self):
tree = b3.BehaviorTree()
tree.load(data, names)
blackboard = Blackboard()
tree.tick({}, blackboard)
