-
Notifications
You must be signed in to change notification settings - Fork 1
/
trajectoryGenerationTest.py
59 lines (53 loc) · 1.73 KB
/
trajectoryGenerationTest.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
from parameters import par
from control import CBTS
from classes import node
from classes import agent
from classes import gmrf
import matplotlib.pyplot as plt
import matplotlib
import numpy as np
import math
matplotlib.use('TkAgg')
N = 3
depth = 3
par = par('seqBayes', 'cbts', 'noUpdates', 'random', False, False, 1000, 0, 0, 0, 0, 0, 6, 3, 50, 50, 0.05, 0.5, 0.9)
cbts = CBTS(par)
auv = agent(par, par.x0, par.y0, par.alpha0)
gmrf1 = gmrf(par, par.nGridX, par.nGridY, par.nEdge)
v = node(par, gmrf, auv)
derivXStart = auv.derivX
derivYStart = auv.derivY
fig = plt.figure()
for i in range(N):
v.auv.x = par.x0
v.auv.y = par.y0
v.auv.derivX = derivXStart
v.auv.derivY = derivYStart
# Get first trajectory
tau = np.zeros((2,(par.nTrajPoints-1)*depth+1))
theta = (-1 + 2 * np.random.rand()) * np.eye(1)
#theta = [-1]
tau[:,0:par.nTrajPoints], v.auv.derivX, v.auv.derivY = cbts.generateTrajectory(v,theta)
v.auv.x = tau[0, par.nTrajPoints - 1]
v.auv.y = tau[1, par.nTrajPoints - 1]
print("afterf first traj:",v.auv.x,v.auv.y)
# Check next depth
for d in range(1,depth):
theta = (-1+2*np.random.rand())*np.eye(1)
#theta = [-1]
tauNext, v.auv.derivX, v.auv.derivY = cbts.generateTrajectory(v,theta)
v.auv.x = tauNext[0,-1]
v.auv.y = tauNext[1,-1]
tau[:,(par.nTrajPoints-1)*d:(par.nTrajPoints-1)*(d+1)+1] = tauNext
stepSizes = []
xOld = par.x0
yOld = par.y0
for i in range(tau.shape[1]-1):
stepSizes.append(math.sqrt((xOld-tau[0,i+1])**2+(yOld-tau[1,i+1])**2))
xOld = tau[0,i+1]
yOld = tau[1,i+1]
plt.plot(tau[0,:],tau[1,:])
plt.scatter(tau[0,:],tau[1,:])
plt.axis('equal')
print(np.mean(stepSizes))
plt.show()