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 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_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 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 __init__(self):
healOrBuyHeal = b3.Priority(
[CheckPotionAndConsumeSEQ(),
CheckBuyPotionGoBuySEQ()])
childSeq = b3.Sequence([needsHealthCondition(), healOrBuyHeal])
childList = [childSeq, b3.Succeeder()]
super().__init__(childList)
def __init__(self):
childSeq = b3.Sequence(
[GetNextMove(),
isNextMoveValidCondition(),
ConsumeNextMove()])
childList = [childSeq, b3.Succeeder()]
super().__init__(childList)
def test_initialization(self):
node = b3.Sequence()
self.assertIsNotNone(node.id)
self.assertEqual(node.name, 'Sequence')
self.assertEqual(node.title, 'Sequence')
self.assertIsNotNone(node.description)
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 __init__(self):
isFarFromGoal = b3.Inverter(isNextToGoalCondition())
childSequence = b3.Sequence(
[isFarFromGoal,
WalkToGoalSEQ(),
SetGoalToRessource()])
childList = [childSequence, b3.Succeeder()]
super().__init__(childList)
def __init__(self):
isNotFull = b3.Inverter(isFullCondition())
childSequence = b3.Sequence([
isNotFull,
isGoalLootCondition(),
isNextToGoalCondition(),
MineGoal()
])
childList = [childSequence, b3.Succeeder()]
super().__init__(childList)
def test_success(self):
node1 = get_node(b3.SUCCESS)
node2 = get_node(b3.SUCCESS)
node3 = get_node(b3.SUCCESS)
sequence = b3.Sequence(children=[node1, node2, node3])
status = sequence.tick(TickStub())
self.assertEqual(status, b3.SUCCESS)
self.assertEqual(node1._execute.call_count, 1)
self.assertEqual(node2._execute.call_count, 1)
self.assertEqual(node3._execute.call_count, 1)
def test_running(self):
node1 = get_node(b3.SUCCESS)
node2 = get_node(b3.SUCCESS)
node3 = get_node(b3.RUNNING)
node4 = get_node(b3.SUCCESS)
sequence = b3.Sequence(children=[node1, node2, node3, node4])
status = sequence.tick(TickStub())
self.assertEqual(status, b3.RUNNING)
self.assertEqual(node1._execute.call_count, 1)
self.assertEqual(node2._execute.call_count, 1)
self.assertEqual(node3._execute.call_count, 1)
self.assertEqual(node4._execute.call_count, 0)
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 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)
###### 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()
B = Rn3()
C = Rn3()
# Vanilla selector node
extinguish = SmrtSel01([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 S2: P(s|F)"): # argv = 3
A = Rn3()
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_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.'
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
W2 = sn.walk()
W3 = sn.walk()
A = sn.fightfire()
B = sn.fightfire()
C = sn.fightfire()
fly_to_dest = sn.fly()
if (test == "S0"):
id = "Sel00: Dumb Selector"
extinguish = ss.SmrtSel00([A, B, C])
walker = ss.SmrtSel00([W1, W2, W3])
walk_to_dest = sn.WalkTillDestination(
walker, walktime_arg) # walk till destination or walktime_arg
transport = ss.SmrtSel00([walk_to_dest, fly_to_dest])
#transport = ss.SmrtSel01([walk_to_dest, fly_to_dest])
task = b3.Sequence([transport, extinguish])
tr1.root = b3.Priority([task]) # root doesn't use b
if (test == "S1"):
id = "Sel01: P(s) ranking"
extinguish = ss.SmrtSel01([A, B, C])
walker = ss.SmrtSel01([W1, W2, W3])
walk_to_dest = sn.WalkTillDestination(
walker, walktime_arg) # walk till destination or walktime_arg
transport = ss.SmrtSel01([walk_to_dest, fly_to_dest])
#transport = ss.SmrtSel01([walk_to_dest, fly_to_dest])
task = b3.Sequence([transport, extinguish])
tr1.root = b3.Priority([task]) # root doesn't use baseclass so stub it.
if (test == "S2"): # rebuild tree with smart sel 2
id = "Sel02: P(s|F) ranking"
###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() tanSolver.BHdebug = SolverDebug tanSolver.Name = "Tangent Solver" tanSol = b3.Sequence([tanID, tanSolver]) tanSol.Name = "TanID+Solv" tanSol.BHdebug = LeafDebug algID = algebra_id() algID.Name = "Algebra ID" algID.BHdebug = LeafDebug algSolver = algebra_solve() algSolver.Name = "Algebra Solver" algSolver.BHdebug = LeafDebug algSol = b3.Sequence([algID, algSolver]) algSol.Name = "Algebra ID and Solve" algSol.BHdebug = SolverDebug
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
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)
ik_tester.tick("Test the sum of angle identifier", bb)
unknowns = bb.get("unknowns")
def __init__(self):
isThereNoGoal = b3.Inverter(isThereAGoalCondition())
childSequence = b3.Sequence([isThereNoGoal, SetGoalToRessource()])
childList = [childSequence, b3.Succeeder()]
super().__init__(childList)
def __init__(self):
atkseq = b3.Sequence([CAtkCondition(), CAtkAction()])
eatseq = b3.Sequence([CEatCondition(), CEatAction()])
sleepseq = b3.Sequence([CSleepCondition(), CSleepAction()])
childlist = [atkseq, eatseq, sleepseq]
super(CSelector, self).__init__(childlist)
def __init__(self):
repeatTwiceAttack = b3.Repeater(AttackLastDirection(), 2)
childSeq = b3.Sequence([wasStillCondition(), repeatTwiceAttack])
childList = [childSeq, b3.Succeeder()]
super().__init__(childList)
def __init__(self):
isThereNoPath = b3.Inverter(isThereAPathCondition())
ifNoPathCreateIt = b3.Sequence([isThereNoPath, CreatePath()])
childList = [ifNoPathCreateIt, b3.Succeeder()]
super().__init__(childList)
