Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 256:dc1faa7287bd
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 |