This is one of the first python scripts I wrote. Primarily used for flight dynamics visualisation, it reads simulation data from a text file, or Matlab file and with the help of a template, exports it to a VRML model.
The Matlab program I used to generate the helicopter slung-load models, like this one was more sophisticated, but this code is adequate for visualising the behaviour of any dynamic non-coupled body through space.
![[S70-A9 Visualisation with VRMLtrace]](/images/s70a9sim.png "[S70-A9 Visualisation with VRMLtrace]")
The template file is available here and the S70-A9 model here. Of course, you'll have to supply your own data file :P The text format is very basic, comprising six columns of [x, y, z, phi, theta, psi] values. You can read more about the general procedure in one of my papers.
#!/usr/bin/python
"""
Convert Simulation Data to a VRML Model
---------------------------------------
**Command Line Usage**
vrmltrace.py [options] input_file template_file output_file
**Options**
-h, --help Print this help and exit.
-v Verbose output.
This script converts any simulation/pose data (position+orientation) into a VRML
model for visualisation. It uses a template file, a VRML model of the vehicle
and a data file to generate the VRML.
The input_file can be in either of three formats:
1. Matlab .MAT binary data file containing Config and Data structs.
2. Raw column-data format (>=7 cols):
t0 x0 y0 z0 phi0 theta0 psi0 ...
t1 x1 y1 z1 phi1 theta1 psi1 ...
...
3. Flightlab Scope format, with variables TIME and Y (>=6 vars):
TIME 1000 1 0
t0 t1 t2
...
Y 1000 6 0
x0 x1 x2
...
y0 y1 y2
...
...
where positions are in ft, angles are in radians.
Copyright (c) 2010, Roger Stuckey. All rights reserved.
"""
import getopt, math, re, sys, time
try:
import scipy.io
except StandardError:
SciPiExists = False
else:
SciPiExists = True
class VRML:
"""
The VRML class can be employed to ingest simulation output data and using a
template file, export it to a VRML model.
"""
def read(self, infile):
"""
Open and read simulation output data from file.
"""
if re.match("\.mat$", infile):
if (SciPiExists):
# Read the MAT file data directly
mat_struct = scipy.io.loadmat(infile)
self.N = X['Config'][0][0].simdata[0][0].numsteps[0][0]
self.t = [ mat_struct['Data'][0][0].time[ii][0]for ii in range(self.N) ]
self.X = [[ mat_struct['Data'][0][0].state[ii][jj] for jj in range(6) ] for ii in range(self.N) ]
else:
print "\nError: SciPy Package required to read Matlab (.mat) data files!",
sys.exit(2)
else:
# Read the input data file
fin = open(infile,'r')
# Determine file data format
dataformat = 0
for line in fin.xreadlines():
line = line.strip()
if (not line): break
if (line[0] == "#"): continue
if (re.match("[a-zA-Z]", line)):
dataformat = 1
# Return to the start
fin.seek(0)
if (dataformat == 0):
# File is just columns of data (unknown length)
while 1:
line = fin.readline()
line = line.strip()
if ( (not line) or (line[0] == "#") ): continue
if (re.match("[\-\+0-9]", line)):
Numstr = line.split()
# Determine number of variables
numvars = len(Numstr)
if (numvars < 7):
print "\nError: Insufficient number of data columns (%d < 7)" % numvars,
sys.exit(2)
elif (numvars > 7):
print "\nWarning: Superfulous data columns (%d > 7)" % numvars,
break
# Read the actual data
Y = self.readdata(fin, numvars)
self.N = len(Y)
self.t = [ Y[ii][0] for ii in range(self.N) ]
self.X = [[ Y[ii][jj+1] for jj in range(6) ] for ii in range(self.N) ]
else:
# File is tagged (includes data descriptors)
while 1:
line = fin.readline()
line = line.strip()
if (not line): break
if (line[0] == "#"): continue
if (re.match("TIME", line)):
# Read the time
Varstr = line.split()
self.N = int(Varstr[1])
numvars = int(Varstr[2]) # should be 1
Y = self.readdata(fin,self.N)
# Remove the first (empty) dimension
self.t = [ Y[0][jj] for jj in range(self.N) ]
elif (re.match("Y", line)):
# Read the position & orientation data
Varstr = line.split()
numvars = int(Varstr[2]) # should be 6!
if (numvars < 6):
print "\nError: Insufficient number of data columns (%d < 6)" % numvars,
sys.exit(2)
elif (numvars > 6):
print "\nWarning: Superfulous data columns (%d > 6)" % numvars,
Y = self.readdata(fin, self.N)
# Transpose the matrix
self.X = [[ Y[ii][jj] for ii in range(6) ] for jj in range(self.N) ]
fin.close()
self.range = [1, self.N]
self.N = self.range[1] - self.range[0] + 1
# Pre-size the state variable array
self.minr = [ 0.0, 0.0, 0.0 ]
self.maxr = [ 0.0, 0.0, 0.0 ]
self.numr = [ 0, 0, 0 ]
for i in range(self.N):
# Determine flight range
for j in range(3):
if (self.X[i][j] < self.minr[j]): self.minr[j] = self.X[i][j]
if (self.X[i][j] > self.maxr[j]): self.maxr[j] = self.X[i][j]
# Nondimensionalise range & calculate grid steps
for j in range(3):
self.minr[j] = math.floor(self.minr[j]/self.dx - 1.0)
if (j < 2):
self.maxr[j] = math.ceil(self.maxr[j]/self.dx + 1.0)
else:
self.maxr[j] = 0.0
self.numr[j] = int(self.maxr[j] - self.minr[j] + 1.0)
# Calculate time delta and range
self.dt = self.t[1] - self.t[0]
self.dT = self.t[-1] - self.t[0]
def readdata(self, fin, numcols):
"""
Read data from an opened file.
"""
# Initialise some variables
Y = [ ]
k = 0
lastrow = -1
lastpos = 0
while 1:
line = fin.readline()
line = line.strip()
if (not line): break
if (line[0] == "#"): continue
# Check for numeric data
if (not re.match("[\-\+0-9]", line)):
fin.seek(lastpos)
break
Numstr = line.split()
# Read the data
for numstr in Numstr:
i = k / numcols
j = k % numcols
# Check if new row
if (i > lastrow):
Y.append([ 0.0 for jj in range(numcols) ])
lastrow = i
# Assign the data
Y[i][j] = float(numstr)
k += 1
lastpos = fin.tell()
return Y
def keyframes(self):
"""
Generate the dictionary of keyframes.
"""
# Define the template variables
self.d = { 'WRL_FILE': "",
'VT_DATE': "",
'PY_VERSION': "",
'CTIME_END': "",
'DTIME_END': "",
'NUM_FRAMES': "",
'KEY': "",
'HELI_ROT_0': "",
'HELI_TRANS_0': "",
'HELI_ROT': "",
'HELI_TRANS': "",
'FWDROT_ROT': "",
'AFTROT_ROT': "",
'GRND_IFS_COORD_PTS': "",
'GRND_IFS_COORD_IDX': "",
'FLGRID_ILS_COORD_PTS': "",
'FLGRID_ILS_COORD_IDX': "",
'LWGRID_ILS_COORD_PTS': "",
'LWGRID_ILS_COORD_IDX': "",
'BWGRID_ILS_COORD_PTS': "",
'BWGRID_ILS_COORD_IDX': "",
'CLGRID_TRANS': "",
'RWGRID_TRANS': "",
'FWGRID_TRANS': "",
'VIEWSIM_USER_TRANS_0': "",
'VIEWSIM_USER_ROT_1': "",
'VIEWSIM_USER_TRANS_1': "",
'VIEWSIM_SIDE_TRANS_0': "",
'VIEWSIM_SIDE_ROT_1': "",
'VIEWSIM_SIDE_TRANS_1': "",
'VIEWSIM_FRONT_TRANS_0': "",
'VIEWSIM_FRONT_ROT_1': "",
'VIEWSIM_FRONT_TRANS_1': "",
'VIEWSIM_TOP_TRANS_0': "",
'VIEWSIM_TOP_ROT_1': "",
'VIEWSIM_TOP_TRANS_1': "",
'VIEWSIM_BEHIND_TRANS_0': "",
'VIEWSIM_BEHIND_ROT_1': "",
'VIEWSIM_BEHIND_TRANS_1': "",
'VIEWSIM_BEHIND_ORIENT_0': "" }
# Fill the variables
self.d['VT_DATE'] = time.strftime("%a, %d %b %Y %H:%M:%S",time.gmtime())
self.d['PY_VERSION'] = (sys.version).replace("\n"," ")
self.d['CTIME_END'] = "%8.4f" % self.dT
self.d['DTIME_END'] = "%8.4f" % (self.N*1.5)
self.d['NUM_FRAMES'] = "%6.1f" % self.N
x = self.X[0]
q = self.e2q(x[3:6])
v = self.q2v(q)
r = x[0:3]
# Generate the key array
key = [ k/(self.N - 1.0) for k in range(self.N) ]
self.d['KEY'] = " %8.4f"*self.N % tuple(key)
# Initial helicopter rotation & translation
self.d['HELI_ROT_0'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v(v))
self.d['HELI_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
af = 0.0
ar = 0.0
# Loop through each frame
for i in range(self.N):
x = self.X[i]
q = self.e2q(x[3:6])
v = self.q2v(q)
r = x[0:3]
# Intermediate helicopter rotation & translation
self.d['HELI_ROT'] += "%8.4f %8.4f %8.4f %8.4f,\n" % tuple(self.a2v(v))
self.d['HELI_TRANS'] += "%8.4f %8.4f %8.4f,\n" % tuple(self.a2v(r))
# Intermediate forward & aft rotor rotations
self.d['FWDROT_ROT'] += "%8.4f %8.4f %8.4f %8.4f,\n" % (0.0,1.0,0.0,af)
self.d['AFTROT_ROT'] += "%8.4f %8.4f %8.4f %8.4f,\n" % (0.0,0.0,1.0,-ar)
if (tplfile.find(".tpl")):
af += 12.0/25.0*1.0/6.0*math.pi
ar += 12.0/25.0*1.0/6.0*math.pi
else:
af += 1.0/6.0*math.pi
ar += 1.0/6.0*math.pi
if (af > 2.0*math.pi): af -= 2.0*math.pi
if (ar > 2.0*math.pi): ar -= 2.0*math.pi
# The floor texture map
for i in [0,self.numr[0] - 1]:
for j in [0,self.numr[1] - 1]:
mg = [ (self.minr[0]+i)*self.dx, (self.minr[1]+j)*self.dx, self.maxr[2]*self.dx ]
self.d['GRND_IFS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
img = [ 0, 1, 3, 2, -1 ]
self.d['GRND_IFS_COORD_IDX'] += "%3.0f %3.0f %3.0f %3.0f %3.0f,\n" % tuple(img)
# The floor grid
for i in range(self.numr[0]):
for j in [0,self.numr[1]-1]:
mg = [ (self.minr[0]+i)*self.dx, (self.minr[1]+j)*self.dx, self.minr[2]*self.dx ]
self.d['FLGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for j in range(self.numr[1]):
for i in [0,self.numr[0]-1]:
mg = [ (self.minr[0]+i)*self.dx, (self.minr[1]+j)*self.dx, self.minr[2]*self.dx ]
self.d['FLGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for i in range(self.numr[0]):
img = [ i*2, i*2 + 1, -1 ]
self.d['FLGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
for j in range(self.numr[0],self.numr[0] + self.numr[1]):
img = [ j*2, j*2 + 1, -1 ]
self.d['FLGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
# The left wall grid
for i in range(self.numr[0]):
for k in [0,self.numr[2]-1]:
mg = [ (self.minr[0]+i)*self.dx, self.minr[1]*self.dx, (self.minr[2]+k)*self.dx ]
self.d['LWGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for k in range(self.numr[2]):
for i in [0,self.numr[0]-1]:
mg = [ (self.minr[0]+i)*self.dx, self.minr[1]*self.dx, (self.minr[2]+k)*self.dx ]
self.d['LWGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for i in range(self.numr[0]):
img = [ i*2, i*2 + 1, -1 ]
self.d['LWGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
for k in range(self.numr[0],self.numr[0] + self.numr[2]):
img = [ k*2, k*2 + 1, -1 ]
self.d['LWGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
# The back wall grid
for j in range(self.numr[1]):
for k in [0,self.numr[2]-1]:
mg = [ self.minr[0]*self.dx, (self.minr[1]+j)*self.dx, (self.minr[2]+k)*self.dx ]
self.d['BWGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for k in range(self.numr[2]):
for j in [0,self.numr[1]-1]:
mg = [ self.minr[0]*self.dx, (self.minr[1]+j)*self.dx, (self.minr[2]+k)*self.dx ]
self.d['BWGRID_ILS_COORD_PTS'] += "%10.4f %10.4f %10.4f,\n" % tuple(self.a2v(mg))
for j in range(self.numr[1]):
img = [ j*2, j*2 + 1, -1 ]
self.d['BWGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
for k in range(self.numr[1],self.numr[1] + self.numr[2]):
img = [ k*2, k*2 + 1, -1 ]
self.d['BWGRID_ILS_COORD_IDX'] += "%3.0f %3.0f %3.0f,\n" % tuple(img)
# The ceiling
self.d['CLGRID_TRANS'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([0.0, 0.0, (self.maxr[2] - self.minr[2])*self.dx]))
# The right wall
self.d['RWGRID_TRANS'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([0.0, (self.maxr[1] - self.minr[1])*self.dx, 0.0]))
# The front wall
self.d['FWGRID_TRANS'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([(self.maxr[0] - self.minr[0])*self.dx, 0.0, 0.0]))
# Calculate the viewpoints
x = self.X[0]
q = self.e2q(x[3:6])
v = self.q2v(q)
r = x[0:3]
# User viewpoint
self.d['VIEWSIM_USER_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
self.d['VIEWSIM_USER_ROT_1'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v(self.av))
self.d['VIEWSIM_USER_TRANS_1'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(self.rv))
# Side viewpoint
dv = self.mag(self.rv)
self.d['VIEWSIM_SIDE_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
self.d['VIEWSIM_SIDE_ROT_1'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, 1.0, 0.0, 0.0 ]))
self.d['VIEWSIM_SIDE_TRANS_1'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, dv, 0.0 ]))
# Front viewpoint
self.d['VIEWSIM_FRONT_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
self.d['VIEWSIM_FRONT_ROT_1'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, 0.0, 1.0, -math.pi/2.0 ]))
self.d['VIEWSIM_FRONT_TRANS_1'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([ dv, 0.0, 0.0 ]))
# Top viewpoint
self.d['VIEWSIM_TOP_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
self.d['VIEWSIM_TOP_ROT_1'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v([ 1.0, 0.0, 0.0, -math.pi/2.0 ]))
self.d['VIEWSIM_TOP_TRANS_1'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, 0.0, -dv ]))
# Behind viewpoint
self.d['VIEWSIM_BEHIND_TRANS_0'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v(r))
self.d['VIEWSIM_BEHIND_ROT_1'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, 1.0, 0.0, -math.pi/4.0 ]))
self.d['VIEWSIM_BEHIND_TRANS_1'] = "%8.4f %8.4f %8.4f" % tuple(self.a2v([ -dv/math.sqrt(2.0), 0.0, -dv/math.sqrt(2.0) ]))
self.d['VIEWSIM_BEHIND_ORIENT_0'] = "%8.4f %8.4f %8.4f %8.4f" % tuple(self.a2v([ 0.0, 0.0, 1.0, math.pi/2.0 ]))
def write(self,outfile):
"""
Write the converted data to a VRML model.
"""
self.d['WRL_FILE'] = outfile
ftpl = open(tplfile,'r')
fout = open(outfile,'w')
for line in ftpl.xreadlines():
# Search for template variables
tagbeg = line.find("$")
tagend = line.find("$",tagbeg + 1)
while (tagbeg > -1):
# Extract the variable & validate
dkey = line[(tagbeg + 1):tagend]
if self.d.has_key(dkey):
line = line.replace("$" + dkey + "$",self.d[dkey])
# Replace with the corresponding dictionary value
tagbeg = line.find("$")
tagend = line.find("$",tagbeg + 1)
fout.write(line)
fout.close()
ftpl.close()
def e2q(self,e):
"""
Convert a list (vector) of Euler angles to quaternions.
"""
se = [ math.sin(e[i]/2) for i in range(3) ]
ce = [ math.cos(e[i]/2) for i in range(3) ]
q = [ se[0]*se[1]*se[2] + ce[0]*ce[1]*ce[2], \
-ce[0]*se[1]*se[2] + se[0]*ce[1]*ce[2], \
se[0]*ce[1]*se[2] + ce[0]*se[1]*ce[2], \
ce[0]*ce[1]*se[2] - se[0]*se[1]*ce[2] ]
return q
def q2v(self,q):
"""
Convert a list (vector) of quaternions to rotation parameters.
"""
z = 2*math.acos(q[0])
sz = math.sin(z/2)
if (sz == 0.0): sz = 1.0
v = [ q[1]/sz, q[2]/sz, q[3]/sz, z ]
return v
def v2v(self,v1,v2):
"""
Calculate normal & rotation angle from two intersecting vectors.
"""
if (v1 == v2): return [ v1[0], v1[1], v1[2], 0.0 ]
v1cv2 = [ v1[1]*v2[2] - v1[2]*v2[1], \
v1[2]*v2[0] - v1[0]*v2[2], \
v1[0]*v2[1] - v1[1]*v2[0] ]
v1dv2 = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]
v1cv2m = self.mag(v1cv2)
if (v1cv2m == 0.0): v1cv2m = 1.0
v1mv2m = self.mag(v1)*self.mag(v2)
if (v1mv2m == 0.0): v1mv2m = 1.0
a = [ v1cv2[0]/v1cv2m, v1cv2[1]/v1cv2m, v1cv2[2]/v1cv2m, math.acos(v1dv2/v1mv2m) ]
return a
def mag(self,v):
"""
Vector magnitude.
"""
return math.sqrt(v[0]**2 + v[1]**2 + v[2]**2)
def a2v(self,a):
"""
Convert aircraft axis space to VRML axis space.
"""
v = [ a[0], -a[2], a[1] ]
if (len(a) > 3): v += [a[3]]
return v
if __name__ == "__main__":
try:
opts, args = getopt.getopt(sys.argv[1:], "hv", ["help"])
except getopt.GetoptError:
# Print help information and exit:
sys.stdout.write(__doc__)
sys.exit(2)
verbose = 0
for o, a in opts:
if o == "-v":
verbose = 1
if o in ("-h", "--help"):
sys.stdout.write(__doc__)
sys.exit()
infile = args[0]
if (len(args) < 3):
p = re.compile('\.\w+$')
outfile = p.sub('.wrl', infile)
else:
outfile = args[2]
if (len(args) < 2):
tplfile = 'vrmltrace.wrt'
else:
tplfile = args[1]
v = VRML()
# The grid spacing
v.dx = 100.0
# User viewpoint offset
v.rv = [ 75.0, 75.0, 0.0 ]
# User viewpoint rotation vector
v.av = v.v2v([ 0.0, 1.0, 0.0 ], v.rv)
# Read the input data, compute the variables & write the output
if (verbose): sys.stdout.write("Reading data")
v.read(infile)
if (verbose): sys.stdout.write("\nGenerating keyframes")
v.keyframes()
if (verbose): sys.stdout.write("\nWriting output [%s]" % outfile)
v.write(outfile)
if (verbose): sys.stdout.write("\n")