def forward_search(agent: Agent,
foe: Foe,
reward: Reward,
utility: float,
depth: int = 3,
discount: float = 0.9) -> (str, float):
if depth <= 0:
return ("none", utility)
best_action = ("none", np.NINF)
best_next_action, Up = forward_search(agent, foe, reward, utility,
depth - 1)
utilities = {}
for policy_step in agent.get_available_actions():
utilities[policy_step] = Up + __lookahead(agent, foe, policy_step,
reward, discount)
# print(f'UTILITIES: {utilities}')
max_u_action = max(utilities, key=utilities.get)
best_action = (max_u_action, Up)
turn(agent, best_action[0], foe, "expectation")
return best_action
def moveToBottle_2(currentPosition, bottlePosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
wx = bottlePosition[0]
wy = bottlePosition[1]
angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta,
currentPosition)
turn.turn(math.degrees(angle))
particles = [
updateRotation(particles[i], math.degrees(angle))
for i in range(numberOfParticles)
]
preAngle = interface.getMotorAngle(motors[0])[0]
_gostraight = gostraight.go()
distance, angle = _gostraight.run(30)
aftAngle = interface.getMotorAngle(motors[0])[0]
disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124))
particles = [
update(particles[i], disMoved) for i in range(numberOfParticles)
]
particles = [
updateRotation(particles[i], angle) for i in range(numberOfParticles)
]
return particles
def goback(self, length):
# angle1 = -(length * 0.124) * 3.14159 * (100.0/108.0)+interface.getMotorAngle(motors[0])[0]
# angle2 = -(length * 0.124) * 3.14159 * (100.0/108.0)+interface.getMotorAngle(motors[1])[0]
# interface.setMotorAngleReferences(motors[0:2],[angle1,angle2],[6,6])
angle = (length * 0.124) * 3.14159 * (100.0 / 108.0)
interface.increaseMotorAngleReferences(motors[0:2], [-angle, -angle],
[2, 2])
while not interface.motorAngleReferencesReached(motors[0:2]):
time.sleep(0.1)
tmpDistance = (interface.getMotorAngle(motors[0])[0] -
self.initAngle) / (100.0 / 108.0) / 3.14159 / 0.124
turn.turn(90)
self.touchDetected = False
return tmpDistance
def navigateToWayPoint(wx, wy, currentPosition, particles, sonic, range_index):
Drawer = pds.Canvas()
_gostraight = gostraight.go()
bottle_particles = []
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
#Calculate the distance and Angle to turn
angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta,
currentPosition)
#Let it rotate towards that position
turn.turn(math.degrees(angle))
particles = [
updateRotation(particles[i], math.degrees(angle))
for i in range(numberOfParticles)
]
everymotion = 20.0
stop_times = int(distance / everymotion)
remain_distance = distance - stop_times * everymotion
bottle_particles = []
for index in range(
stop_times): #Every 20 cm, stop and search for the bottle
preAngle = interface.getMotorAngle(motors[0])[0]
_gostraight.run(everymotion)
aftAngle = interface.getMotorAngle(motors[0])[0]
disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124))
particles = [
update(particles[i], disMoved) for i in range(numberOfParticles)
]
particles = mcl(particles)
Drawer.drawParticles(particles)
if index >= 1:
#bottle_particles.append(detect_bottle_2(sonic,getCurrentPosition(particles),bottle_particles))
bottle_particles = detect_bottle(sonic,
getCurrentPosition(particles),
bottle_particles, range_index)
if bottle_particles:
print 'Bottle_position:', bottle_particles
print 'current_Position:', getCurrentPosition(particles)
#Drawer.drawParticles(bottle_particles)
#sorted_d = fuse_information(bottle_particles,range_index)
# particles = moveToBottle(getCurrentPosition(particles),getBottlePosition_2(sorted_d),particles)
particles = moveToBottle_2(getCurrentPosition(particles),
getBottlePosition(bottle_particles), particles)
return particles
def __lookahead(
agent: Agent, # Agent and foe represent the full state
foe: Foe,
policy_step: str,
reward: Reward,
discount: float) -> float:
agent_copy = copy.deepcopy(agent)
foe_copy = copy.deepcopy(foe)
utility = reward.get_reward(agent_copy, foe_copy)
turn(agent_copy, policy_step, foe_copy, "expectation")
return utility + discount * reward.get_reward(agent_copy, foe_copy)
def random_encounter(
agent=Agent,
foe=Foe,
max_turns=int,
) -> pd.DataFrame:
"""
:param agent:
:param foe:
:param max_turns:
:return:
"""
reward = Reward(agent, foe)
utility = reward.get_reward(agent, foe)
# Arrays to hold encounter_stats
agent_policies = []
agent_spell_slots = []
agent_shields = []
agent_healths = []
foe_healths = []
rewards = []
for i in range(max_turns):
agent_policy = agent.random_action()
agent_action, __ = turn(agent, agent_policy, foe)
utility = reward.get_reward(agent, foe)
# Collect turn data into encounter_stats
agent_policies.append(agent_policy)
agent_spell_slots.append(agent.states["spell slots"])
agent_shields.append(agent.states["shield"])
agent_healths.append(agent.hp)
foe_healths.append(foe.hp)
rewards.append(utility)
if agent.hp <= 0 or foe.hp <= 0:
# end encounter if either dies
break
encounter_stats = pd.DataFrame({
"agent actions": agent_policies,
"agent spell slots": agent_spell_slots,
"agent shield": agent_shields,
"agent health": agent_healths,
"foe health": foe_healths,
"reward": rewards,
})
return agent, foe, encounter_stats
def wallCheck(go, _sonnic, _dir):
sonicArr = []
sonicArrAngle = []
sonicArr, _dir = _sonnic.rotateSonar(-3 * math.pi / 4, _dir)
_dir *= -1
zipped = zip(*sonicArr)
min_dist = min(zipped[1])
for son in sonicArr:
if son[1] <= min_dist:
sonicArrAngle.append(son[0])
print str(sonicArr)
minAngle = np.median(sonicArrAngle)
print "mina angle is " + str(minAngle)
angleToTurn = - (90 - minAngle) # in degrees
print "going to turn angle is " + str(-angleToTurn)
turn.turn(-angleToTurn + 90)
distanceToMove = 30 - min_dist
go.run(distanceToMove)
print 'going to move distance: ', distanceToMove
turn.turn(-90)
_dir = _sonnic.rotateSonar(-3 * math.pi / 4, _dir)
def move():
data = bottle.request.json
#TODO: get initial time in millisecond
#print(data)
gameBoard = Board(data['height'], data['width'])
squatchy = None
#initialize the list of opponent snakes
enemies = []
#return the direction to move.
move = turn(data, gameBoard, squatchy, enemies)
#directions = ['up', 'down', 'left', 'right']
return {
#'move': random.choice(directions),
'move': move,
'taunt': 'I ate Bigfoot'
}
def navigateToWayPoint(wx, wy, currentPosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
distance = math.hypot(wx - cx, wy - cy) # This is the main distance
print "navigating to " + str(wx) + "," + str(wy) + " from " + str(currentPosition)
print "distance is " + str(distance)
angle = (math.atan2(wy - cy, wx - cx)) - ctheta # math.atan2 returns in radians
print "angle before is " + str(angle)
if abs(angle) >= math.pi:
if angle > 0:
angle = -((math.pi * 2) - angle)
else:
angle = -((-math.pi * 2) - angle)
print "angle to turn is " + str(angle)
if abs(math.degrees(angle) - 90) < 8:
turn.turn(90)
particles = [updateRotation(particles[i], 90) for i in range(numberOfParticles)]
elif abs(math.degrees(angle) + 90) < 8:
turn.turn(-90)
particles = [updateRotation(particles[i], -90) for i in range(numberOfParticles)]
else:
turn.turn(math.degrees(angle))
particles = [updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles)]
Angle_before = interface.getMotorAngle(0)[0]
_gostraight = gostraight.go()
distanceMoved = _gostraight.run(distance)
print "dist moved: " + str(distanceMoved)
if distanceMoved[1] is 90:
particles = [update(particles[i], distanceMoved[0]) for i in range(numberOfParticles)]
return particles,True
else:
Angle_after = interface.getMotorAngle(0)[0]
d = (Angle_after - Angle_before) / (-math.pi * 0.124)
d = abs(d)
print 'd:', d
particles = [update(particles[i], d) for i in range(numberOfParticles)]
# print "Particles are now " + str(particles)
# update particles
# particles = mcl(particles)
# print "Current Position is " + str(currentPosition)
return particles,False
def turnToWayPoint(wx, wy, currentPosition, particles):
cx = currentPosition[0]
cy = currentPosition[1]
ctheta = math.radians(currentPosition[2]) # We store the theta in degree
distance = math.hypot(wx - cx, wy - cy) # This is the main distance
print "navigating to " + str(wx) + "," + str(wy) + " from " + str(currentPosition)
print "distance is " + str(distance)
angle = (math.atan2(wy - cy, wx - cx)) - ctheta # math.atan2 returns in radians
if abs(angle) >= math.pi:
if angle > 0:
angle = -((math.pi * 2) - angle)
else:
angle = -((-math.pi * 2) - angle)
print "angle to turn is " + str(angle)
if abs(math.degrees(angle) - 90) < 8:
turn.turn(90)
particles = [updateRotation(particles[i], 90) for i in range(numberOfParticles)]
elif abs(math.degrees(angle) + 90) < 8:
turn.turn(-90)
particles = [updateRotation(particles[i], -90) for i in range(numberOfParticles)]
else:
turn.turn(math.degrees(angle))
particles = [updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles)]
# Angle_before = interface.getMotorAngle(0)[0]
# _gostraight = gostraight.go()
# _gostraight.run(distance)
# Angle_after = interface.getMotorAngle(0)[0]
# d = (Angle_after - Angle_before) / (-math.pi * 0.124)
# print 'd:', d
# particles = [update(particles[i], d) for i in range(numberOfParticles)]
# update particles
particles = mcl(particles)
return particles
current_particles = pu.getCurrentPosition(particles)
# ls = prb.LocationSignature()
_dir, readings = prb.characterize_location_for_real(_sonnic, _dir)
#print "Scanned " + str(readings)
scannedHisto = ConvertToHisto(readings)
#print "Got histo " + str(scannedHisto)
# match, _dir = prb.recognize_location(signatures, _dir)
match = signatures.read(index)
histo_match = signatures.read_histogram(index)
angle = prb.findAnomaly(
readings, match
) #, histo_match, scannedHisto) # anomaly in absolute angle (degrees)
#print 'Anomalous angle is at dsfsdfsdf: ', angle
#print 'particlesTurnning is ', current_particles[2]
toturn = angle - current_particles[2]
turn.turn(-toturn)
particles = [
pu.updateRotation(particles[i], toturn)
for i in range(numberOfParticles)
]
ob = gostraight.go()
distance, angle = ob.run(80)
particles = [
pu.updateRotation(particles[i], angle)
for i in range(numberOfParticles)
]
particles = [
pu.update(particles[i], distance) for i in range(numberOfParticles)
]
def encounter(
agent=Agent,
foe=Foe,
max_turns=int,
forward_search_and_reward_kwargs={},
) -> pd.DataFrame:
"""
TODO: document me!!
:param agent:
:param foe:
:param max_turns:
:param forward_search_and_reward_kwargs:
- forward_search_kwargs:
- depth: int = 3,
- reward_kwargs:
- reward_for_kill: float = 1000,
- penalty_for_dying: float = -1000,
- agent_hp_bonus: float = 2,
- foe_hp_bonus: float = -1
:return:
"""
# Handle kwargs
forward_search_kwargs, reward_kwargs = __get_kwargs(
forward_search_and_reward_kwargs)
reward = Reward(agent, foe, **reward_kwargs)
utility = reward.get_reward(agent, foe)
faux_foe = Foe() # The belief state of our foe
# Arrays to hold encounter_stats
agent_policies = []
agent_spell_slots = []
agent_shields = []
agent_healths = []
foe_healths = []
foe_reactions = []
faux_foe_healths = []
forward_search_utilities = []
rewards = []
for i in range(max_turns):
agent_policy, forward_search_utility = forward_search(
agent=copy.deepcopy(agent),
foe=copy.deepcopy(faux_foe),
reward=reward,
utility=utility,
**forward_search_kwargs)
agent_action, foe_reaction = turn(agent, agent_policy, foe)
faux_foe = update_foe_belief(faux_foe, foe_reaction)
utility += reward.get_reward(agent, foe)
# Collect turn data into encounter_stats
agent_policies.append(agent_policy)
agent_spell_slots.append(agent.states["spell slots"])
agent_shields.append(agent.states["shield"])
agent_healths.append(agent.hp)
foe_healths.append(foe.hp)
foe_reactions.append(foe_reaction)
faux_foe_healths.append(faux_foe.hp)
forward_search_utilities.append(forward_search_utility)
rewards.append(utility)
if agent.hp <= 0 or foe.hp <= 0:
# end encounter if either dies
break
encounter_stats = pd.DataFrame({
"agent actions": agent_policies,
"agent spell slots": agent_spell_slots,
"agent shield": agent_shields,
"agent health": agent_healths,
"foe health": foe_healths,
"foe reactions": foe_reactions,
"faux foe health": faux_foe_healths,
"forward search utility": forward_search_utilities,
"utility": rewards,
})
return agent, foe, encounter_stats
def top3(self):
return sorted(set([turn(e) for e in self]))[0:3]
waypointab = (126, 30, 1)
waypointb = (126, 120, 0)
waypointbc = (126, 112, 1)
waypointc = (42, 80, 0)
waypoints = [waypointa, waypointab, waypointb, waypointbc, waypointc]
ob = gostraight.go()
dir = 1
row = 1
noOfRows = 4
halfDistance = 100
while (1):
distance = ob.run(800)
if (row == noOfRows):
ob.goback(halfDistance - 20)
turn.turn(90 * dir)
ob.run(lengthOfPi)
turn.turn(-90 * dir)
ob.run(20)
turn.turn(180)
dir *= -1
row += 1
# at each waypoint do a scan
# go to new location with detected object
# bump
# go to next waypoint
# repeat till last object is bumped
# go back to waypoint 1