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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Tue Mar 20 21:18:28 2012
@author: Bernhard Tittelbach, Othmar Gsenger
"""
import numpy as np
import pylab as pl
import scipy.io
import re
simduration = 300 #s
simtimestep = 0.1 #s
w_x = np.random.ranf() * np.pi/4 #rad/s
w_y = np.random.ranf() * np.pi/4 #rad/s
w_z = np.random.ranf() * np.pi/4 #rad/s
filename_export_mat = "./pd_sim.mat"
filename_export_csv = "./pd_sim.csv"
print "Simulation Input:"
print " Duration:", simduration
print " Step Size", simtimestep
print " Angular Speed x-axis [rad/s]", w_x
print " Angular Speed y-axis [rad/s]", w_y
print " Angular Speed z-axis [rad/s]", w_z
simtimeline = np.arange(0,simduration,simtimestep)
w_xyz = np.matrix([w_x,w_y,w_z])
angular_speed_sum_vectors = ( w_xyz.transpose() * simtimeline ).transpose() % (2 *np.pi)
sensors = np.matrix([
[ 1,0,0],
[ -1,0,0],
[0,1,0],
[0,-1,0],
[0,0,1],
[0,0,-1]
])
sun= np.matrix([ 1,0,0])
def getRotationMatrix(angles):
x_angle = angles[0,0]
y_angle = angles[0,1]
z_angle = angles[0,2]
return np.matrix([
[1, 0, 0],
[0, np.cos(x_angle), -np.sin(x_angle)],
[0, np.sin(x_angle), np.cos(x_angle)]
]) * np.matrix([
[np.cos(y_angle) ,0,np.sin(y_angle)],
[0, 1,0],
[-np.sin(y_angle),0,np.cos(y_angle)]
]) * np.matrix([
[np.cos(z_angle),-np.sin(z_angle),0],
[np.sin(z_angle), np.cos(z_angle),0],
[0,0,1]
])
def rotateVector(vec_to_rotate, angles_vector):
return vec_to_rotate * getRotationMatrix(angles_vector)
def sensor_model_rel_sensitivity_cos(angle):
if angle < np.pi/2.0:
return np.cos(angle[0,0])
else:
return 0.0
def _quote(str):
return '"'+ re.sub(r'(?!\\)"','\\"',str) +'"'
def exportCSV(dst_file_path, names, tline, smatrix):
with open(dst_file_path, "w") as fh:
fh.truncate()
fh.write(";".join(map(_quote, ["SimTime"]+names)) +"\n")
for row in np.vstack((tline,smatrix.T)).T:
fh.write(";".join(["%f" % x for x in row.T]) +"\n")
phicostrace=[]
senstrace=[]
for cur_rot in angular_speed_sum_vectors:
sens1=rotateVector(sensors[0],cur_rot)
rot_sensors=rotateVector(sensors,cur_rot)
#sinphi=np.cross(sens1,sun) # /1 / 1
cosphi=np.dot(rot_sensors,sun.transpose())
#print np.arccos(sincos)
#phi=np.sqrt(np.square(sinphi).sum())*float(np.sign(sinphi).prod())
#phisintrace.append(np.arcsin(np.sqrt(np.sum(np.square(sinphi)))))
angle=np.arccos(cosphi)
#print angle
#print [sensor_rel_sensitivity_cos(a) for a in angle]
phicostrace.append(angle)
senstrace.append([sensor_model_rel_sensitivity_cos(a) for a in angle])
senstrace = np.matrix(senstrace)
legend_names=["Sensor%d %s" % (x+1, sensors[x]) for x in range(0,6)]
print "Saving to " + filename_export_mat
scipy.io.savemat(filename_export_mat, mdict={"legend":legend_names, "senstrace":senstrace,"simtimeline":simtimeline,"angular_speed_xyz":w_xyz}, oned_as="column")
print "Saving to " + filename_export_csv
exportCSV(filename_export_csv, legend_names, simtimeline, senstrace)
pl.figure()
for f in range(0,3):
fs = 2*f
fe = fs + 2
pl.subplot(3,1,f+1)
pl.plot(simtimeline, senstrace[:,fs:fe])
pl.legend(legend_names[fs:fe])
pl.figure()
spncols=2
spnrows=3
for s in range(0,6):
pl.subplot(spnrows,spncols,s+1)
pl.plot(simtimeline, senstrace[:,s])
pl.title(legend_names[s])
pl.show()
|
