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)