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)