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_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_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 __init__(self):
repeatTwiceAttack = b3.Repeater(AttackLastDirection(), 2)
childSeq = b3.Sequence([wasStillCondition(), repeatTwiceAttack])
childList = [childSeq, b3.Succeeder()]
super().__init__(childList)
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.'