def __init__(self, army, number=0):
Battalion.Battalion.__init__(self, army=army, number=number)
self._action = Action.ActionFeatures(
defense=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Defense'),
attack=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Attack'),
speed=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Speed'),
reloadspeed=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Reload'),
accuracy=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Accuracy'),
distance=Battles.Utils.Settings.SETTINGS.Get_F(
'Army', 'Archers', 'Distance'),
stationary=Battles.Utils.Settings.SETTINGS.Get_B(
'Army', 'Archers', 'Stationary'),
movementpriority=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Archers', 'Accuracy'))
self._bounding = Bounding(
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Archers',
'Bounding/Length'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Archers',
'Bounding/Width'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Archers',
'Bounding/Height'))
self._label = "Archers_" + str(Archers.archersCounter)
Archers.archersCounter += 1
blackboard = b3.Blackboard()
archerBlackboards[self._label] = blackboard
def __init__(self, army, number = 0):
Battalion.Battalion.__init__(self, army = army, number = number)
self._action = Action.ActionFeatures(defense = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'Defense'),
attack = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'Attack'),
speed = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'Speed'),
reloadspeed = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'Reload'),
accuracy = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'Accuracy'),
distance = Battles.Utils.Settings.SETTINGS.Get_F('Army', 'SiegeTowers', 'Distance'),
stationary = Battles.Utils.Settings.SETTINGS.Get_B('Army', 'SiegeTowers', 'Stationary'),
movementpriority = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'SiegeTowers', 'MovementPriority'))
self._bounding = Bounding(Battles.Utils.Settings.SETTINGS.Get_F('Army', 'SiegeTowers', 'Bounding/Length'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'SiegeTowers', 'Bounding/Width'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'SiegeTowers', 'Bounding/Height'))
self._label = "SiegeTowers_" + str(SiegeTowers.siegetowersCounter)
SiegeTowers.siegetowersCounter += 1
#print 'SiegeTowers ->', SiegeTowers.siegetowersCounter
self._targetWall = None
self._path = []
self._status = SiegeTowers.CONSTRUCTING
self.__army = None
self._moatCells = []
self._turtle = None
self._nLevels = 0
self._constructionStatus = 0
self._coverMoatHeight = 0
self._nArchersPerLevel = (self._bounding.length * self._bounding.width) / (Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Archers', 'Bounding/Length') * Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Archers', 'Bounding/Width'))
blackboard = b3.Blackboard()
siegeTowerBlackboards[self._label] = blackboard
def test_treeMemoryInitialization(self):
blackboard = b3.Blackboard()
blackboard.set('var1', 'value', 'tree1')
self.assertIsNotNone(blackboard.get('var1', 'tree1'))
self.assertIsNotNone(blackboard.get('node_memory', 'tree1'))
self.assertIsNotNone(blackboard.get('open_nodes', 'tree1'))
def test_basicReadAndWrite(self):
blackboard = b3.Blackboard()
blackboard.set('var1', 'string')
blackboard.set('var2', 100)
self.assertEqual(blackboard.get('var1'), 'string')
self.assertEqual(blackboard.get('var2'), 100)
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 test_readAndWriteWithinTreeScope(self):
blackboard = b3.Blackboard()
blackboard.set('var1', 'string', 'tree1')
blackboard.set('var2', 100, 'tree2')
self.assertEqual(blackboard.get('var1', 'tree1'), 'string')
self.assertEqual(blackboard.get('var2', 'tree2'), 100)
self.assertEqual(blackboard.get('var1', 'tree2'), None)
self.assertEqual(blackboard.get('var2', 'tree1'), None)
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_readAndWriteWithinNodeScope(self):
blackboard = b3.Blackboard()
blackboard.set('var1', 'value 1', 'tree 1')
blackboard.set('var2', 'value 2', 'tree 1', 'node 1')
blackboard.set('var3', 'value 3', 'tree 1', 'node 2')
blackboard.set('var4', 1000, 'tree 2')
self.assertEqual(blackboard.get('var2', 'tree 1', 'node 1'), 'value 2')
self.assertEqual(blackboard.get('var3', 'tree 1', 'node 2'), 'value 3')
self.assertEqual(blackboard.get('var2', 'tree 1', 'node 2'), None)
self.assertEqual(blackboard.get('var3', 'tree 1', 'node 1'), None)
self.assertEqual(blackboard.get('var2', 'tree 1'), None)
self.assertEqual(blackboard.get('var1', 'tree 1', 'node 1'), None)
self.assertEqual(blackboard.get('var2', 'tree 2', 'node 1'), None)
def __init__(self, army, number = 0):
Battalion.Battalion.__init__(self, army = army, number = number)
self._action = Action.ActionFeatures( defense = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'Defense'),
attack = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'Attack'),
speed = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'Defense'),
reloadspeed = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'Reload'),
accuracy = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'Accuracy'),
distance = Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'Distance'),
climbspeed = Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'ClimbSpeed'),
stationary = Battles.Utils.Settings.SETTINGS.Get_B('Army', 'Infantry', 'Stationary'),
movementpriority = Battles.Utils.Settings.SETTINGS.Get_I('Army', 'Infantry', 'MovementPriority'))
self._bounding = Bounding( Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'Bounding/Length'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'Bounding/Width'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'Bounding/Height'))
self._label = "Infantry_" + str(Infantry.infantryCounter)
Infantry.infantryCounter += 1
#print 'Infantry ->', Infantry.infantryCounter,
self._action.SetRubbleClimgingSpeed(Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Infantry', 'RubbleClimbSpeed'))
blackboard = b3.Blackboard()
infantryBlackboards[self._label] = blackboard
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 __init__(self, army, number=0):
Battalion.Battalion.__init__(self, army=army, number=number)
self._action = Action.ActionFeatures(
defense=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'Defense'),
attack=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'Attack'),
speed=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'Speed'),
reloadspeed=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'Reload'),
accuracy=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'Accuracy'),
distance=Battles.Utils.Settings.SETTINGS.Get_F(
'Army', 'Throwers', 'Distance'),
stationary=Battles.Utils.Settings.SETTINGS.Get_B(
'Army', 'Throwers', 'Stationary'),
movementpriority=Battles.Utils.Settings.SETTINGS.Get_I(
'Army', 'Throwers', 'MovementPriority'))
self._bounding = Bounding(
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Throwers',
'Bounding/Length'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Throwers',
'Bounding/Width'),
Battles.Utils.Settings.SETTINGS.Get_F('Army', 'Throwers',
'Bounding/Height'))
self._label = "Throwers_" + str(Throwers.throwersCounter)
Throwers.throwersCounter += 1
# print 'Throwers ->', Throwers.throwersCounter
self._action.SetInitialReload(False)
self._stair = None
self._dissolvedBattalions = []
blackboard = b3.Blackboard()
throwerBlackboards[self._label] = blackboard
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 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
scSol.BHdebug = F
scID.BHdebug = F
scSolver.BHdebug = F
x2z2_Solver.BHdebug = T
sumOfAnglesT.BHdebug = F
compDetect.BHdebug = F
compDetect.FailAllDone = F # set it up to SUCCEED when there is more work to do. (not default)
algID.BHdebug = F
algSolver.BHdebug = F
tanSol.BHdebug = F
#
# Set up the blackboard for solution
#
bb = b3.Blackboard()
## Generate the lists of soln candidate equations from the matrix equations
[L1, L2,
L3p] = R.scan_for_equations(unknowns) # lists of 1unk and 2unk equations
bb.set('eqns_1u', L1) # eqns with one unk
bb.set('eqns_2u', L2) # two unks
bb.set('eqns_3pu', L3p) # three or more unks
R.sum_of_angles_transform(unknowns) #get the sum of angle
bb.set('Robot', R)
bb.set('unknowns', unknowns)
################################################################################
#
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")
print "----------"
print "test results"
print "found sum of angles: "
for unkn in unknowns:
if unkn.n == 0:
print unkn
print "new T matrix"
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 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 __init__(self, start_running=False):
rospy.init_node(NAME)
rospy.loginfo('Initializing: %s' % NAME)
bt_pub_name = '/cyborg/bt/behavior_tree'
self.bt_pub = rospy.Publisher(bt_pub_name,
BehaviorTree,
latch=True,
queue_size=1)
bt_update_pub_name = '/cyborg/bt/behavior_tree_updates'
self.bt_update_pub = rospy.Publisher(bt_update_pub_name,
BehaviorTreeNodes,
latch=True,
queue_size=1)
bt_enabled_pub_name = '/cyborg/bt/enabled'
self.bt_enabled_pub = rospy.Publisher(bt_enabled_pub_name,
Bool,
latch=True,
queue_size=1)
self.enabled = True
bt_enable_srv_name = '/cyborg/bt/enable'
rospy.Service(bt_enable_srv_name, SetBool, self._enable_cb)
names = {'MoveTo': MoveTo}
with open('/home/mortenmj/.ros/project.json') as f:
rospy.loginfo('Loading project from %s' % f.name)
data = json.load(f, object_pairs_hook=OrderedDict)
self.root = self._load_project(data, names)
if not self.root:
raise AttributeError
# Publish Behavior Tree structure
self._publish_bt(self.root)
self.target = None
self.blackboard = b3.Blackboard()
self._prev_open_nodes = list()
# Set known locations
locations = {
'el5': [-33.768, -33.545, 0],
'waitingarea': [-33.581, 10.627, 0],
'info': [-33.505, 1.139, 0],
'cafeteria': [-33.075, -55.776, 0],
'stairs': [-31.494, -71.056, 0],
'el6': [-30.397, -12.826, 0],
'hallway': [-30.251, -79.089, 0],
'home': [-29.552, 8.747, 0],
'elevator1': [-29.414, -50.281, 0],
'underbridge': [-28.161, -63.308, 0],
'elevator2': [-21.912, -42.451, 0],
'entrance': [-18.422, 6.518, 0],
'entrance2': [-18.246, -65.898, 0]
}
self.blackboard.set('locations', locations)
rospy.loginfo('Running %s' % self.root)
self.enabled = start_running
if start_running:
self._publish_bt_status()
self.run()