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 makeStateEstimationSimulation(worldSM, verbose = False):
"""
Make a machine that simulates the state estimation process. It
takes a state machine representing the world, at construction
time. Let i be an input to the world machine. The input is fed
into the world machine, generating (stochastically) an output, o.
The (o, i) pair is fed into a state-estimator using worldSM as its
model. The output of the state estimator is a belief state, b.
The output of this entire composite machine is (b, (o, i)).
@param worldSM: an instance of C{ssm.StochasticSM}
@returns: a state machine that simulates the world and executes
the state estimation process.
"""
return sm.Cascade(sm.Parallel(worldSM, sm.Wire()),
sm.Parallel(StateEstimator(worldSM, verbose = verbose),
sm.Wire()))
def systemSM(circuit, inComponents, motorNodes, initState):
# circuit takes input which is a dictionary of values (or list of them)
# output is a le.Solution instance.
cSM = CircuitSM(circuit.components, inComponents, circuit.groundNode)
# motor input is Solution instance, output is (vel, angle)
motorSM = sm.Cascade(MotorAccel(initState, motorNodes), sm.R(initState))
# world has Solution as input, ((vel, angle), sol) as output
wSM = sm.Parallel(motorSM, sm.Wire())
if inComponents:
# there's another input besides motor feedback
return Feedback2(sm.Cascade(cSM,wSM), MotorFeedback())
else:
# only motor feedback
return Feedback(sm.Cascade(cSM,wSM), MotorFeedback())