def creatCalibMatrix():
"""Generate and return symbolic expression of 4 x 4 affine rotation matrix from US probe reference frame to US image reference frame.
Returns
-------
prTi : sympy.matrices.matrices.MutableMatrix
The matrix expression.
syms : list
List of ``sympy.core.symbol.Symbol`` symbol objects used to generate the expression.
"""
x1 = Symbol('x1')
y1 = Symbol('y1')
z1 = Symbol('z1')
alpha1 = Symbol('alpha1')
beta1 = Symbol('beta1')
gamma1 = Symbol('gamma1')
prTi = Matrix(([c(alpha1)*c(beta1), c(alpha1)*s(beta1)*s(gamma1)-s(alpha1)*c(gamma1), c(alpha1)*s(beta1)*c(gamma1)+s(alpha1)*s(gamma1), x1],\
[s(alpha1)*c(beta1), s(alpha1)*s(beta1)*s(gamma1)+c(alpha1)*c(gamma1), s(alpha1)*s(beta1)*c(gamma1)-c(alpha1)*s(gamma1), y1],\
[-s(beta1), c(beta1)*s(gamma1), c(beta1)*c(gamma1), z1],\
[0, 0, 0, 1]\
))
syms = [x1, y1, z1, alpha1, beta1, gamma1]
return prTi, syms
def generate_het(self):
"""
Generate heterogeneous terms for integrating the Z_i and I_i terms.
Returns
-------
None.
"""
self.hetx_list, self.hety_list = ([] for i in range(2))
# get the general expression for h in z before plugging in g,z.
# column vectors ax ay for use in matrix A = [ax ay]
self.ax = Matrix([[0], [0]])
self.ay = Matrix([[0], [0]])
for j in range(1, self.trunc_gh + 1):
p1 = lib.kProd(j, self.dx)
p2 = kp(p1, sym.eye(2))
d1 = lib.vec(lib.df(self.NIC1, self.x, j + 1))
d2 = lib.vec(lib.df(self.NIC2, self.x, j + 1))
self.ax += (1 / math.factorial(j)) * p2 * d1
self.ay += (1 / math.factorial(j)) * p2 * d2
self.A = sym.zeros(2, 2)
self.A[:, 0] = self.ax
self.A[:, 1] = self.ay
self.z_expansion = Matrix([[self.zx], [self.zy]])
het = self.A * self.z_expansion
# expand all terms
self.hetx = sym.expand(het[0].subs([(self.dx1, self.gx),
(self.dx2, self.gy)]))
self.hety = sym.expand(het[1].subs([(self.dx1, self.gx),
(self.dx2, self.gy)]))
# collect all psi terms into factors of pis^k
self.hetx_powers = sym.collect(self.hetx, self.psi, evaluate=False)
self.hety_powers = sym.collect(self.hety, self.psi, evaluate=False)
self.hetx_list = []
self.hety_list = []
counter = 0
while (counter <= self.trunc_g + 1):
# save current term
self.hetx_list.append(self.hetx_powers[self.psi**counter])
self.hety_list.append(self.hety_powers[self.psi**counter])
counter += 1
# substitute limit cycle
for i in range(len(self.ghx_list)):
self.hetx_list[i] = self.hetx_list[i].subs({
self.x1:
sym.cos(2 * sym.pi * self.t),
self.x2:
sym.sin(2 * sym.pi * self.t),
sym.Indexed('gx', 0):
s(0),
sym.Indexed('gy', 0):
s(0)
})
self.hety_list[i] = self.hety_list[i].subs({
self.x1:
sym.cos(2 * sym.pi * self.t),
self.x2:
sym.sin(2 * sym.pi * self.t),
sym.Indexed('gx', 0):
s(0),
sym.Indexed('gy', 0):
s(0)
})
def generate_gh(self):
"""
generate heterogeneous terms for the Floquet eigenfunctions g.
Returns
-------
list of symbolic heterogeneous terms in self.ghx_list, self.ghy_list.
"""
self.ghx_list = []
self.ghy_list = []
# get the general expression for h before plugging in g.
self.hx = 0
self.hy = 0
for i in range(2, self.trunc_gh + 1):
# all x1,x2 are evaluated on limit cycle x=cos(t), y=sin(t)
p = lib.kProd(i, self.dx)
d1 = lib.vec(lib.df(self.NIC1, self.x, i))
d2 = lib.vec(lib.df(self.NIC2, self.x, i))
self.hx += (1 / math.factorial(i)) * np.dot(p, d1)
self.hy += (1 / math.factorial(i)) * np.dot(p, d2)
self.hx = sym.Matrix(self.hx)
self.hy = sym.Matrix(self.hy)
# expand all terms
self.hx = sym.expand(
self.hx.subs([(self.dx1, self.gx), (self.dx2, self.gy)]))
self.hy = sym.expand(
self.hy.subs([(self.dx1, self.gx), (self.dx2, self.gy)]))
# collect all psi terms into list of some kind
self.tempx = sym.collect(self.hx[0], self.psi, evaluate=False)
self.tempy = sym.collect(self.hy[0], self.psi, evaluate=False)
counter = 0
while (counter <= self.trunc_g + 1):
# save current term
self.ghx_list.append(self.tempx[self.psi**counter])
self.ghy_list.append(self.tempy[self.psi**counter])
counter += 1
# substitute limit cycle. maybe move elsewhere.
for i in range(len(self.ghx_list)):
self.ghx_list[i] = self.ghx_list[i].subs({
self.x1:
sym.cos(2 * sym.pi * self.t),
self.x2:
sym.sin(2 * sym.pi * self.t),
sym.Indexed('gx', 0):
s(0),
sym.Indexed('gy', 0):
s(0)
})
self.ghy_list[i] = self.ghy_list[i].subs({
self.x1:
sym.cos(2 * sym.pi * self.t),
self.x2:
sym.sin(2 * sym.pi * self.t),
sym.Indexed('gx', 0):
s(0),
sym.Indexed('gy', 0):
s(0)
})
"Cd": 0.3, # unitless
"p": 1.3, # Kg/m**3
"A": 0.0042, # m**2
"m": 0.145,# kg
"g": -9.8, # m/s**2
}
# time without drag
tf = sympy.solve(1+45*np.sin(np.deg2rad(35)*2)*t+0.5*(-9.8)*t**2,t)[1]
# distance without drag
xf = qd.e(45*np.cos(np.deg2rad(35.)*2)*t, {"t": tf})
print "distance without drag:", xf
print "time without drag:", tf
# drag equation
dfdt = lambda x, v: qd.e(M([0, s("g")]) + (s("-1/2*p*Cd*A")*v.norm()**2*v.normalized()), c)
v = M(["vx", "vy"])
print "acceleration:", qd.e(M([0, s("g")]) + (s("-1/2*p*Cd*A")*v.norm()**2*v.normalized()), c)
# initial velocity matrix
v0 = M([45.*np.cos(np.deg2rad(35.)*2), 45.*np.sin(np.deg2rad(35.)*2)])
x0 = M([0, 1])
c['dt'] = 0.1
c['tol'] = 0# 0.001*xf # 0.1% of estimated range
rData = qd.rk4(dfdt, x0, v0, lambda t, x, v: x[1] > 0, c)
print 'rk45 complete'
hData = qd.heun(dfdt, x0, v0, lambda t, x, v: x[1] > 0, c)
print 'modeuler complete'
eData = qd.euler(dfdt, x0, v0, lambda t, x, v: x[1] > 0, c)
print 'fwdeuler complete'
plt.plot(rData[0], rData[1], marker='o', label='RK4')
plt.plot(hData[0], hData[1], marker='.', label='Heun')
def createCalibEquations():
"""Generate and return symbolic calibration equations (1) in [Ref2]_.
Returns
-------
Pph : sympy.matrices.matrices.MutableMatrix
3 x 1 matrix containing symbolic equations (1) in [Ref2]_.
J : sympy.matrices.matrices.MutableMatrix*)
3 x 14 matrix representing the Jacobian of equations ``Pph``.
prTi : sympy.matrices.matrices.MutableMatrix*)
4 x 4 affine rotation matrix from US probe reference frame to US image reference frame.
syms : dict
Dictionary of where keys are variable names and values are ``sympy.core.symbol.Symbol`` objects.
These symbols were used to create equations in ``Pph``, ``J``, ``prTi``.
variables : list
14-elem list of variable names (see ``process.Process.calibrateProbe()``).
mus : list
14-elem list of varables measurement units.
"""
# Pi
sx = Symbol('sx')
sy = Symbol('sy')
u = Symbol('u')
v = Symbol('v')
Pi = Matrix(([sx * u],\
[sy * v],\
[0],\
[1]\
))
# prTi
prTi, syms = creatCalibMatrix()
[x1, y1, z1, alpha1, beta1, gamma1] = syms
# wTpr
#wTpr = Matrix(MatrixSymbol('wTpr', 4, 4))
wTpr = MatrixOfMatrixSymbol('wTpr', 4, 4)
wTpr[3, 0:4] = np.array([0,0,0,1])
# phTw
x2 = Symbol('x2')
y2 = Symbol('y2')
z2 = Symbol('z2')
alpha2 = Symbol('alpha2')
beta2 = Symbol('beta2')
gamma2 = Symbol('gamma2')
phTw = Matrix(([c(alpha2)*c(beta2), c(alpha2)*s(beta2)*s(gamma2)-s(alpha2)*c(gamma2), c(alpha2)*s(beta2)*c(gamma2)+s(alpha2)*s(gamma2), x2],\
[s(alpha2)*c(beta2), s(alpha2)*s(beta2)*s(gamma2)+c(alpha2)*c(gamma2), s(alpha2)*s(beta2)*c(gamma2)-c(alpha2)*s(gamma2), y2],\
[-s(beta2), c(beta2)*s(gamma2), c(beta2)*c(gamma2), z2],\
[0, 0, 0, 1]\
))
# Calculate full equations
Pph = phTw * wTpr * prTi * Pi
Pph = Pph[0:3,:]
# Calculate full Jacobians
x = Matrix([sx, sy, x1, y1, z1, alpha1, beta1, gamma1, x2, y2, z2, alpha2, beta2, gamma2])
J = Pph.jacobian(x)
# Create symbols dictionary
syms = {}
for expr in Pph:
atoms = list(expr.atoms(Symbol))
for i in xrange(0, len(atoms)):
syms[atoms[i].name] = atoms[i]
# Create list of variables and measurements units
variables = ['sx', 'sy', 'x1', 'y1', 'z1', 'alpha1', 'beta1', 'gamma1', 'x2', 'y2', 'z2', 'alpha2', 'beta2', 'gamma2']
mus = ['mm/px', 'mm/px', 'mm', 'mm', 'mm', 'rad', 'rad', 'rad', 'mm', 'mm', 'mm', 'rad', 'rad', 'rad']
# Return data
return Pph, J, prTi, syms, variables, mus
# Set path for markers coordinates file
p.setKineFiles(('test_baloon_2ang.c3d',))
# Calculate pose from US probe to laboratory reference frame
p.calculatePoseForUSProbe(mkrList=('Rigid_Body_1-Marker_1','Rigid_Body_1-Marker_2','Rigid_Body_1-Marker_3','Rigid_Body_1-Marker_4'))
# Calculate pose from US images to laboratory reference frame
p.calculatePoseForUSImages()
# Reorient global reference frame to be approximately aligned with US scans direction
from sympy import Matrix, Symbol, cos as c, sin as s
alpha = Symbol('alpha')
beta = Symbol('beta')
T1 = Matrix(([1,0,0,0],
[0,c(alpha),s(alpha),0],
[0,-s(alpha),c(alpha),0],
[0,0,0,1]
))
T = T1.evalf(subs={'alpha':np.deg2rad(-10.)})
T = np.array(T).astype(np.float)
# Set time frames for images that can be cointaned in the voxel array
p.setValidFramesForVoxelArray(voxFrames='auto')
# Calculate convenient pose for the voxel array
p.calculateConvPose(T)
# Calculate scale factors
#fxyz = 'auto_bounded_parallel_scans'
fxyz = (1,10,1)