Mercurial > hg > nsaunier > traffic-intelligence

comparison python/extrapolation.py @ 256:dc1faa7287bd

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added the normal adaptation class
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 24 Jul 2012 01:37:21 -0400
parents 13ec22bec5d4
children 9281878ff19e
comparison
equal deleted inserted replaced
255:13ec22bec5d4 256:dc1faa7287bd
7 class ExtrapolatedTrajectory: 7 class ExtrapolatedTrajectory:
8 '''Class for extrapolated trajectories with lazy evaluation 8 '''Class for extrapolated trajectories with lazy evaluation
9 if the predicted position has not been already computed, compute it 9 if the predicted position has not been already computed, compute it
10 10
11 it should also have a probability''' 11 it should also have a probability'''
12
12 def predictPosition(self, nTimeSteps): 13 def predictPosition(self, nTimeSteps):
13 return None 14 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
15 self.predictPosition(nTimeSteps-1)
16 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed)
17 return self.predictedPositions[nTimeSteps]
18
19 def getPredictedTrajectory(self):
20 return moving.Trajectory.fromPointList(self.predictedPositions.values())
21
22 def getPredictedSpeeds(self):
23 return [so.norm for so in self.predictedSpeedOrientations.values()]
24
25 def draw(self, options = '', withOrigin = False, **kwargs):
26 self.getPredictedTrajectory().draw(options, withOrigin, **kwargs)
14 27
15 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): 28 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory):
16 '''Extrapolated trajectory at constant speed or acceleration 29 '''Extrapolated trajectory at constant speed or acceleration
17 TODO add limits if acceleration 30 TODO add limits if acceleration
18 TODO generalize by passing a series of velocities/accelerations''' 31 TODO generalize by passing a series of velocities/accelerations'''
22 self.maxSpeed = maxSpeed 35 self.maxSpeed = maxSpeed
23 self.probability = probability 36 self.probability = probability
24 self.predictedPositions = {0: initialPosition} 37 self.predictedPositions = {0: initialPosition}
25 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 38 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
26 39
27 def predictPosition(self, nTimeSteps): 40 def getControl(self):
28 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): 41 return self.control
29 self.predictPosition(nTimeSteps-1) 42
30 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.control, self.maxSpeed) 43 class ExtrapolatedTrajectoryNormalAdaptation(ExtrapolatedTrajectory):
31 return self.predictedPositions[nTimeSteps]
32
33 class ExtrapolatedTrajectoryAdaptation(ExtrapolatedTrajectoryConstant):
34 '''Random small adaptation of vehicle control ''' 44 '''Random small adaptation of vehicle control '''
35 def __init__(self, initialPosition, initialVelocity, accelerationBounds, steeringBounds, probability = 1, maxSpeed = None): 45 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1, maxSpeed = None):
36 self.accelerationBounds = accelerationBounds 46 '''Constructor
37 self.steeringBounds = steeringBounds 47 accelerationDistribution and steeringDistribution are distributions
48 that return random numbers drawn from them'''
49 self.accelerationDistribution = accelerationDistribution
50 self.steeringDistribution = steeringDistribution
38 self.maxSpeed = maxSpeed 51 self.maxSpeed = maxSpeed
39 self.probability = probability 52 self.probability = probability
40 self.predictedPositions = {0: initialPosition} 53 self.predictedPositions = {0: initialPosition}
41 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} 54 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
42 55
43 def predictPosition(self, nTimeSteps): 56 def getControl(self):
44 # draw acceleration and steering 57 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution())
45 # TODO should the predictPosition be in the base class ?? 58
46 self.control = moving.NormAngle(0,0)
47 return ExtrapolatedTrajectoryConstant.predictPosition(self, nTimeSteps)
48 59
49 # Default values: to remove because we cannot tweak that from a script where the value may be different 60 # Default values: to remove because we cannot tweak that from a script where the value may be different
50 FPS= 25 # No. of frame per second (FPS) 61 FPS= 25 # No. of frame per second (FPS)
51 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec 62 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec
52 deltaT= FPS*5 # extrapolatation time Horizon = 5 second 63 deltaT= FPS*5 # extrapolatation time Horizon = 5 second