def makeLineLocalizer(numObservations, numStates, ideal, xMin, xMax, robotY): #! pass """ Create behavior controlling robot to move in a line and to localize itself in one dimension @param numObservations: number of discrete observations into which to divide the range of good sonar observations, between 0 and C{goodSonarRange} @param numStates: number of discrete states into which to divide the range of x coordinates @param ideal: list of ideal sonar readings @param xMin: minimum x coordinate for center of robot @param xMax: maximum x coordinate for center of robot @param robotY: constant y coordinate for center of robot @returns: an instance of {\tt sm.SM} that implements the behavior """ # Size of a state in meters stateWidthMeters = (xMax - xMin) / float(numStates) preprocessor = PreProcess(numObservations, stateWidthMeters) navModel = makeRobotNavModel(ideal, xMin, xMax, numStates, numObservations) estimator = seGraphics.StateEstimator(navModel) driver = move.MoveToFixedPose(util.Pose(xMax, robotY, 0.0), maxVel=0.5) return sm.Cascade(sm.Parallel(sm.Cascade(preprocessor, estimator), driver), sm.Select(1))
def computeIdealReadings(worldPath, xMin, xMax, y, numStates, numObs): """ :param worldPath: string naming file to read the world description from :param xMin: minimum x coordinate for center of robot :param xMax: maximum x coordinate for center of robot :param y: constant y coordinate for center of robot :param numStates: number of discrete states into which to divide the range of x coordinates :param numObs: number of discrete observations into which to divide the range of good sonar observations, between 0 and ``goodSonarRange`` :returns: list of ``numStates`` values, each of which is between 0 and ``numObs-1``, which lists the ideal discretized sonar reading that the robot would receive if it were at the midpoint of each of the x bins. """ # read obstacles from the map world = soarWorld.SoarWorld(worldPath) xStep = (xMax - xMin) / float(numStates) readings = [] # Start in the middle of the first box x = xMin + (xStep / 2.0) for ix in range(numStates): # left-hand sonar reading assuming we're heading to the right readings.append(discreteSonarValue(\ idealSonarReading(util.Pose(x, y, 0), sonarDist.sonarPoses[0], world), numObs)) x = x + xStep return readings
def main(): #w = world(-1,2,-1,2) w = world(-2, 3, -2, 3) #w.add_obs((0.1,0.1,0.1), util.Pose(0,0,0,0)) # origin #w.add_obs((0.1,0.1,0.1), util.Pose(1,0,0,0)) # reference pt cube = (1, 1, 1) traj_set = trajectory_set((0, 0, 1), (0.9, 0.9, 0.9)) traj_set.create_set(w.x_min, w.x_max, w.y_min, w.y_max) for traj in traj_set.trajectories: w.add_traj(traj) w.add_obs(cube, util.Pose(-1, 0, 0, 0)) w.add_obs(cube, util.Pose(1, 0, 0, 0)) # w.add_obs(cube, util.Pose(0,-1,0,0)) # w.add_obs(cube, util.Pose(0,1,0,0)) # print w.allows_bad() w.draw()
def collides(self, obstacles): pts = self.discretize(4) for pt in pts: traj_pt = om.Box(self.size[0], self.size[1], self.size[2], name='traj_pt', \ pose=util.Pose(pt[0], pt[1], pt[2], self.theta)) for obs in obstacles: if (traj_pt.collides(obs)): return True return False
def draw(self, window): #window.drawPoint(self.goal[0],self.goal[1], color="blue") # window.drawRect(((self.goal[0]-self.size[0]/2),(self.goal[1]-self.size[1]/2)),((self.goal[0]+self.size[0]/2), (self.goal[1]+self.size[1]/2)), color='yellow') #window.drawOval(((self.goal[0]-0.05),(self.goal[1]-0.05)), ((self.goal[0]+0.05),(self.goal[1]+0.05)), color='blue') box = om.Box(self.size[0], self.size[1], self.size[2], name='traj', \ pose=util.Pose(self.goal[0], self.goal[1], 0, \ self.theta)) box.draw(window)
def permute_gripper(self, bad_poses): obs_size = (0.05, 0.05, 0.05) best_combo = () pos_positions = [(x,y) for x in drange(self.x_min, self.x_max,0.05) \ for y in drange(self.y_min, self.y_max,0.05)] for pos in pos_positions: if (pos[0] + (2 * obs_size[0])) <= self.x_max: combo = (pos, (pos[0] + (2 * obs_size[0]), pos[1])) self.add_obs(obs_size, util.Pose(pos[0], pos[1], 0, 0)) self.add_obs(obs_size, util.Pose(pos[0]+(2*obs_size[0]), \ pos[1], 0, 0)) if not self.pose_equal(combo, bad_poses): metric = self.allows_bad() #print "combo="+str(combo)+" , "+str(metric) if (best_combo == ()): best_combo = (combo, metric) if (metric < best_combo[1]): best_combo = (combo, metric) # print "best="+str(best_combo[0])+' , '+str(best_combo[1]) else: print "NECK" self.remove_obs() if (pos[1] + (2 * obs_size[0])) <= self.y_max: combo = (pos, (pos[0], pos[1] + (2 * obs_size[0]))) self.add_obs((1, 1, 1), util.Pose(pos[0], pos[1], 0, 0)) self.add_obs((1, 1, 1), util.Pose(pos[0], pos[1] + (2 * obs_size[0]), 0, 0)) if not self.pose_equal(combo, bad_poses): metric = self.allows_bad() #print "combo="+str(combo)+" , "+str(metric) if (best_combo == ()): best_combo = (combo, metric) if (metric < best_combo[1]): best_combo = (combo, metric) #print "best="+str(best_combo[0])+' , '+str(best_combo[1]) else: print "CRACKS" self.remove_obs() return best_combo
def permute_obs(self, obs_num): pos_combos = [(x,y) for x in range(self.x_min, self.x_max) \ for y in range(self.y_min, self.y_max)] obs_combos = itertools.combinations(pos_combos, obs_num) best_combo = () for combo in obs_combos: for i in range(obs_num): self.add_obs((1, 1, 1), util.Pose(combo[i][0], combo[i][1], 0, 0)) metric = self.allows_bad() if (best_combo == ()): best_combo = (combo, metric) if (metric < best_combo[1]): best_combo = (combo, metric) print "best=" + str(best_combo[0]) + ' , ' + str(best_combo[1]) self.remove_obs() return best_combo
""" import math import util #CHANGED: commented out two sonar poses ##sonarPoses = [util.Pose(0.08, 0.134, math.pi/2), ## util.Pose(0.122, 0.118, 5*math.pi/18), ## util.Pose(0.156, 0.077, math.pi/6), ## util.Pose(0.174, 0.0266, math.pi/18), ## util.Pose(0.174, -0.0266, -math.pi/18), ## util.Pose(0.156, -0.077, -math.pi/6), ## util.Pose(0.122, -0.118, -5*math.pi/18), ## util.Pose(0.08, -0.134, -math.pi/2)] sonarPoses = [util.Pose(0.073, 0.105, 90*math.pi/180), util.Pose(0.130, 0.078, 41*math.pi/180), util.Pose(0.154, 0.030, 15*math.pi/180), util.Pose(0.154, -0.030, -15*math.pi/180), util.Pose(0.130, -0.078, -41*math.pi/180), util.Pose(0.073, -0.105, -90*math.pi/180)] #eBot Sonars #onarPoses = [util.Pose(-0.012, -0.04, 90*math.pi/180), # util.Pose(0.012, -0.028, 45*math.pi/180), # util.Pose(0.029, 0, 0*math.pi/180), # util.Pose(0.012, 0.028, -45*math.pi/180), # util.Pose( -0.012, 0.04, -90*math.pi/180), # util.Pose(-0.029, 0, 180*math.pi/180)] """Positions and orientations of sonar sensors with respect to the center of the robot."""
def getNextValues(self, state, inp): return (None, CorruptedSensorInput(\ [random.gauss(s, self.sonarStDev) for s in inp.sonars], util.Pose(random.gauss(inp.odometry.x, self.odoStDev), inp.odometry.y, inp.odometry.theta)))
import ssm import sonarDist import move import seGraphics import idealReadings # For testing your preprocessor class SensorInput: def __init__(self, sonars, odometry): self.sonars = sonars self.odometry = odometry preProcessTestData = [ SensorInput([0.8, 1.0], util.Pose(1.0, 0.5, 0.0)), SensorInput([0.25, 1.2], util.Pose(2.4, 0.5, 0.0)), SensorInput([0.16, 0.2], util.Pose(7.3, 0.5, 0.0)) ] testIdealReadings = (5, 1, 1, 5, 1, 1, 1, 5, 1, 5) testIdealReadings100 = (50, 10, 10, 50, 10, 10, 10, 50, 10, 50) #! sonarStDev = 0.02 odoStDev = 0.02 ########################################################################## # Preprocessor ##########################################################################
## util.Pose(0.174, 0.0266, math.pi/18), ## util.Pose(0.174, -0.0266, -math.pi/18), ## util.Pose(0.156, -0.077, -math.pi/6), ## util.Pose(0.122, -0.118, -5*math.pi/18), ## util.Pose(0.08, -0.134, -math.pi/2)] #onarPoses = [util.Pose(0.073, 0.105, 90*math.pi/180), # util.Pose(0.130, 0.078, 41*math.pi/180), # util.Pose(0.154, 0.030, 15*math.pi/180), # util.Pose(0.154, -0.030, -15*math.pi/180), # util.Pose(0.130, -0.078, -41*math.pi/180), # util.Pose(0.073, -0.105, -90*math.pi/180)] #eBot Sonars sonarPoses = [ util.Pose(-0.012, -0.04, 90 * math.pi / 180), util.Pose(0.012, -0.028, 45 * math.pi / 180), util.Pose(0.029, 0, 0 * math.pi / 180), util.Pose(0.012, 0.028, -45 * math.pi / 180), util.Pose(-0.012, 0.04, -90 * math.pi / 180), util.Pose(-0.029, 0, 180 * math.pi / 180) ] """Positions and orientations of sonar sensors with respect to the center of the robot.""" sonarMax = 1.5 """Maximum good sonar reading.""" #TODO: check if returning right value def getDistanceRight(sonarValues):
grid.clearCell((ix, iy)) #! # For testing your map maker class SensorInput: def __init__(self, sonars, odometry): self.sonars = sonars self.odometry = odometry testData = [ SensorInput([0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2], util.Pose(1.0, 2.0, 0.0)), SensorInput([0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4], util.Pose(4.0, 2.0, -math.pi)) ] testClearData = [ SensorInput([1.0, 5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.0], util.Pose(1.0, 2.0, 0.0)), SensorInput([1.0, 5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.0], util.Pose(4.0, 2.0, -math.pi)) ] def testMapMaker(data): (xMin, xMax, yMin, yMax, gridSquareSize) = (0, 5, 0, 5, 0.1) mapper = MapMaker(xMin, xMax, yMin, yMax, gridSquareSize)