def check_conditions(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares): our_position = bombers[player_index]['position'] path_planner = PathPlanner() if path_planner.check_adjacency(map_list, bombers[player_index]['position'], 'BLOCK'): self.moves_we_could_take = [] for possible_move in Directions.values(): possible_x = our_position[0] + possible_move.dx possible_y = our_position[1] + possible_move.dy if map_list[possible_x][possible_y] in WALKABLE: if (path_planner.query_safe_bomb_drop(map_list, bombs, bombers, explosion_list, possible_move, player_index) == True): self.moves_we_could_take.append(possible_move) if len(self.moves_we_could_take) > 0: return True return False
def __init__(self):
print("******Setting Up Behaviours and Weighting Them!******")
self.avoid_death = Avoid_Death_Behaviour(1.0)
self.bomb_a_block = Bomb_A_Block_Behaviour(0.9)
self.open_space_bombing = Open_Space_Bombing_Behaviour(0.8)
self.seek_powerup = Seek_Powerup_Behaviour(0.7)
self.seek_block = Seek_Block_Behaviour(0.6)
self.random_move = Random_Move_Behaviour(0.5)
self.behaviours = [self.avoid_death, self.bomb_a_block, self.random_move, self.open_space_bombing, self.seek_block, self.seek_powerup]
self.map_converter = DangerMap()
self.path_planner = PathPlanner()
def testAllTogether(self):
hippocampus = self.getNode("r_hippo")
ventricles = self.getNode("ventricles")
bloodVessels = self.getNode("vessels")
cortex = self.getNode("cortex")
entries = self.getNode("entries")
targets = self.getNode("targets")
angle = 55
trajectoriesForAllHardConstraints = PathPlanner.applyAllHardConstraints(entries, targets, hippocampus,
ventricles, bloodVessels, cortex, angle)
# PathPlanner.getBestTrajectory(trajectoriesForAllHardConstraints, bloodVessels, 0.01)
self.delayDisplay('testAllTogether passed!')
def testCountRejectedTrajectoriesForBloodVessels(self, entriesAndTargets, bloodVessels, total, printOutput):
startTime = time.time()
filteredForBloodVessels = PathPlanner.getTrajectoriesAvoidingArea(entriesAndTargets, bloodVessels)
endTime = time.time()
totalTrajectoriesFilteringBloodVessels = 0
for _, targetValues in filteredForBloodVessels.items():
totalTrajectoriesFilteringBloodVessels += len(targetValues)
if printOutput:
print('Filtering for blood vessels total time: ', endTime - startTime, 'seconds')
print("Total accepted filtering blood vessels: ", totalTrajectoriesFilteringBloodVessels)
print("Total rejected filtering blood vessels: ", total - totalTrajectoriesFilteringBloodVessels)
self.assertTrue(total - totalTrajectoriesFilteringBloodVessels == 47855)
self.delayDisplay('testCountRejectedTrajectoriesForBloodVessels passed!')
class Open_Space_Bombing_Behaviour(object):
def __init__(self, priority):
self.priority = priority
self.path_planner = PathPlanner()
def check_conditions(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares):
# print("Check: Open Space Bombing")
our_position = bombers[player_index]['position']
adjacency_accumulator = 0
for adjacent_square in Directions.values():
if (adjacent_square.name != 'still'):
adjacent_x = our_position[0] + adjacent_square.dx
adjacent_y = our_position[1] + adjacent_square.dy
if map_list[adjacent_x][adjacent_y] in WALKABLE:
adjacency_accumulator += 1
if adjacency_accumulator == 4 and self.path_planner.is_opponent_accessible(map_list, bombers):
self.moves_we_could_take = []
for possible_move in Directions.values():
possible_x = our_position[0] + possible_move.dx
possible_y = our_position[1] + possible_move.dy
if (self.path_planner.query_safe_bomb_drop(map_list, bombs, bombers, explosion_list, possible_move, player_index) == True):
self.moves_we_could_take.append(possible_move)
if len(self.moves_we_could_take) > 0:
print("Conditions Met: Open Space Bombing")
return True
else:
return False
def take_action(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares):
print("Action: Open Space Bombing")
if len(self.moves_we_could_take) > 0:
return self.moves_we_could_take[random.randrange(0, len(self.moves_we_could_take))].bombaction
def testCountRejectedTrajectoriesForAngle(self, entriesAndTargets, cortex, angle, total, printOutput):
startTime = time.time()
filteredForAngles = PathPlanner.getTrajectoriesWithSpecifiedAngle(entriesAndTargets, cortex, angle)
endTime = time.time()
totalTrajectoriesFilteringAngles = 0
for _, targetValues in filteredForAngles.items():
totalTrajectoriesFilteringAngles += len(targetValues)
if printOutput:
print('Filtering for angles total time: ', endTime - startTime, 'seconds')
print("Total accepted filtering angles: ", totalTrajectoriesFilteringAngles)
print("Total rejected filtering angles: ", total - totalTrajectoriesFilteringAngles)
self.assertTrue(total - totalTrajectoriesFilteringAngles == 17426)
self.delayDisplay('testCountRejectedTrajectoriesForAngle passed!')
def run(self, hippocampus, ventricles, bloodVessels, cortex, entries, targets, angle):
"""
Run the actual algorithm
"""
if not self.isValidInputOutputData(hippocampus, ventricles, bloodVessels, cortex, entries, targets,
angle):
slicer.util.errorDisplay('Invalid input.')
return False
logging.info('Processing started')
# uncomment to see one-by-one
# startTime = time.time()
# PointUtils.getFilteredTargets(targets, hippocampus)
# endTime = time.time()
# print('Filtered targets time: ', endTime - startTime, 'seconds')
#
# entriesAndTargets = PointUtils.getTrajectoryDictionary(entries,
# PointUtils.convertMarkupNodeToPoints(targets))
#
# startTime = time.time()
# PathPlanner.getTrajectoriesAvoidingArea(entriesAndTargets, ventricles)
# endTime = time.time()
# print('Avoid area - Ventricles: ', endTime - startTime, 'seconds')
#
# startTime = time.time()
# PathPlanner.getTrajectoriesAvoidingArea(entriesAndTargets, bloodVessels)
# endTime = time.time()
# print('Avoid area - Blood Vessels: ', endTime - startTime, 'seconds')
#
# startTime = time.time()
# PathPlanner.getTrajectoriesWithSpecifiedAngle(entriesAndTargets, cortex, angle)
# endTime = time.time()
# print('Use only specified angle: ', endTime - startTime, 'seconds')
startTime = time.time()
trajectoriesForAllHardConstraints = PathPlanner.applyAllHardConstraints(entries, targets, hippocampus,
ventricles,
bloodVessels, cortex,
angle)
# finalTrajectories = PathPlanner.getBestTrajectory(trajectoriesForAllConstraints, bloodVessels, 0.01)
endTime = time.time()
print('All together: ', endTime - startTime, 'seconds')
# add to slicer scene to view
self.registerToSlicer(trajectoriesForAllHardConstraints)
logging.info('Processing completed')
return True
def testCountRejectedTrajectoriesCombiningAllHard(self, entries, targets, hippocampus, ventricles, bloodVessels,
cortex,
angle, total, printOutput):
startTime = time.time()
totalTrajectoriesForAllConstraints = PathPlanner.applyAllHardConstraints(entries, targets, hippocampus,
ventricles,
bloodVessels, cortex, angle)
endTime = time.time()
totalTrajectoriesFilteringAllConstraints = 0
for _, targets in totalTrajectoriesForAllConstraints.items():
totalTrajectoriesFilteringAllConstraints += len(targets)
if printOutput:
print('Filtering for all hard constraints total time: ', endTime - startTime, 'seconds')
print("Total accepted filtering all hard constraints: ", totalTrajectoriesFilteringAllConstraints)
print("Total rejected filtering all hard constraints: ", total - totalTrajectoriesFilteringAllConstraints)
self.assertTrue(total - totalTrajectoriesFilteringAllConstraints == 88603)
self.delayDisplay('testCountRejectedTrajectoriesCombiningAll passed!')
def main():
# initialize a map object
map_path = "D://Programming//Seer_Robot//My_Implementation//3_v2.smap"
my_map = Map()
my_map.initialize_map(map_path)
# initialize a path planner by giving it map object and the coordinate of
# the robot in map's original unit. Then start generating spanning tree,
# generating path, and drawing out resulted path graph
planner = PathPlanner(my_map, 20, 10)
planner.spanning_tree()
# planner.show_sp(root)
planner.draw_path()
result = planner.get_path()
show_graph(my_map, result)
def __init__(self, x, y, game, i):
self.id = i
self.position = pygame.math.Vector2(x, y)
self.heading = pygame.math.Vector2(1, 0)
self.facing = pygame.math.Vector2(1, 0)
self.velocity = pygame.math.Vector2(0, 0)
self.force = pygame.math.Vector2(0, 0)
self.mass = 0.02
self.size = 11
self.health = self.maxHealth
self.isAlive = True
self.dead = 0
self.respawnTime = 40
self.fieldOfView = math.radians(180) #from degrees to radians
#self.weapon = [None, None] # railgun, rocket launcher
#self.ammo = [0, 0]
#self.maxAmmo = [20, 40]
self.world = game
self.steeringBehavior = sb.SteeringBehaviors(self)
self.steeringBehaviorIsOn = [
False, False, False, False, False
] # wall avoidance, separation, seek, arrive, wander
self.pathPlanner = pp.PathPlanner(self, self.world.graph)
self.path = []
self.sensoryMemory = sm.SensoryMemory(self,
3000) # 5 - bot_memory_span
self.targetingSystem = ts.TargetingSystem(self)
self.weaponSystem = ws.WeaponSystem(
self, 0.2, 0.1, 1
) # reactionTime = 0.2, aimAccuracy = 0.0 ~ 0.2, aimPersistance = 1
#self.goal = None
self.brain = th.Goal_Think(self)
self.hit = False
self.attacher = None
self.thinkinTime = 10
self.think = 0
class Decider(object):
def __init__(self):
print("******Setting Up Behaviours and Weighting Them!******")
self.avoid_death = Avoid_Death_Behaviour(1.0)
self.bomb_a_block = Bomb_A_Block_Behaviour(0.9)
self.open_space_bombing = Open_Space_Bombing_Behaviour(0.8)
self.seek_powerup = Seek_Powerup_Behaviour(0.7)
self.seek_block = Seek_Block_Behaviour(0.6)
self.random_move = Random_Move_Behaviour(0.5)
self.behaviours = [self.avoid_death, self.bomb_a_block, self.random_move, self.open_space_bombing, self.seek_block, self.seek_powerup]
self.map_converter = DangerMap()
self.path_planner = PathPlanner()
def decide(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number):
"""By default, we will do nothing"""
self.action_to_take_next = Do_Nothing_Behaviour()
"""Set up the DangerMap!"""
danger_map = self.map_converter.convert_to_danger_map(map_list, bombs, explosion_list)
"""Check accessible squares"""
accessible_squares = self.path_planner.query_accessible_squares(map_list, bombers, player_index)
print("******Decision Time!******")
for behaviour in self.behaviours:
if behaviour.check_conditions(map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares) == True and (behaviour.priority > self.action_to_take_next.priority):
self.action_to_take_next = behaviour
print("******Action Time!******")
self.move = self.action_to_take_next.take_action(map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares)
if self.move == None:
self.move = self.random_move.take_action(map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares)
print self.move
return self.move
def testOne():
print('TEST CASE ONE')
GPSdata = [0, 0, 7.19457e-5, 7.19457e-5]
costs = [[1, 1, 9, 7, 7, 9, 8, 8, 9], [7, 1, 1, 9, 8, 9, 7, 9, 9],
[8, 9, 1, 8, 7, 8, 9, 7, 9], [9, 7, 1, 1, 1, 1, 9, 7, 9],
[9, 8, 7, 7, 9, 1, 9, 8, 9], [7, 7, 9, 9, 7, 1, 1, 7, 9],
[9, 8, 9, 7, 9, 1, 1, 7, 9], [9, 8, 9, 7, 8, 1, 7, 7, 7],
[9, 8, 9, 7, 8, 1, 1, 1, 1]]
temp = PathPlanner.PathPlanner(*GPSdata)
temp.updateCostMap(costs)
# Display graph
print('\nCost Map...')
print(" ", end="")
for i in range(len(costs)):
print(i, end=" ")
print()
for i in range(len(costs)):
print(str(i) + "", costs[i])
# Find graph
print('\nTrajectory...')
out = temp.planPath(*GPSdata)
string = ''
for i in range(len(out)):
string += str(out[i]) + ' '
if not i % 8 and i > 0:
string += '\n'
print(string)
return None
class Seek_Block_Behaviour(object):
def __init__(self, priority):
self.priority = priority
self.path_planner = PathPlanner()
def check_conditions(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares):
self.options = self.path_planner.locate_accessible_objects(map_list, accessible_squares, bombers[player_index]['position'], 'BLOCK')
if len(self.options)==0: return False
print("Conditions Met: Seek Block")
return True
def take_action(self, map_list, bombs, powerups, bombers, explosion_list, player_index, move_number, danger_map, accessible_squares):
if len(self.options)>0:
goal = self.options[min(self.options)]
my_pos = bombers[player_index]['position']
path = self.path_planner.A_star(accessible_squares, my_pos, goal)
if len(path) < 2:
print("Error with the path planner for block search.")
return
next_action = path[1]
for move in Directions.values():
if move.name == 'still': pass
x = min(max(my_pos[0] + move.dx,0),MAP_SIZE)
y = min(max(my_pos[1] + move.dy,0),MAP_SIZE)
if (x,y) == next_action:
print("Action: Seek Block")
return move.action
def testTwo():
print('\n\n\n\nTEST CASE TWO')
data = loadFromFile(inputFile)
temp = PathPlanner.PathPlanner(*data[0])
temp.updateCostMap(data[1])
# Time plan path execution
start = time()
out = temp.planPath(*data[0])
end = time()
dt = end - start
print('dt: ', dt)
string = ''
print('\nTrajectory...')
for i in range(len(out)):
string += str(out[i]) + ' '
if not i % 8 and i > 0:
string += '\n'
print(string)
def __init__(self, priority): self.priority = priority self.path_planner = PathPlanner()
def testAvoidVentriclesValidPath(self):
ventricles = self.getNode("ventricles")
entriesAndTargets = {(212.09, 147.385, 76.878): [[162.0, 133.0, 90.0]]}
result = PathPlanner.getTrajectoriesAvoidingArea(entriesAndTargets, ventricles)
self.assertTrue(len(result) > 0)
self.delayDisplay('testAvoidVentriclesValidPath passed!')
def testAvoidBloodVesselsInvalidPath(self):
vessels = self.getNode("vessels")
entriesAndTargets = {(212.09, 147.385, 76.878): [[158.0, 133.0, 82.0]]}
result = PathPlanner.getTrajectoriesAvoidingArea(entriesAndTargets, vessels)
self.assertTrue(len(result) == 0)
self.delayDisplay('testAvoidBloodVesselsInvalidPath passed!')
def testAngleValidPath(self):
cortex = self.getNode("cortex")
entriesAndTargets = {(212.09, 147.385, 76.878): [[162.0, 133.0, 90.0]]}
result = PathPlanner.getTrajectoriesWithSpecifiedAngle(entriesAndTargets, cortex, 55)
self.assertTrue(len(result) > 0)
self.delayDisplay('testAngleValidPath passed!')
def testAngleInvalidPath(self):
cortex = self.getNode("cortex")
entriesAndTargets = {(208.654, 134.777, 61.762): [[162.0, 128.0, 106.0]]}
result = PathPlanner.getTrajectoriesWithSpecifiedAngle(entriesAndTargets, cortex, 55)
self.assertTrue(len(result) == 0)
self.delayDisplay('testAngleInvalidPath passed!')
13: {'pos': (0.9112422509614865, 0.1839028760606296), 'connections': [27, 24, 18, 10]},
14: {'pos': (0.04580558670435442, 0.5886703168399895), 'connections': [33, 30, 16, 5, 8]},
15: {'pos': (0.4582523173083307, 0.1735506267461867), 'connections': [35, 31, 26, 25, 20, 17, 1, 3, 6, 11]},
16: {'pos': (0.12939557977525573, 0.690016328140396), 'connections': [37, 30, 5, 14]},
17: {'pos': (0.607698913404794, 0.362322730884702), 'connections': [34, 31, 28, 26, 25, 18, 0, 1, 10, 12, 15]},
18: {'pos': (0.719569201584275, 0.13985272363426526), 'connections': [31, 27, 26, 25, 24, 1, 10, 13, 17]},
19: {'pos': (0.8860336256842246, 0.891868301175821), 'connections': [21, 2, 4, 7, 9]},
20: {'pos': (0.4238357358399233, 0.026771817842421997), 'connections': [35, 26, 1, 3, 6, 11, 15]},
21: {'pos': (0.8252497121120052, 0.9532681441921305), 'connections': [2, 4, 7, 9, 19]},
22: {'pos': (0.47415009287034726, 0.7353428557575755), 'connections': [39, 37, 29, 7, 12]},
23: {'pos': (0.26253385360950576, 0.9768234503830939), 'connections': [38, 32, 29]},
24: {'pos': (0.9363713903322148, 0.13022993020357043), 'connections': [27, 10, 13, 18]},
25: {'pos': (0.6243437191127235, 0.21665962402659544), 'connections': [34, 31, 27, 26, 1, 10, 15, 17, 18]},
26: {'pos': (0.5572917679006295, 0.2083567880838434), 'connections': [34, 31, 27, 1, 10, 15, 17, 18, 20, 25]},
27: {'pos': (0.7482655725962591, 0.12631654071213483), 'connections': [31, 1, 10, 13, 18, 24, 25, 26]},
28: {'pos': (0.6435799740880603, 0.5488515965193208), 'connections': [39, 36, 34, 31, 0, 12, 17]},
29: {'pos': (0.34509802713919313, 0.8800306496459869), 'connections': [38, 37, 32, 22, 23]},
30: {'pos': (0.021423673670808885, 0.4666482714834408), 'connections': [33, 8, 14, 16]},
31: {'pos': (0.640952694324525, 0.3232711412508066), 'connections': [34, 0, 1, 10, 12, 15, 17, 18, 25, 26, 27, 28]},
32: {'pos': (0.17440205342790494, 0.9528527425842739), 'connections': [38, 5, 23, 29]},
33: {'pos': (0.1332965908314021, 0.3996510641743197), 'connections': [8, 14, 30]},
34: {'pos': (0.583993110207876, 0.42704536740474663), 'connections': [0, 12, 17, 25, 26, 28, 31]},
35: {'pos': (0.3073865727705063, 0.09186645974288632), 'connections': [1, 3, 6, 11, 15, 20]},
36: {'pos': (0.740625863119245, 0.68128520136847), 'connections': [39, 0, 2, 4, 7, 9, 28]},
37: {'pos': (0.3345284735051981, 0.6569436279895382), 'connections': [12, 16, 22, 29]},
38: {'pos': (0.17972981733780147, 0.999395685828547), 'connections': [23, 29, 32]},
39: {'pos': (0.6315322816286787, 0.7311657634689946), 'connections': [2, 4, 7, 22, 28, 36]}
}
planner = PathPlanner.PathPlanner(map_40, 30, 19)
path = planner.bestPath
print(path)
C_base1 = np.arctan2(arm.Cy, arm.Cx) - np.arcsin(
(L2 * np.sin(C_joint1)) / np.sqrt(arm.Cx**2 + arm.Cy**2))
C_joint2 = (2 * np.pi) - C_joint1
C_base2 = np.arctan2(arm.Cy, arm.Cx) - np.arcsin(
(L2 * np.sin(C_joint2)) / np.sqrt(arm.Cx**2 + arm.Cy**2))
#Plan Path Here
waypoints = np.array([
np.array([int(A_base1 / (np.pi / 300)),
int(A_joint1 / (np.pi / 300))]),
np.array([int(B_base1 / (np.pi / 300)),
int(B_joint1 / (np.pi / 300))]),
np.array([int(C_base1 / (np.pi / 300)),
int(C_joint1 / (np.pi / 300))])
])
path1, path2 = plan.get_paths(waypoints, world)
#plan.graph_path(path1, L1, L2)
angles = path1 + path2 #[base_angle, joint_angle]
angles = np.asarray(angles)
numberOfWaypoints = len(angles) # Change this based on your path
crash = np.delete(crash, 0, 0)
plan.graph_path(angles, crash, L1, L2)
pidJoint = Pid(.003, 0, 0.00009)
pidBase = Pid(0.0045, 0.0000015, -0.00025)
arm.reset() # start simulation
for waypoint in range(numberOfWaypoints):