def _makefp(self, params):
zdot = self.x[self.dim:]
out = np.concatenate((
zdot,
np.dot(
self.Mzi, -tn.einsum('i,ijk,k', zdot, self.dMz, zdot) +
tn.einsum('i,ikl,k', zdot, self.dMz, zdot) / 2 + self.dVz) +
matmult(
tn.subs(self._dPsi, self.qtoz),
self.Mqi, # here we might need subs
self.u)))
out = tn.SymExpr(tn.subs(out, self.ztox))
out.callable(*params)
return out
def _makefp(self, params):
zdot = self.x[self.dim:]
out = np.concatenate((zdot,
np.dot(self.Mzi,
- tn.einsum(
'i,ijk,k', zdot, self.dMz, zdot)
+ tn.einsum(
'i,ikl,k', zdot, self.dMz, zdot) / 2
+ self.dVz)
+ matmult(
tn.subs(self._dPsi, self.qtoz),
self.Mqi, # here we might need subs
self.u)
))
out = tn.SymExpr(tn.subs(out, self.ztox))
out.callable(*params)
return out
def _makefm(self, params):
zdot = self.x[self.dim:]
OhmP = tn.subs(self._Ohm, zip(self.z, self._P))
OhmI = tn.subs(self._dPsi, zip(self.q, OhmP))
zz = self._P
zzdot = np.dot(self._dP, zdot)
out = -tn.einsum('i,ijk,k', zzdot, self.dMzz, zzdot) \
+ tn.einsum('i,ikj,k', zzdot, self.dMzz, zzdot) / 2
out = np.dot(self.Mzzi, out + self.dVzz)
out = out - tn.einsum('ijk,j,k', tn.diff(self._dP, self.z), zdot, zdot)
out = out + matmult(OhmI, self.Mqi, self.u)
# in general, there should be a subs here
out = matmult(self._dPi, out)
out = np.concatenate((zdot, out))
out = tn.SymExpr(tn.subs(out, self.ztox))
out.callable(*params)
return out
def _makefm(self, params):
zdot = self.x[self.dim:]
OhmP = tn.subs(self._Ohm, zip(self.z, self._P))
OhmI = tn.subs(self._dPsi, zip(self.q, OhmP))
zz = self._P
zzdot = np.dot(self._dP, zdot)
out = -tn.einsum('i,ijk,k', zzdot, self.dMzz, zzdot) \
+ tn.einsum('i,ikj,k', zzdot, self.dMzz, zzdot) / 2
out = np.dot(self.Mzzi, out + self.dVzz)
out = out - tn.einsum('ijk,j,k',
tn.diff(self._dP, self.z), zdot, zdot)
out = out + matmult(OhmI, self.Mqi, self.u)
# in general, there should be a subs here
out = matmult(self._dPi, out)
out = np.concatenate((zdot, out))
out = tn.SymExpr(tn.subs(out, self.ztox))
out.callable(*params)
return out
def __init__(self, si=0, **kwargs):
# this is the special index: z[si] = phi(z)
self.si = si
self.t = S('t')
n = self.dim
self.qtoz = zip(self.q, self._Ohm)
self.alltoz = self.qtoz + zip(self.x, self.z)
self.ztoq = zip(self.z, self._Psi)
self.alltoq = self.ztoq + zip(self.x, self._Psi)
self.ztox = zip(self.z, self.x)
# dOhm/dz, dPhi/dq, assuming the pieces are already defined
self._dOhm = tn.diff(self._Ohm, self.z)
self._dPsi = tn.diff(self._Psi, self.q)
self.Mz = matmult(self._dOhm.T, self.Mq, self._dOhm)
self.Mzi = self._Mzi()
self.dMq = tn.diff(self.Mq, self.q)
self.dMz = tn.diff(self.Mz, self.z)
self.delta = self.Mzi[:, self.si] / self.Mzi[self.si, self.si]
# self.ddelta = tn.diff(self.delta, self.z)
self.Vz = self.Vq.subs(self.alltoz, simultaneous=True)
self.dVz = tn.diff(self.Vz, self.z)
self._P = self._makeP(self.k)
self._dP = tn.diff(self._P, self.z)
self._dPi = np.array(sym.Matrix(self._dP).inv())
self.ztozz = {self.z[i]: self._P[i] for i in range(self.dim)}
self.Mzzi = tn.subs(self.Mzi, self.ztozz)
self.dMzz = tn.subs(self.dMz, self.ztozz)
self.dVzz = tn.subs(self.dVz, self.ztozz)
self.dPzz = tn.subs(self._dP, self.ztozz)
params = [self.t, self.x, self.u]
self._fplus = self._makefp(params)
self._fmins = self._makefm(params)
self._dfxp = tn.SymExpr(self._fplus.diff(self.x))
self._dfxp.callable(*params)
self._dfxm = tn.SymExpr(self._fmins.diff(self.x))
self._dfxm.callable(*params)
self._dfup = tn.SymExpr(self._fplus.diff(self.u))
self._dfup.callable(*params)
self._dfum = tn.SymExpr(self._fmins.diff(self.u))
self._dfum.callable(*params)
self._ohm = tn.lambdify(self.z, self._Ohm)
self._psi = tn.lambdify(self.q, self._Psi)
# make the jump term generator callable
# and a bunch of other stuff as well
self.delf = lambda t, x, u: self._delf(t, x, u)
self.Ohm = lambda z: tn.eval(self._Ohm, self.z, z)
self.dOhm = lambda z: tn.eval(self._dOhm, self.z, z)
self.Psi = lambda q: tn.eval(self._Psi, self.q, q)
self.dPsi = lambda q: tn.eval(self._dPsi, self.q, q)
def __init__(self, si=0, **kwargs):
# this is the special index: z[si] = phi(z)
self.si = si
self.t = S('t')
n = self.dim
self.qtoz = zip(self.q, self._Ohm)
self.alltoz = self.qtoz + zip(self.x, self.z)
self.ztoq = zip(self.z, self._Psi)
self.alltoq = self.ztoq + zip(self.x, self._Psi)
self.ztox = zip(self.z, self.x)
# dOhm/dz, dPhi/dq, assuming the pieces are already defined
self._dOhm = tn.diff(self._Ohm, self.z)
self._dPsi = tn.diff(self._Psi, self.q)
self.Mz = matmult(self._dOhm.T, self.Mq, self._dOhm)
self.Mzi = self._Mzi()
self.dMq = tn.diff(self.Mq, self.q)
self.dMz = tn.diff(self.Mz, self.z)
self.delta = self.Mzi[:, self.si] / self.Mzi[self.si, self.si]
# self.ddelta = tn.diff(self.delta, self.z)
self.Vz = self.Vq.subs(self.alltoz, simultaneous=True)
self.dVz = tn.diff(self.Vz, self.z)
self._P = self._makeP(self.k)
self._dP = tn.diff(self._P, self.z)
self._dPi = np.array(sym.Matrix(self._dP).inv())
self.ztozz = {self.z[i]: self._P[i] for i in range(self.dim)}
self.Mzzi = tn.subs(self.Mzi, self.ztozz)
self.dMzz = tn.subs(self.dMz, self.ztozz)
self.dVzz = tn.subs(self.dVz, self.ztozz)
self.dPzz = tn.subs(self._dP, self.ztozz)
params = [self.t, self.x, self.u]
self._fplus = self._makefp(params)
self._fmins = self._makefm(params)
self._dfxp = tn.SymExpr(self._fplus.diff(self.x))
self._dfxp.callable(*params)
self._dfxm = tn.SymExpr(self._fmins.diff(self.x))
self._dfxm.callable(*params)
self._dfup = tn.SymExpr(self._fplus.diff(self.u))
self._dfup.callable(*params)
self._dfum = tn.SymExpr(self._fmins.diff(self.u))
self._dfum.callable(*params)
self._ohm = tn.lambdify(self.z, self._Ohm)
self._psi = tn.lambdify(self.q, self._Psi)
# make the jump term generator callable
# and a bunch of other stuff as well
self.delf = lambda t, x, u: self._delf(t, x, u)
self.Ohm = lambda z: tn.eval(self._Ohm, self.z, z)
self.dOhm = lambda z: tn.eval(self._dOhm, self.z, z)
self.Psi = lambda q: tn.eval(self._Psi, self.q, q)
self.dPsi = lambda q: tn.eval(self._dPsi, self.q, q)
