Mercurial Hosting > traffic-intelligence
annotate python/extrapolation.py @ 262:a048066bd20f
correcting bug in framesToTime
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 25 Jul 2012 18:22:18 -0400 |
parents | 36cb40c51a5e |
children | a04a6af4b810 |
rev | line source |
---|---|
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
1 #! /usr/bin/env python |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
2 '''Library for moving object extrapolation hypotheses''' |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
3 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
4 import moving |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
5 import math |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
6 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
7 class ExtrapolatedTrajectory: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
8 '''Class for extrapolated trajectories with lazy evaluation |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
9 if the predicted position has not been already computed, compute it |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
10 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
11 it should also have a probability''' |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
12 |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
13 def __init__(self): |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
14 self.probability = 0. |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
15 self.predictedPositions = {} |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
16 self.predictedSpeedOrientations = {} |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
17 self.collisionPoints = {} |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
18 self.crossingZones = {} |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
19 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
20 def predictPosition(self, nTimeSteps): |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
21 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
22 self.predictPosition(nTimeSteps-1) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
23 self.predictedPositions[nTimeSteps], self.predictedSpeedOrientations[nTimeSteps] = moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], self.getControl(), self.maxSpeed) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
24 return self.predictedPositions[nTimeSteps] |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
25 |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
26 def getPredictedTrajectory(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
27 return moving.Trajectory.fromPointList(self.predictedPositions.values()) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
28 |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
29 def getPredictedSpeeds(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
30 return [so.norm for so in self.predictedSpeedOrientations.values()] |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
31 |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
32 def draw(self, options = '', withOrigin = False, **kwargs): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
33 self.getPredictedTrajectory().draw(options, withOrigin, **kwargs) |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
34 |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
35 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory): |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
36 '''Extrapolated trajectory at constant speed or acceleration |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
37 TODO generalize by passing a series of velocities/accelerations''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
38 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
39 def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None): |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
40 self.control = control |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
41 self.maxSpeed = maxSpeed |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
42 self.probability = probability |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
43 self.predictedPositions = {0: initialPosition} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
44 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
45 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
46 def getControl(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
47 return self.control |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
48 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
49 class ExtrapolatedTrajectoryNormalAdaptation(ExtrapolatedTrajectory): |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
50 '''Random small adaptation of vehicle control ''' |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
51 def __init__(self, initialPosition, initialVelocity, accelerationDistribution, steeringDistribution, probability = 1, maxSpeed = None): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
52 '''Constructor |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
53 accelerationDistribution and steeringDistribution are distributions |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
54 that return random numbers drawn from them''' |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
55 self.accelerationDistribution = accelerationDistribution |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
56 self.steeringDistribution = steeringDistribution |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
57 self.maxSpeed = maxSpeed |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
58 self.probability = probability |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
59 self.predictedPositions = {0: initialPosition} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
60 self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} |
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
61 |
256
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
62 def getControl(self): |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
63 return moving.NormAngle(self.accelerationDistribution(),self.steeringDistribution()) |
dc1faa7287bd
added the normal adaptation class
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
255
diff
changeset
|
64 |
250
59f547aebaac
modified prediction functions, added norm/angle representation of Points
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
245
diff
changeset
|
65 |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
66 class ExtrapolationParameters: |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
67 def __init__(self, name): |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
68 self.name = name |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
69 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
70 def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity): |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
71 '''extrapolationParameters specific to each method (in name field) ''' |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
72 if extrapolationParameters.name == 'constant velocity': |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
73 return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)] |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
74 elif extrapolationParameters.name == 'normal adaptation': |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
75 # generate several and with the right distribution |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
76 extrapolatedTrajectories = [] |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
77 for i in xrange(extrapolationParameters.nExtrapolatedTrajectories): |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
78 extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
79 extrapolationParameters.accelerationDistribution, |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
80 extrapolationParameters.steeringDistribution, |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
81 maxSpeed = extrapolationParameters.maxSpeed)) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
82 return extrapolatedTrajectories |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
83 else: |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
84 print('Unknown extrapolation hypothesis') |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
85 return [] |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
86 |
259
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
87 class SafetyPoint(moving.Point): |
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
88 '''Can represent a collision point or crossing zone |
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
89 with respective safety indicator, TTC or pPET''' |
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
90 def __init__(self, p, probability = 1., indicator = -1): |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
91 self.x = p.x |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
92 self.y = p.y |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
93 self.probability = probability |
259
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
94 self.indicator = indicator |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
95 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
96 def saveSafetyPoints(out, objNum1, objNum2, instant, points): |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
97 for p in points: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
98 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator)) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
99 |
259
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
100 def computeExpectedIndicator(points): |
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
101 from numpy import sum |
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
102 return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points]) |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
103 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
104 def computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon): |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
105 '''returns the lists of collision points and crossing zones ''' |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
106 collisionPoints = [] |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
107 crossingZones = [] |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
108 for et1 in extrapolatedTrajectories1: |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
109 for et2 in extrapolatedTrajectories2: |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
110 t = 1 |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
111 p1 = et1.predictPosition(t) |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
112 p2 = et2.predictPosition(t) |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
113 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold: |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
114 p1 = et1.predictPosition(t) |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
115 p2 = et2.predictPosition(t) |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
116 t += 1 |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
117 |
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
118 if t <= timeHorizon: |
259
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
119 collisionPoints.append(SafetyPoint((p1+p2).multiply(0.5), et1.probability*et2.probability, t)) |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
120 else: # check if there is a crossing zone |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
121 # TODO? zone should be around the points at which the traj are the closest |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
122 # look for CZ at different times, otherwise it would be a collision |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
123 # an approximation would be to look for close points at different times, ie the complementary of collision points |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
124 cz = None |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
125 t1 = 0 |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
126 while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
127 t2 = 0 |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
128 while not cz and t2 < timeHorizon: |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
129 #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
130 # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
131 cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
132 if cz: |
259
8ab76b95ee72
added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
258
diff
changeset
|
133 crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) |
258
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
134 t2 += 1 |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
135 t1 += 1 |
d90be3c02267
reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
257
diff
changeset
|
136 return collisionPoints, crossingZones |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
137 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
138 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False): |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
139 collisionPoints={} |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
140 crossingZones={} |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
141 #TTCs = {} |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
142 #pPETs = {} |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
143 commonTimeInterval = obj1.commonTimeInterval(obj2) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
144 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
145 extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1.getPositionAtInstant(i), obj1.getVelocityAtInstant(i)) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
146 extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i)) |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
147 |
260
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
148 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
149 # if len(collisionPoints[i]) > 0: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
150 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i]) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
151 # if len(crossingZones[i]) > 0: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
152 # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i]) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
153 saveSafetyPoints(outCP, obj1.num, obj2.num, i, collisionPoints[i]) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
154 saveSafetyPoints(outCZ, obj1.num, obj2.num, i, crossingZones[i]) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
155 |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
156 if debug: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
157 from matplotlib.pyplot import figure, axis |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
158 figure() |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
159 obj1.draw('r') |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
160 obj2.draw('b') |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
161 for et in extrapolatedTrajectories1: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
162 et.predictPosition(timeHorizon) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
163 et.draw('rx') |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
164 for et in extrapolatedTrajectories2: |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
165 et.predictPosition(timeHorizon) |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
166 et.draw('bx') |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
167 axis('equal') |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
168 |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
169 |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
170 # probability of successful evasive action = 1-P(Collision) using larger control values |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
171 |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
172 return collisionPoints, crossingZones |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
173 |
36cb40c51a5e
modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
259
diff
changeset
|
174 ############### |
257
9281878ff19e
untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
256
diff
changeset
|
175 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
176 # Default values: to remove because we cannot tweak that from a script where the value may be different |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
177 FPS= 25 # No. of frame per second (FPS) |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
178 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
179 deltaT= FPS*5 # extrapolatation time Horizon = 5 second |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
180 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
181 def motion (position, velocity, acceleration): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
182 ''' extrapolation hypothesis: constant acceleration''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
183 from math import atan2,cos,sin |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
184 vInit= velocity |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
185 vInitial= velocity.norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
186 theta= atan2(velocity.y,velocity.x) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
187 vFinal= vInitial+acceleration |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
188 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
189 if acceleration<= 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
190 v= max(0,vFinal) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
191 velocity= moving.Point(v* cos(theta),v* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
192 position= position+ (velocity+vInit). multiply(0.5) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
193 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
194 v= min(vLimit,vFinal) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
195 velocity= moving.Point(v* cos(theta),v* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
196 position= position+ (velocity+vInit). multiply(0.5) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
197 return(position,velocity) |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
198 |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
199 def motionPET (position, velocity, acceleration, deltaT): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
200 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
201 from math import atan2,cos,sin,fabs |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
202 vInit= velocity |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
203 vInitial= velocity.norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
204 theta= atan2(velocity.y,velocity.x) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
205 vFinal= vInitial+acceleration * deltaT |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
206 if acceleration< 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
207 if vFinal> 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
208 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
209 position= position+ (vInit+ velocity). multiply(0.5*deltaT) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
210 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
211 T= fabs(vInitial/acceleration) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
212 position= position + vInit. multiply(0.5*T) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
213 elif acceleration> 0 : |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
214 if vFinal<= vLimit: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
215 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
216 position= position+ (vInit+ velocity). multiply(0.5*deltaT) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
217 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
218 time1= fabs((vLimit-vInitial)/acceleration) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
219 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
220 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
221 elif acceleration == 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
222 position= position + velocity. multiply(deltaT) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
223 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
224 return position |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
225 |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
226 def timePET (position, velocity, acceleration, intersectedPoint ): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
227 ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
228 from math import atan2,cos,sin,fabs |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
229 vInit= velocity |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
230 vInitial= velocity.norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
231 theta= atan2(velocity.y,velocity.x) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
232 vFinal= vInitial+acceleration * deltaT |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
233 if acceleration< 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
234 if vFinal> 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
235 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
236 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
237 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
238 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
239 elif acceleration> 0 : |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
240 if vFinal<= vLimit: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
241 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
242 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
243 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
244 time1= fabs((vLimit-vInitial)/acceleration) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
245 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
246 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
247 if time2<=time1: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
248 time= time2 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
249 else: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
250 position2= (position+ (velocity+vInit). multiply(0.5*time1)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
251 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
252 elif acceleration == 0: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
253 time= fabs((intersectedPoint.x-position.x)/(velocity.x)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
254 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
255 return time |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
256 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
257 def motionSteering (position, velocity, deltaTheta, deltaT ): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
258 ''' extrapolation hypothesis: steering with deltaTheta''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
259 from math import atan2,cos,sin |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
260 vInitial= velocity.norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
261 theta= atan2(velocity.y,velocity.x) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
262 newTheta= theta + deltaTheta |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
263 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
264 position= position+ (velocity). multiply(deltaT) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
265 return position |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
266 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
267 def MonteCarlo(movingObject1,movingObject2, instant): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
268 ''' Monte Carlo Simulation : estimate the probability of collision''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
269 from random import uniform |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
270 from math import pow, sqrt, sin, cos,atan2 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
271 N=1000 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
272 ProbOfCollision = 0 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
273 for n in range (1, N): |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
274 # acceleration limit |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
275 acc1 = uniform(-0.040444,0) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
276 acc2 = uniform(-0.040444,0) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
277 p1= movingObject1.getPositionAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
278 p2= movingObject2.getPositionAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
279 v1= movingObject1.getVelocityAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
280 v2= movingObject2.getVelocityAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
281 distance= (p1-p2).norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
282 distanceThreshold= 1.8 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
283 t=1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
284 while distance > distanceThreshold and t <= deltaT: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
285 # Extrapolation position |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
286 (p1,v1) = motion(p1,v1,acc1) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
287 (p2,v2) = motion(p2,v2,acc2) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
288 distance= (p1-p2).norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
289 if distance <=distanceThreshold: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
290 ProbOfCollision= ProbOfCollision+1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
291 t+=1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
292 POC= float(ProbOfCollision)/N |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
293 return POC |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
294 |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
295 def velocitySteering(velocity,steering): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
296 from math import atan2,cos,sin |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
297 vInitial= velocity.norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
298 theta= atan2(velocity.y,velocity.x) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
299 newTheta= theta + steering |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
300 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
301 return v |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
302 |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
303 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): |
244
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
304 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
305 from random import uniform |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
306 from math import pow, sqrt, sin, cos,atan2 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
307 N=1000 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
308 L= 2.4 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
309 ProbOfCollision = 0 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
310 for n in range (1, N): |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
311 # acceleration limit |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
312 acc1 = uniform(-0.040444,0) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
313 acc2 = uniform(-0.040444,0) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
314 p1= movingObject1.getPositionAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
315 p2= movingObject2.getPositionAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
316 vInit1= movingObject1.getVelocityAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
317 v1= velocitySteering (vInit1,steering1) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
318 vInit2= movingObject2.getVelocityAtInstant(instant) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
319 v2= velocitySteering (vInit2,steering2) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
320 distance= (p1-p2).norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
321 distanceThreshold= 1.8 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
322 t=1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
323 while distance > distanceThreshold and t <= deltaT: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
324 # Extrapolation position |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
325 (p1,v1) = motion(p1,v1,acc1) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
326 (p2,v2) = motion(p2,v2,acc2) |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
327 distance= (p1-p2).norm2() |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
328 if distance <=distanceThreshold: |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
329 ProbOfCollision= ProbOfCollision+1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
330 t+=1 |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
331 POC= float(ProbOfCollision)/N |
5027c174ab90
moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
243
diff
changeset
|
332 return POC |
243
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
333 |
e0988a8ace0c
started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
240
diff
changeset
|
334 |
255
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
335 if __name__ == "__main__": |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
336 import doctest |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
337 import unittest |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
338 suite = doctest.DocFileSuite('tests/extrapolation.txt') |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
339 #suite = doctest.DocTestSuite() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
340 unittest.TextTestRunner().run(suite) |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
341 #doctest.testmod() |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
342 #doctest.testfile("example.txt") |
13ec22bec5d4
corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
250
diff
changeset
|
343 |