def __call__(self, traj, tspace=False):
tj = traj if traj.feasible or tspace else self.projector(traj)
ta, tb = tj.tlims
T = tb
tlist = np.linspace(ta, tb, (tb-ta)*1e3, endpoint=True)
xlist = [tj.x(t) for t in tlist]
ulist = [tj.u(t) for t in tlist]
elist = []
for (t, x, u) in zip(tlist, xlist, ulist):
# only consider reference if not in tangent space
xd = (0.0 if tspace else 1.0)*self.ref.x(t)
ud = (0.0 if tspace else 1.0)*self.ref.u(t)
Q = self.Q(t)
R = self.R(t)
expr = matmult(x - xd, Q, x - xd) + matmult(u - ud, R, u - ud)
elist.append(expr)
# integrate the above
out = 0.5 * trapz(elist, tlist)
# don't add a terminal cost in tangent space
if not tspace:
out += 0.5 * matmult(tj.x(T) - self.ref.x(T),
self.PT, tj.x(T) - self.ref.x(T))
return out
def _delf(self, t, xval, uval):
# calculates the jump term assuming the field switches
# between fplus and fminus at (t, x)
params = np.concatenate(([t], xval, uval))
dphi = self.dphi(xval)
# determine if going from f- to f+
# or vice-versa
if matmult(dphi[:self.dim], xval[-self.dim:]) > 0:
fp = self._fplus.func(*params)
fm = self._fmins.func(*params)
else:
fp = self._fmins.func(*params)
fm = self._fplus.func(*params)
#out = 2*np.outer(fp-fm, dphi)/np.abs(matmult(fm+fp, dphi))
#out = np.outer(fp-fm, dphi)/np.abs(matmult(fm, dphi))
out = np.outer(fp, dphi)/matmult(dphi, fm) \
#+ np.eye(len(fp)) - np.outer(dphi, dphi)/np.dot(dphi,dphi)
#Tracer()()
#out = np.zeros((2*self.dim, 2*self.dim))
#for i in range(self.dim):
# out[self.si, i] = -M[self.si, i]
return out
def _delf(self, t, xval, uval):
# calculates the jump term assuming the field switches
# between fplus and fminus at (t, x)
params = np.concatenate(([t], xval, uval))
dphi = self.dphi(xval)
# determine if going from f- to f+
# or vice-versa
if matmult(dphi[:self.dim],xval[-self.dim:]) > 0:
fp = self._fplus.func(*params)
fm = self._fmins.func(*params)
else:
fp = self._fmins.func(*params)
fm = self._fplus.func(*params)
#out = 2*np.outer(fp-fm, dphi)/np.abs(matmult(fm+fp, dphi))
#out = np.outer(fp-fm, dphi)/np.abs(matmult(fm, dphi))
out = np.outer(fp, dphi)/matmult(dphi, fm) \
#+ np.eye(len(fp)) - np.outer(dphi, dphi)/np.dot(dphi,dphi)
#Tracer()()
#out = np.zeros((2*self.dim, 2*self.dim))
#for i in range(self.dim):
# out[self.si, i] = -M[self.si, i]
return out
def __call__(self, traj, tspace=False):
tj = traj if traj.feasible or tspace else self.projector(traj)
ta, tb = tj.tlims
T = tb
tlist = np.linspace(ta, tb, (tb - ta) * 1e3, endpoint=True)
xlist = [tj.x(t) for t in tlist]
ulist = [tj.u(t) for t in tlist]
elist = []
for (t, x, u) in zip(tlist, xlist, ulist):
# only consider reference if not in tangent space
xd = (0.0 if tspace else 1.0) * self.ref.x(t)
ud = (0.0 if tspace else 1.0) * self.ref.u(t)
Q = self.Q(t)
R = self.R(t)
expr = matmult(x - xd, Q, x - xd) + matmult(u - ud, R, u - ud)
elist.append(expr)
# integrate the above
out = 0.5 * trapz(elist, tlist)
# don't add a terminal cost in tangent space
if not tspace:
out += 0.5 * matmult(
tj.x(T) - self.ref.x(T), self.PT,
tj.x(T) - self.ref.x(T))
return out
def _Mzi(self):
dpsi = np.empty_like(self._dPsi)
for i in range(self.dim):
for j in range(self.dim):
dpsi[i, j] = self._dPsi[i, j].subs(self.alltoz,
simultaneous=True)
return matmult(dpsi, self.Mqi, dpsi.T)
def _Mzi(self):
dpsi = np.empty_like(self._dPsi)
for i in range(self.dim):
for j in range(self.dim):
dpsi[i, j] = self._dPsi[i, j].subs(self.alltoz,
simultaneous=True)
return matmult(dpsi, self.Mqi, dpsi.T)
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 _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 __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)