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)
