def __init__(self, k=50.0, m=1.0, g=9.8, **kw):
self.k = k
self.m = m
self.g = g
self.dim = 2
self.z = map(S, ['z0', 'z1'])
self.p = map(S, ['p0', 'p1'])
self.q = map(S, ['q0', 'q1'])
self.x = map(S, ['x0', 'x1', 'x2', 'x3'])
self.u = map(S, ['u0', 'u1'])
self.Mq = np.eye(self.dim) * self.m
self.Mqi = np.eye(self.dim) / self.m
self.Vq = -self.m * self.g * self.q[1]
# create Ohm and Psi
self._Ohm = np.array([self.z[0], self.z[1] + sym.sin(self.z[0])])
self._Psi = np.array([self.q[0], self.q[1] - sym.sin(self.q[0])])
#self.controller = lambda t, x: [0, 0]
super(SinFloor2D, self).__init__(si=1)
def __init__(self, k=50.0, m=1.0, g=9.8, **kw):
self.k = k
self.m = m
self.g = g
self.dim = 2
self.z = map(S, ['z0', 'z1'])
self.p = map(S, ['p0', 'p1'])
self.q = map(S, ['q0', 'q1'])
self.x = map(S, ['x0', 'x1', 'x2', 'x3'])
self.u = map(S, ['u0', 'u1'])
self.Mq = np.eye(self.dim) * self.m
self.Mqi = np.eye(self.dim) / self.m
self.Vq = -self.m * self.g * self.q[1]
# create Ohm and Psi
self._Ohm = np.array([self.z[0], self.z[1] + sym.sin(self.z[0])])
self._Psi = np.array([self.q[0], self.q[1] - sym.sin(self.q[0])])
#self.controller = lambda t, x: [0, 0]
super(SinFloor2D, self).__init__(si=1)
def _makeP(self, k):
# builds the symbolic expression for the projection
# does NOT check for feasible/infeasible stuff
si = self.si
z = self.z
zs = z[si]
out = deepcopy(z)
out[si] = -zs
for i in range(len(z)):
if i != si:
out[i] = z[i] - self.delta[i] * 2 * zs / (1 + k * zs ** 2)
return np.array(out)
def _makeP(self, k):
# builds the symbolic expression for the projection
# does NOT check for feasible/infeasible stuff
si = self.si
z = self.z
zs = z[si]
out = deepcopy(z)
out[si] = -zs
for i in range(len(z)):
if i != si:
out[i] = z[i] - self.delta[i] * 2 * zs / (1 + k * zs**2)
return np.array(out)
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S
if __name__ == "__main__":
x1 = S('x1')
x2 = S('x2')
x = np.array([x1, x2])
f = np.array([x[0], x[1]**2, 3 * x[0] + x[1]])
print f
print tn.subs(f, {x1: 2.0, x2: 3.0})
print tn.eval(f, x, [2.0, 3.0])
A = tn.diff(f, x)
print A
print A[0, 1]
Anum = tn.eval(A, x, [2, 3])
print Anum[0, 1]
g = np.dot(A, x)
print g
g = tn.einsum('ij,j', A, x)
print g
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S
if __name__ == "__main__":
x1 = S('x1')
x2 = S('x2')
x = np.array([x1, x2])
f = np.array([x[0], x[1]**2, 3*x[0]+x[1]])
print f
print tn.subs(f, {x1:2.0, x2:3.0})
print tn.eval(f, x, [2.0, 3.0])
A = tn.diff(f, x)
print A
print A[0, 1]
Anum = tn.eval(A, x, [2,3])
print Anum[0,1]
g = np.dot(A, x)
print g
g = tn.einsum('ij,j', A, x)
print g
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S, sin, cos
from Cdynamics import WriteDynamics, WriteLinearizations
if __name__ == "__main__":
# x, u
x = np.array([S('x1'), S('x2')]) # theta, theta'
u = np.array([S('u1')]) # x''
# constants
g = S('m_g')
h = S('h')
# dynamics
f = np.array([x[1], (g * sin(x[0]) + cos(x[0]) * u[0]) / h])
# linearizations
A = tn.diff(f, x)
B = tn.diff(f, u)
# write to file
WriteDynamics(f, x, u)
WriteLinearizations(A, B, x, u)
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)
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S, sin, cos
from Cdynamics import WriteDynamics, WriteLinearizations
if __name__ == "__main__":
x = np.array([S('x1'), S('x2'), S('x3')]) # x, y, theta
u = np.array([S('u1'), S('u2')]) # v, omega
f = np.array([cos(x[2]) * u[0], sin(x[2]) * u[0], u[1]])
A = tn.diff(f, x)
B = tn.diff(f, u)
WriteDynamics(f, x, u)
WriteLinearizations(A, B, x, u)
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S, sin, cos
from Cdynamics import WriteDynamics, WriteLinearizations
if __name__ == "__main__":
# x, u
x = np.array([S('x1'),
S('x2'),
S('x3'),
S('x4'),
S('x5'),
S('x6')]) # x,y,theta, x',y',theta'
u = np.array([S('u1'), S('u2')]) # x''
# constants
g = S('g_')
eps = S('eps_')
# dynamics
f = np.array([
x[3], x[4], x[5], -u[0] * sin(x[2]) + eps * u[1] * cos(x[2]),
u[0] * cos(x[2]) + eps * u[1] * sin(x[2]) - g, u[1]
])
# linearizations
A = tn.diff(f, x)
B = tn.diff(f, u)
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)
from nlsymb import np, sym, matmult, tensor
import nlsymb.tensor as tn
from sympy import Symbol as S, sin, cos
from Cdynamics import WriteDynamics, WriteLinearizations
if __name__ == "__main__":
# x, u
x = np.array([S('x1'), S('x2'), S('x3'), S('x4')]) # theta, theta', x, x'
u = np.array([S('u1')]) # x''
# constants
g = S('m_g')
h = S('h')
# dynamics
f = np.array([x[1], (g * sin(x[0]) + cos(x[0]) * u[0]) / h, x[3], u[0]])
# linearizations
A = tn.diff(f, x)
B = tn.diff(f, u)
# write to file
WriteDynamics(f, x, u)
WriteLinearizations(A, B, x, u)