def addWall(self, lopt, hipt):
xlo, ylo = lopt
xhi, yhi = hipt
# walls are defined by two points
self.walls.append((util.Point(xlo, ylo), util.Point(xhi, yhi)))
# also store representation as line segments
self.wall_segs.append(
util.LineSeg(util.Point(xlo, ylo), util.Point(xhi, yhi)))
def get_path(worldname, maze):
if worldname == 'dl12World':
#pass # your code here
path = search(make_maze_successors(maze), maze.start, lambda x: x == maze.goal, False, True)
ans = []
for p in path:
ans.append(maze.indices_to_point((p[0], p[1])))
return ans
else:
return [util.Point(0.911250, 0.911250), util.Point(1.721250, 0.506250), util.Point(2.531250, 1.316250), util.Point(1.721250, 1.721250), util.Point(0.911250, 2.126250), util.Point(1.721250, 2.936250), util.Point(2.531250, 2.531250)]
def planner(initialPose, goalPoint, worldPath, gridSquareSize):
"""
A planner method which implements A^* search to find optimal paths for the
robot to move among states in the discrete map of the world
"""
goalList = list(goalPoint.xyTuple())
initialList = list(initialPose.xytTuple()[:2])
gm = basicGridMap.BasicGridMap(worldPath, gridSquareSize)
goalIndices = gm.pointToIndices(goalPoint)
(initialX, initialY) = initialPose.xytTuple()[:2]
iI = util.Point(initialX, initialY)
initialIndices = gm.pointToIndices(iI)
def g(s):
gm.drawSquare(s, 'gray')
return s == goalIndices # goalTest = lambda x: x == goalTuple)
def heuristic(s):
return ((goalIndices[0] - s[0])**2 + (goalIndices[1] - s[1])**2)**(0.5)
path = ucSearch.smSearch(GridDynamics(gm),
initialState=initialIndices,
goalTest=lambda x: g(x),
heuristic=lambda x: heuristic(x))
print('path = ', path)
pathDrawing = []
for element in path:
loe = list(element)
pathDrawing.append(loe[1])
gm.drawPath(pathDrawing)
return path
def plot_soar_world_dw(path_to_world,
plot_win=None,
linestyle='k',
title=None,
windowSize=600):
body = ast.parse(open(path_to_world).read()).body
walls = []
offset = (0, 0)
dim = (0, 0)
for elt in body:
if isinstance(elt, ast.Expr) and isinstance(elt.value, ast.Call):
#everything we care about falls here (i think)
if isinstance(elt.value.func, ast.Name):
args = tuple(pythonic_from_ast(i) for i in elt.value.args)
if elt.value.func.id == 'initial_robot_loc':
offset = args
elif elt.value.func.id == 'wall':
walls.append(args)
elif elt.value.func.id == 'dimensions':
dim = args
# Robot's location needs to be (0, 0)
# In the world definition, y decreases from the top
xmin = -offset[0]
xmax = xmin + dim[0]
ymin = -offset[1]
ymax = ymin + dim[1]
print('x, y', (xmin, xmax), (ymin, ymax))
print('window size', windowSize, int(windowSize * float(dim[1]) / dim[0]))
print('dim', dim)
if plot_win is None:
if title is None:
title = "Soar World Plot: {}, -- {}".\
format(os.path.basename(path_to_world),
datetime.now().strftime("%b %d, '%y; %I:%M:%S %p"))
eps = 0.1 * dim[0]
plot_win = dw.DrawingWindow(windowSize,
int(windowSize * float(dim[1]) / dim[0]),
xmin - eps,
xmax + eps,
ymin - eps,
ymax + eps,
title=title)
plot_win.drawLineSeg(xmin, ymin, xmin, ymax)
plot_win.drawLineSeg(xmin, ymax, xmax, ymax)
plot_win.drawLineSeg(xmax, ymax, xmax, ymin)
plot_win.drawLineSeg(xmax, ymin, xmin, ymin)
#now add in walls
for wall in walls:
plot_win.draw_path([xmin + i[0] for i in wall],
[ymin + i[1] for i in wall],
color='black')
return (plot_win, util.Point(offset[0], offset[1]))
def sonar_hit(distance, sonar_pose, robot_pose):
"""
@param distance: distance along ray that the sonar hit something
@param sonar_pose: C{util.Pose} of the sonar on the robot
@param robot_pose: C{util.Pose} of the robot in the global frame
@return: C{util.Point} representing position of the sonar hit in the
global frame.
"""
return robot_pose.transformPoint(sonar_pose.transform_point(\
util.Point(distance,0)))
class SoarWorld:
"""
Represents a world in the same way as the soar simulator
"""
def __init__(self, path):
"""
@param path: String representing location of world file
"""
self.walls = []
"""
Walls represented as list of pairs of endpoints
"""
self.wallSegs = []
"""
Walls represented as list of C{util.lineSeg}
"""
# set the global world
global world
world = self
# execute the file for side effect on world
execfile(path)
# put in the boundary walls
(dx, dy) = self.dimensions
wall((0, 0), (0, dy))
wall((0, 0), (dx, 0))
wall((dx, 0), (dx, dy))
wall((0, dy), (dx, dy))
def initialLoc(self, x, y):
# Initial robot location
self.initialRobotLoc = util.Point(x, y)
def dims(self, dx, dy):
# x and y dimensions
self.dimensions = (dx, dy)
def addWall(self, (xlo, ylo), (xhi, yhi)):
# walls are defined by two points
self.walls.append((util.Point(xlo, ylo), util.Point(xhi, yhi)))
# also store representation as line segments
self.wallSegs.append(
util.LineSeg(util.Point(xlo, ylo), util.Point(xhi, yhi)))
def on_step():
x, y, theta = io.get_position(cheat=True)
robot.slime_x.append(x)
robot.slime_y.append(y)
checkoff.update(globals())
# the following lines compute the robot's current position and angle
current_point = util.Point(x,y).add(robot.initial_location)
current_angle = theta
forward_v, rotational_v = drive(robot.path, current_point, current_angle)
io.set_forward(forward_v)
io.set_rotational(rotational_v)
def getPath(worldname, maze):
if worldname == 'dl9World':
goal_test = lambda (r, c): (r, c) == maze.goal
return search(maze_successors(maze), maze.start, goal_test)
else:
return [
util.Point(0.911250, 0.911250),
util.Point(1.721250, 0.506250),
util.Point(2.531250, 1.316250),
util.Point(1.721250, 1.721250),
util.Point(0.911250, 2.126250),
util.Point(1.721250, 2.936250),
util.Point(2.531250, 2.531250)
]
def testMove():
target = util.Point(1.0, 0.5)
sonars = [0.0] * 8
poseList = [
util.Pose(0, 0, 0),
util.Pose(0, 0, math.pi / 2),
util.Pose(0, 0, math.atan2(0.5, 1)),
util.Pose(1.0001, 0.499999, 0)
]
moveTestInput = [(target, io.FakeSensorInput(sonars, pose)) \
for pose in poseList]
mover = dynamicMoveToPointSkeleton.DynamicMoveToPoint()
result = mover.transduce(moveTestInput, check=True)
print 'The actual inputs are (target, sensorInput) pairs; here we'
print 'are just showing the odometry part of the sensorInput.'
for ((goal, sensors), o) in zip(moveTestInput, result):
print '\nInput:', (goal, sensors.odometry)
print 'Output:', o
class DynamicRobotMaze(GraphicsMaze):
def __init__(self, height, width, x0, y0, x1, y1):
GraphicsMaze.__init__(self, height, width) #do not remove
self.x0,self.x1 = x0,x1
self.y0,self.y1 = y0,y1
self.width, self.height = width, height
self.grid = [[True for c in xrange(width)] for r in xrange(height)]
def pointToIndices(self, point):
ix = int(math.floor((point.x-self.x0)*self.width/(self.x1-self.x0)))
iix = min(max(0,ix),self.width-1)
iy = int(math.floor((point.y-self.y0)*self.height/(self.y1-self.y0)))
iiy = min(max(0,iy),self.height-1)
return ((self.height-1-iiy,iix))
def indicesToPoint(self, (r,c)):
x = self.x0 + (c+0.5)*(self.x1-self.x0)/self.width
y = self.y0 + (self.height-r-0.5)*(self.y1-self.y0)/self.height
return util.Point(x,y)
def step():
global i
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
currentPoint = util.Point(x, y).add(robot.initialLocation)
currentAngle = theta
destinationPoint = robot.path[i]
thetad = currentPoint.angleTo(destinationPoint)
if util.nearAngle(currentAngle, thetad, math.pi / 180.0):
io.setForward(0.1)
io.setRotational(0)
if currentPoint.distance(destinationPoint) < 0.02:
i += 1
print i
else:
theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle)
io.setRotational(theta_constant)
io.setForward(0)
class RobotMaze(Maze):
def __init__(self, mapText, x0, y0, x1, y1):
Maze.__init__(self, mapText)
self.x0 = x0
self.y0 = y0
self.x1 = x1
self.y1 = y1
def pointToIndices(self, point):
ix = int(math.floor((point.x-self.x0)*self.width/(self.x1-self.x0)))
iix = min(max(0,ix),self.width-1)
iy = int(math.floor((point.y-self.y0)*self.height/(self.y1-self.y0)))
iiy = min(max(0,iy),self.height-1)
return ((self.height-1-iiy,iix))
def indicesToPoint(self, (r,c)):
x_index = (float(c)/(self.width)) * (self.x1 - self.x0) + self.x0
y_index = (float(self.height - r - 1)/(self.height)) * (self.y1 - self.y0) + self.y0
x = x_index + self.cellWidth() / 2.0
y = y_index + self.cellHeight() / 2.0
return util.Point(x, y)
class RobotMaze(Maze):
def __init__(self, mapText, x0, y0, x1, y1):
Maze.__init__(self, mapText) #run Maze's __init__ on this instance
self.x0 = float(x0)
self.y0 = float(y0)
self.x1 = float(x1)
self.y1 = float(y1)
def pointToIndices(self, point):
ix = int(
math.floor((point.x - self.x0) * self.width / (self.x1 - self.x0)))
iix = min(max(0, ix), self.width - 1)
iy = int(
math.floor(
(point.y - self.y0) * self.height / (self.y1 - self.y0)))
iiy = min(max(0, iy), self.height - 1)
return ((self.height - 1 - iiy, iix))
def indicesToPoint(self, (r, c)):
x = self.x0 + (c + 0.5) * (self.x1 - self.x0) / self.width
y = self.y0 + (self.height - r - 0.5) * (self.y1 -
self.y0) / self.height
return util.Point(x, y)
def planner(initialPose, goalPoint, worldPath, gridSquareSize): goalList = list(goalPoint.xyTuple()) initialList = list(initialPose.xytTuple()[:2]) gm = basicGridMap.BasicGridMap(worldPath,gridSquareSize) goalIndices = gm.pointToIndices(goalPoint) (initialX,initialY) = initialPose.xytTuple()[:2] iI = util.Point(initialX,initialY) initialIndices = gm.pointToIndices(iI) def g(s): gm.drawSquare(s,'gray') return s == goalIndices #goalTest = lambda x: x == goalTuple) def heuristic(s): return ((goalIndices[0]-s[0])**2.+(goalIndices[1]-s[1])**2)**(0.5) # ucSearch.somewhatVerbose = True path = ucSearch.smSearch(GridDynamics(gm),initialState = initialIndices, goalTest = lambda x: g(x),heuristic = lambda x: heuristic(x)) print "path = ", path pathDrawing = [] for element in path: loe = list(element) pathDrawing.append(loe[1]) gm.drawPath(pathDrawing) return path
def step():
if len(robot.path) == 0:
io.setForward(0)
io.setRotational(0)
return
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
currentPoint = util.Point(x,y).add(robot.initialLocation)
currentAngle = theta
destinationPoint = robot.maze.indicesToPoint(robot.path[0])
desiredTheta = math.atan2(float(destinationPoint.y - currentPoint.y), float(destinationPoint.x - currentPoint.x))
if desiredTheta < 0:
desiredTheta += math.pi * 2
if util.nearAngle(desiredTheta, theta, 0.1):
io.setForward(0.3)
io.setRotational(0)
else:
io.setForward(0)
if desiredTheta > theta:
if desiredTheta - theta < math.pi:
io.setRotational(.5)
else:
io.setRotational(-.5)
else:
if desiredTheta - theta < math.pi:
io.setRotational(-.5)
else:
io.setRotational(.5)
if closeToPoint(currentPoint, destinationPoint, 0.1):
if len(robot.path) > 0:
robot.path.pop(0)
import math
import lib601.ucSearch as ucSearch
import lib601.util as util
import lib601.basicGridMap as basicGridMap
import lib601.gridMap as gridMap
import lib601.sm as sm
######################################################################
### Picking worlds
######################################################################
mapTestWorld = ['mapTestWorld.py', 0.2, util.Point(2.0, 5.5),
util.Pose(2.0, 0.5, 0.0)]
bigPlanWorld = ['bigPlanWorld.py', 0.25, util.Point(3.0, 1.0),
util.Pose(1.0, 1.0, 0.0)]
class GridDynamics(sm.SM):
legalInputs = ['u','ur','r','dr','d','dl','l','ul']
def __init__(self, theMap):
pass
def nextState(self,state,inp):
nextState = list(state)
if inp == 'u':
nextState[1] += 1
cost = 1.
elif inp == 'ur':
nextState[0] += 1
nextState[1] += 1
cost = 2.**(0.5)
elif inp == 'r':
def drive(path, pos, angle):
"""
this function should return a tuple (fv, rv), where
fv is the forward velocity the robot should use on this step, and
rv is the rotational velocity the robot should use on this step
in order to follow the desired path
pos is the robot's current position, as an instance of util.Point
angle is the robot's current angle in radians
"""
# will follow the suggested algorithm:
# * turn towards the new point without moving forward
# * move forward until close enough
global loc_in_path
global turning
# if we made it to the end..
if loc_in_path == len(path):
return (0, 0)
if pos.distance(path[loc_in_path]) < 0.1:
turning = True
loc_in_path += 1
return (0, 0)
copy_pos = None
copy_next = None
difference = 100000
theta_d = None
if turning:
k_a = -2
# not sure if calling methods on Points alters the original.. I think it does?
copy_pos = util.Point(pos.xy_tuple()[0], pos.xy_tuple()[1])
copy_next = util.Point(path[loc_in_path].xy_tuple()[0],
path[loc_in_path].xy_tuple()[1])
copy_next = copy_next.add(copy_pos.scale(-1)).scale(-1)
## GOD EXPLAIN CLEARLY HOW ANGLE_TO WORKS!!!! I WASTED SO MUCH TIME FIGURING THIS OUT -_-
theta_d = copy_next.angle_to(util.Point(0, 0))
difference = (angle_difference(angle, theta_d)) #%math.pi
if abs(difference) < 0.01:
turning = False
#print("TURNING FALSE")
fv = 0
rv = 0
else:
rv = k_a * difference
fv = 0
else:
k_d = 1
distance = pos.distance(path[loc_in_path])
# checks for closeness to goal at the start of this function. Don't need to check here
fv = k_d #*distance
rv = 0
#print("turning " + str(turning))
#print("distance " + str(pos.distance(path[loc_in_path])))
#print("angle difference " + str(difference))
#print("theta_d " + str(theta_d))
#print("robot angle " + str(angle))
#print("going to " + str(path[loc_in_path].xy_tuple()))
#print(str(copy_pos))#.xy_tuple()))
#print(str(copy_next))#.xy_tuple()))
#print()
return (fv, rv)
import math
import lib601.ucSearch as ucSearch
import lib601.util as util
import lib601.basicGridMap as basicGridMap
import lib601.gridMap as gridMap
import lib601.sm as sm
######################################################################
### Picking worlds
######################################################################
mapTestWorld = [
'mapTestWorld.py', 0.2,
util.Point(2.0, 5.5),
util.Pose(2.0, 0.5, 0.0)
]
bigPlanWorld = [
'bigPlanWorld.py', 0.25,
util.Point(3.0, 1.0),
util.Pose(1.0, 1.0, 0.0)
]
class GridDynamics(sm.SM):
'''
total are 8 directions around the square
'''
legalInputs = [
'up', 'down', 'right', 'left', 'upLeft', 'upRight', 'downLeft',
'downRight'
import lib601.io as io
import lib601.util as util
import math
import ffSkeleton
reload(ffSkeleton)
# Robot is trying to drive in a rotated square
points = [
util.Point(0.5, 0.5),
util.Point(0.0, 1.0),
util.Point(-0.5, 0.5),
util.Point(0.0, 0.0)
]
def testFF():
sonars = [0.0] * 8
poseList = [
util.Pose(0, 0, 0),
util.Pose(0, 1, 0),
util.Pose(0.499, 0.501, 2),
util.Pose(2, 3, 4)
]
ffTestInput = [io.FakeSensorInput(sonars, pose) for pose in poseList]
targetGenerator = ffSkeleton.FollowFigure(points)
result = targetGenerator.transduce(ffTestInput, check=True)
print 'The actual inputs are whole instances of io.SensorInput; here we'
print 'are just showing the odometry part of the input.'
for (i, o) in zip(poseList, result):
print '\nInput:', i
def indices_to_point(self, loc):
(r, c) = loc
x = self.x0 + (c + 0.5) * (self.x1 - self.x0) / self.width
y = self.y0 + (self.height - r - 0.5) * (self.y1 -
self.y0) / self.height
return util.Point(x, y)
# instead of yours.
import mapMakerSkeleton as mapMaker
reload(mapMaker)
# Parameters in motion controller
move.MoveToDynamicPoint.forwardGain = 1.0
move.MoveToDynamicPoint.rotationGain = 1.0
move.MoveToDynamicPoint.angleEps = 0.05
move.MoveToDynamicPoint.maxVel = 0.5
######################################################################
### Setup
######################################################################
mapTestWorld = [0.18, util.Point(2.0, 5.5), (-0.5, 5.5, -0.5, 8.5)]
raceWorld = [0.18, util.Point(2.0, 5.5), (-0.5, 5.5, -0.5, 8.5)]
mazeWorld = [0.15, util.Point(2.0, 0.5), (-0.5, 5.5, -0.5, 5.5)]
frustrationWorld = [0.15, util.Point(3.5, 0.5), (-0.5, 5.5, -0.5, 5.5)]
bigPlanWorld = [0.25, util.Point(3.0, 1.0), (-0.5, 10.5, -0.5, 10.5)]
lizWorld = [0.25, util.Point(9.0, 1.0), (-0.5, 10.5, -0.5, 10.5)]
realRobot = [0.1, util.Point(4.1, -0.4), (-0.5, 5.0, -3.0, 0.5)]
def useWorld(data):
global gridSquareSize, goalPoint, xMin, xMax, yMin, yMax
(gridSquareSize, goalPoint, (xMin, xMax, yMin, yMax)) = data
useWorld(realRobot)
import lib601.util as util a = util.Point(1,0) b = util.Point(0,1) print(a.angle_to(b)) print(a.distance(b))
import lib601.util as util
secret = [
util.Point(1 / 10., -1 / 10.),
util.Point(0 / 10., -2 / 10.),
util.Point(-1 / 10., -1 / 10.),
util.Point(0 / 10., 0 / 10.),
util.Point(-5 / 10., 0 / 10.),
util.Point(-5 / 10., -3 / 10.),
util.Point(11 / 10., -3 / 10.),
util.Point(11 / 10., -1 / 10.),
util.Point(9 / 10., -2 / 10.),
util.Point(9 / 10., 4 / 10.),
util.Point(9 / 10., -2 / 10.),
util.Point(6 / 10., -2 / 10.),
util.Point(6 / 10., 1 / 10.),
util.Point(6 / 10., -2 / 10.),
util.Point(3 / 10., -2 / 10.),
util.Point(3 / 10., 4 / 10.),
util.Point(0 / 10., 4 / 10.),
util.Point(0 / 10., 1 / 10.),
util.Point(0 / 10., 4 / 10.),
util.Point(-3 / 10., 4 / 10.),
util.Point(-3 / 10., -2 / 10.),
util.Point(-3 / 10., 4 / 10.),
util.Point(-5 / 10., 3 / 10.),
util.Point(-5 / 10., 5 / 10.),
util.Point(11 / 10., 5 / 10.),
util.Point(11 / 10., 2 / 10.),
util.Point(6 / 10., 2 / 10.),
util.Point(5 / 10., 3 / 10.),
import noiseModel35 as noiseModel
import checkoff35 as checkoff
else:
import noiseModel34 as noiseModel
import checkoff34 as checkoff
import time
import math
import random
import sonarDist
###### SETUP
NOISE_ON = True
sl13World = [0.15, util.Point(7.0, 1.0), (-0.5, 8.5, -0.5, 6.5)]
bigFrustrationWorld = [0.2, util.Point(7.0, 1.0), (-0.5, 8.5, -0.5, 8.5)]
frustrationWorld = [0.15, util.Point(3.5, 0.5), (-0.5, 5.5, -0.5, 5.5)]
raceWorld = [0.1, util.Point(2.0, 5.5), (-0.5, 5.5, -0.5, 8.5)]
bigPlanWorld = [0.25, util.Point(3.0, 1.0), (-0.5, 10.5, -0.5, 10.5)]
realRobotWorld = [0.1, util.Point(1.5,0.0), (-2.0, 6.0, -2.0, 6.0)]
robotRaceWorld = [0.1, util.Point(3.0,0.0), (-2.0, 6.0, -2.0, 6.0)]
THE_WORLD = raceWorld#bigFrustrationWorld #sl13World
(grid_square_size, goal_point, (x_min, x_max, yMin, yMax)) = THE_WORLD
# graphics options
show_heatmap = True#False
show_passable = False
keep_old_windows = False
def on_step():
global inp
robot.count += 1
inp = io.SensorInput(cheat=True)
# discretize sonar readings
# each element in discrete_sonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discrete_sonars = []
for (sonar_pose, distance) in zip(sonarDist.sonar_poses,inp.sonars):
if NOISE_ON:
distance = noiseModel.noisify(distance, grid_square_size)
if distance >= 5:
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(1, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
continue
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(distance, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
# figure out where robot is
start_point = inp.odometry.point()
startCell = robot.map.point_to_indices(start_point)
def plan_wont_work():
for point in robot.plan:
if not robot.map.is_passable(point):
return True
return False
# if necessary, make new plan
if robot.plan is None or plan_wont_work(): #discrete_sonars[3][0] < 0.3: ###### or if the robot is about to crash into a wall
print('REPLANNING')
robot.plan = search.uniform_cost_search(robot.successors,
robot.map.point_to_indices(inp.odometry.point()),
lambda x: x == robot.goalIndices,
lambda x: 0)
# make the path less jittery
#print(robot.plan)
to_remove = []
if robot.plan is not None:
for i in range(len(robot.plan)-2):
line = util.LineSeg(util.Point(robot.plan[i][0], robot.plan[i][1]), util.Point(robot.plan[i+1][0], robot.plan[i+1][1]))
if line.dist_to_point(util.Point(robot.plan[i+2][0], robot.plan[i+2][1])) < 0.1:
to_remove.append(robot.plan[i+1])
print(to_remove)
for i in to_remove:
robot.plan.remove(i)
#print(robot.plan)
#print()
# graphics (draw robot's plan, robot's location, goal_point)
# do not change this block
for w in robot.windows:
w.redraw_world()
robot.windows[-1].mark_cells(robot.plan,'blue')
robot.windows[-1].mark_cell(robot.map.point_to_indices(inp.odometry.point()),'gold')
robot.windows[-1].mark_cell(robot.map.point_to_indices(goal_point),'green')
# update map
for (d,cells) in discrete_sonars:
#print(cells)
if d < 5:
robot.map.sonar_hit(cells[-1])
for i in range(len(cells)-1):
robot.map.sonar_pass(cells[i])
# if we are within 0.1m of the goal point, we are done!
if start_point.distance(goal_point) <= 0.1:
io.Action(fvel=0,rvel=0).execute()
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
raise Exception('Goal Reached!\n\n%s' % code)
# otherwise, figure out where to go, and drive there
destination_point = robot.map.indices_to_point(robot.plan[0])
while start_point.isNear(destination_point,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destination_point = robot.map.indices_to_point(robot.plan[0])
currentAngle = inp.odometry.theta
angleDiff = start_point.angleTo(destination_point)-currentAngle
angleDiff = util.fix_angle_plus_minus_pi(angleDiff)
fv = rv = 0
if start_point.distance(destination_point) > 0.1:
if abs(angleDiff) > 0.1:
rv = 4*angleDiff
else:
fv = 4*start_point.distance(destination_point)
io.set_forward(fv)
io.set_rotational(rv)
# render the drawing windows
# do not change this block
for w in robot.windows:
w.render()
# Remember to change the import in dynamicMoveToPointSkeleton in order
# to use it from inside soar
import dynamicMoveToPointSkeleton
reload(dynamicMoveToPointSkeleton)
import ffSkeleton
reload(ffSkeleton)
from secretMessage import secret
# Set to True for verbose output on every step
verbose = False
# Rotated square points
squarePoints = [util.Point(0.5, 0.5), util.Point(0.0, 1.0),
util.Point(-0.5, 0.5), util.Point(0.0, 0.0)]
goal_generator = ffSkeleton.FollowFigure(secret)
# goal_generator = sm.Constant(util.Point(1.0, 0.5))
dynamic_move_machine = dynamicMoveToPointSkeleton.DynamicMoveToPoint()
# Put your answer to step 1 here
mySM = sm.Cascade(sm.Parallel(goal_generator, sm.Wire()), dynamic_move_machine)
######################################################################
###
### Brain methods
###
######################################################################
def setup():
noNoise = 0
smallNoise = 0.025
mediumNoise = 0.05
bigNoise = 0.1
# Change noise here
soar.outputs.simulator.SONAR_VARIANCE = lambda mean: noNoise
#soar.outputs.simulator.SONAR_VARIANCE = lambda mean: smallNoise
#soar.outputs.simulator.SONAR_VARIANCE = lambda mean: mediumNoise
#soar.outputs.simulator.SONAR_VARIANCE = lambda mean: bigNoise
######################################################################
### Setup
######################################################################
mapTestWorld = [0.18, util.Point(2.0, 5.5), (-0.5, 5.5, -0.5, 8.5)]
mazeWorld = [0.15, util.Point(2.0, 0.5), (-0.5, 5.5, -0.5, 5.5)]
dl14World = [0.15, util.Point(3.5, 0.5), (-0.5, 5.5, -0.5, 5.5)]
bigPlanWorld = [0.25, util.Point(3.0, 1.0), (-0.5, 10.5, -0.5, 10.5)]
lizWorld = [0.25, util.Point(9.0, 1.0), (-0.5, 10.5, -0.5, 10.5)]
def useWorld(data):
global gridSquareSize, goalPoint, xMin, xMax, yMin, yMax
(gridSquareSize, goalPoint, (xMin, xMax, yMin, yMax)) = data
useWorld(dl14World)
######################################################################
###
def initialLoc(self, x, y):
# Initial robot location
self.initial_robot_loc = util.Point(x, y)
