Mercurial Hosting > traffic-intelligence

annotate python/extrapolation.py @ 267:32e88b513f5c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added code to compute probability of collision
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 27 Jul 2012 20:32:14 -0400
parents aba9711b3149
children 0c0b92f621f6
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
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
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
65 class ExtrapolationParameters:
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
66 def __init__(self, name, maxSpeed):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
67 self.name = name
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
68 self.maxSpeed = maxSpeed
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
69
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
70 def __str__(self):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
71 return '{0} {1}'.format(self.name, self.maxSpeed)
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
72
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
73 # refactor with classes and subclasses
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
74 def createExtrapolatedTrajectories(self, obj, instant):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
75 '''extrapolationParameters specific to each method (in name field) '''
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
76 extrapolatedTrajectories = []
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
77 if self.name == 'constant velocity':
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
78 extrapolatedTrajectories = [ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant), obj.getVelocityAtInstant(instant),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
79 maxSpeed = self.maxSpeed)]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
80 elif self.name == 'normal adaptation':
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
81 for i in xrange(self.nExtrapolatedTrajectories):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
82 extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(obj.getPositionAtInstant(instant),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
83 obj.getVelocityAtInstant(instant),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
84 self.accelerationDistribution,
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
85 self.steeringDistribution,
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
86 maxSpeed = self.maxSpeed))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
87 elif self.name == 'pointset':
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
88 features = [f for f in obj.features if f.existsAtInstant(instant)]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
89 positions = [f.getPositionAtInstant(instant) for f in features]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
90 velocities = [f.getVelocityAtInstant(instant) for f in features]
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
91 for initialPosition,initialVelocity in zip(positions, velocities):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
92 extrapolatedTrajectories.append(ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity,
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
93 maxSpeed = self.maxSpeed))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
94 elif self.name == 'evasive action':
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
95 for i in xrange(self.nExtrapolatedTrajectories):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
96
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
97 extrapolatedTrajectories.append(ExtrapolatedTrajectoryConstant(obj.getPositionAtInstant(instant),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
98 obj.getVelocityAtInstant(instant),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
99 moving.NormAngle(self.accelerationDistribution(), self.steeringDistribution()),
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
100 maxSpeed = self.maxSpeed))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
101 else:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
102 print('Unknown extrapolation hypothesis')
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
103 return extrapolatedTrajectories
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
104
259
8ab76b95ee72 added code to save collision points and crossing zones in txt files
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 258
diff changeset
105 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
106 '''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
107 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
108 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
109 self.x = p.x
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
110 self.y = p.y
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
111 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
112 self.indicator = indicator
258
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
113
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
114 @staticmethod
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
115 def save(out, points, objNum1, objNum2, instant):
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
116 for p in points:
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
117 out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator))
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
118
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 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
120 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
121 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
122
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
123 def computeCollisionTime(extrapolatedTrajectory1, extrapolatedTrajectory2, collisionDistanceThreshold, timeHorizon):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
124 t = 1
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
125 p1 = extrapolatedTrajectory1.predictPosition(t)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
126 p2 = extrapolatedTrajectory2.predictPosition(t)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
127 while t <= timeHorizon and (p1-p2).norm2() > collisionDistanceThreshold:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
128 p1 = extrapolatedTrajectory1.predictPosition(t)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
129 p2 = extrapolatedTrajectory2.predictPosition(t)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
130 t += 1
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
131 return t, p1, p2
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
132
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
133 def computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon):
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
134 '''returns the lists of collision points and crossing zones '''
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
135 extrapolatedTrajectories1 = extrapolationParameters.createExtrapolatedTrajectories(obj1, i)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
136 extrapolatedTrajectories2 = extrapolationParameters.createExtrapolatedTrajectories(obj2, i)
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
137
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
138 collisionPoints = []
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
139 crossingZones = []
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
140 for et1 in extrapolatedTrajectories1:
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
141 for et2 in extrapolatedTrajectories2:
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
142 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
143
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
144 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
145 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
146 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
147 # 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
148 # 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
149 # 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
150 cz = None
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
151 t1 = 0
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
152 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
153 t2 = 0
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
154 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
155 #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
156 # 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
157 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
158 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
159 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
160 t2 += 1
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
161 t1 += 1
d90be3c02267 reasonably efficient computation of collision points and crossing zones
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 257
diff changeset
162 return collisionPoints, crossingZones
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
163
264
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
164 def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False, timeInterval = None):
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
165 collisionPoints={}
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
166 crossingZones={}
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
167 #TTCs = {}
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
168 #pPETs = {}
264
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
169 if timeInterval:
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
170 commonTimeInterval = timeInterval
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
171 else:
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
172 commonTimeInterval = obj1.commonTimeInterval(obj2)
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
173 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
264
a04a6af4b810 modified functions to generate extrapolated trajectories for different positions/velocities
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 260
diff changeset
174 print(obj1.num, obj2.num, i)
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
175 collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(i, obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon)
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
176 # if len(collisionPoints[i]) > 0:
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
177 # TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i])
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
178 # if len(crossingZones[i]) > 0:
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
179 # pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i])
266
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
180 SafetyPoint.save(outCP, collisionPoints[i], obj1.num, obj2.num, i)
aba9711b3149 small modificatons and reorganization
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 265
diff changeset
181 SafetyPoint.save(outCZ, crossingZones[i], obj1.num, obj2.num, i)
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
182
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
183 if debug:
265
7a3bf04cf016 added plotting of indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 264
diff changeset
184 from matplotlib.pyplot import figure, axis, title
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
185 figure()
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
186 obj1.draw('r')
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
187 obj2.draw('b')
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
188 for et in extrapolatedTrajectories1:
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
189 et.predictPosition(timeHorizon)
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
190 et.draw('rx')
265
7a3bf04cf016 added plotting of indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 264
diff changeset
191
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
192 for et in extrapolatedTrajectories2:
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
193 et.predictPosition(timeHorizon)
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
194 et.draw('bx')
265
7a3bf04cf016 added plotting of indicators
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 264
diff changeset
195 title('instant {0}'.format(i))
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
196 axis('equal')
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
197
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
198 return collisionPoints, crossingZones
267
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
199
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
200 def computeCollisionProbability(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, out, debug = False):
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
201 collisionProbabilities = {}
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
202 for i in list(obj1.commonTimeInterval(obj2))[:-1]:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
203 nCollisions = 0
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
204 print(obj1.num, obj2.num, i)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
205 extrapolatedTrajectories1 = extrapolationParameters.createExtrapolatedTrajectories(obj1, i)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
206 extrapolatedTrajectories2 = extrapolationParameters.createExtrapolatedTrajectories(obj2, i)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
207 for et1 in extrapolatedTrajectories1:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
208 for et2 in extrapolatedTrajectories2:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
209 t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
210 if t <= timeHorizon:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
211 nCollisions += 1
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
212 # take into account probabilities ??
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
213 collisionProbabilities[i] = float(nCollisions)/extrapolationParameters.nExtrapolatedTrajectories**2
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
214 out.write('{0} {1} {2} {3} {4}\n'.format(obj1.num, obj2.num, extrapolationParameters.nExtrapolatedTrajectories, i, collisionProbabilities[i]))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
215
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
216 if debug:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
217 from matplotlib.pyplot import figure, axis, title
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
218 figure()
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
219 obj1.draw('r')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
220 obj2.draw('b')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
221 for et in extrapolatedTrajectories1:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
222 et.predictPosition(timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
223 et.draw('rx')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
224
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
225 for et in extrapolatedTrajectories2:
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
226 et.predictPosition(timeHorizon)
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
227 et.draw('bx')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
228 title('instant {0}'.format(i))
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
229 axis('equal')
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
230
32e88b513f5c added code to compute probability of collision
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 266
diff changeset
231 return collisionProbabilities
260
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
232
36cb40c51a5e modified the organization of the code
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 259
diff changeset
233 ###############
257
9281878ff19e untested collision/crossing computation
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 256
diff changeset
234
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
235 # 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
236 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
237 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
238 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
239
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
240 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
241 ''' 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
242 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
243 vInit= velocity
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
244 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
245 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
246 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
247
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
248 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
249 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
250 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
251 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
252 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
253 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
254 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
255 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
256 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
257
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
258 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
259 ''' 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
260 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
261 vInit= velocity
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
262 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
263 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
264 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
265 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
266 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
267 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
268 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
269 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
270 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
271 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
272 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
273 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
274 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
275 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
276 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
277 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
278 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
279 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
280 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
281 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
282
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
283 return position
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
284
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
285 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
286 ''' 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
287 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
288 vInit= velocity
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
289 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
290 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
291 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
292 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
293 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
294 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
295 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
296 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
297 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
298 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
299 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
300 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
301 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
302 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
303 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
304 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
305 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
306 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
307 time= time2
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
308 else:
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
309 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
310 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
311 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
312 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
313
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
314 return time
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
315
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
316 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
317 ''' 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
318 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
319 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
320 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
321 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
322 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
323 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
324 return position
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
325
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
326 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
327 ''' 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
328 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
329 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
330 N=1000
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
331 ProbOfCollision = 0
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
332 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
333 # acceleration limit
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
334 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
335 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
336 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
337 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
338 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
339 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
340 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
341 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
342 t=1
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
343 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
344 # Extrapolation position
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
345 (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
346 (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
347 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
348 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
349 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
350 t+=1
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
351 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
352 return POC
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
353
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
354 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
355 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
356 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
357 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
358 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
359 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
360 return v
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
361
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
362 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
363 ''' 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
364 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
365 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
366 N=1000
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
367 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
368 ProbOfCollision = 0
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
369 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
370 # acceleration limit
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
371 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
372 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
373 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
374 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
375 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
376 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
377 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
378 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
379 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
380 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
381 t=1
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
382 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
383 # Extrapolation position
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
384 (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
385 (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
386 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
387 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
388 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
389 t+=1
5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 243
diff changeset
390 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
391 return POC
243
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
392
e0988a8ace0c started adapting and moving to other modules Mohamed's work
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 240
diff changeset
393
255
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
394 if __name__ == "__main__":
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
395 import doctest
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
396 import unittest
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
397 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
398 #suite = doctest.DocTestSuite()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
399 unittest.TextTestRunner().run(suite)
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
400 #doctest.testmod()
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
401 #doctest.testfile("example.txt")
13ec22bec5d4 corrected typos and bugs and added a test
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 250
diff changeset
402