def _generate_eoms(self):
self.kane = me.KanesMethod(self.frames['inertial'],
self.coordinates.values(),
self.speeds.values(),
self.kin_diff_eqs)
fr, frstar = self.kane.kanes_equations(self.loads.values(),
self.rigid_bodies.values())
sub = {self.specified['platform_speed'].diff(self.time):
self.specified['platform_acceleration']}
self.fr_plus_frstar = sy.trigsimp(fr + frstar).subs(sub)
udots = sy.Matrix([s.diff(self.time) for s in self.speeds.values()])
m1 = self.fr_plus_frstar.diff(udots[0])
m2 = self.fr_plus_frstar.diff(udots[1])
# M x' = F
self.mass_matrix = -m1.row_join(m2)
self.forcing_vector = sy.simplify(self.fr_plus_frstar +
self.mass_matrix * udots)
M_top_rows = sy.eye(2).row_join(sy.zeros(2))
F_top_rows = sy.Matrix(self.speeds.values())
tmp = sy.zeros(2).row_join(self.mass_matrix)
self.mass_matrix_full = M_top_rows.col_join(tmp)
self.forcing_vector_full = F_top_rows.col_join(self.forcing_vector)
def compute_lambda(robo, symo, j, antRj, antPj, lam):
"""Internal function. Computes the inertia parameters
transformation matrix
Notes
=====
lam is the output paramete
"""
lamJJ_list = []
lamJMS_list = []
for e1 in xrange(3):
for e2 in xrange(e1, 3):
u = vec_mut_J(antRj[j][:, e1], antRj[j][:, e2])
if e1 != e2:
u += vec_mut_J(antRj[j][:, e2], antRj[j][:, e1])
lamJJ_list.append(u.T)
for e1 in xrange(3):
v = vec_mut_MS(antRj[j][:, e1], antPj[j])
lamJMS_list.append(v.T)
lamJJ = Matrix(lamJJ_list).T # , 'LamJ', j)
lamJMS = symo.mat_replace(Matrix(lamJMS_list).T, 'LamMS', j)
lamJM = symo.mat_replace(vec_mut_M(antPj[j]), 'LamM', j)
lamJ = lamJJ.row_join(lamJMS).row_join(lamJM)
lamMS = sympy.zeros(3, 6).row_join(antRj[j]).row_join(antPj[j])
lamM = sympy.zeros(1, 10)
lamM[9] = 1
lam[j] = Matrix([lamJ, lamMS, lamM])
def test_multi_mass_spring_damper_double():
m0, m1, c0, c1, k0, k1, g = sm.symbols('m0, m1, c0, c1, k0, k1, g')
x0, x1, v0, v1, f0, f1 = me.dynamicsymbols('x0, x1, v0, v1, f0, f1')
sys = multi_mass_spring_damper(2, True, True)
assert Counter(sys.constants_symbols) == \
Counter([c1, m1, k0, c0, k1, g, m0])
assert sys.specifieds_symbols == [f0, f1]
assert sys.coordinates == [x0, x1]
assert sys.speeds == [v0, v1]
assert sys.states == [x0, x1, v0, v1]
expected_mass_matrix_full = sm.Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, m0 + m1, m1],
[0, 0, m1, m1]])
expected_forcing_full = sm.Matrix([[v0],
[v1],
[-c0 * v0 + g * m0 + g * m1 - k0 * x0 + f0 + f1],
[-c1 * v1 + g * m1 - k1 * x1 + f1]])
assert sm.simplify(sys.eom_method.mass_matrix_full -
expected_mass_matrix_full) == sm.zeros(4, 4)
assert sm.simplify(sys.eom_method.forcing_full -
expected_forcing_full) == sm.zeros(4, 1)
def met_zeyd(A, b):
C, d = iteration_view(A, b)
H = sympy.zeros(3)
F = sympy.zeros(3)
c = sympy.zeros(3,1)
for i in xrange(3):
c[i] = d[i]
for j in xrange(3):
if i > j:
H[i, j] = C[i, j]
else:
F[i, j] = C[i, j]
print "\nx = Cx + d\n"
print "C = \n", C, "\n", "\nd = \n", d
print "\nConvergence: ", convergence_mzeyd(C, d)
if convergence_mzeyd(C, d):
E = sympy.eye(3)
x0 = sympy.ones(3, 1)
x1 = (E-H).inv()*F*x0 + (E-H).inv()*c
while ((x1-x0)[0] > 0.00001 or (x1-x0)[1] > 0.00001 or\
(x1-x0)[2] > 0.00001 or (x0-x1)[0] > 0.00001 or\
(x0-x1)[1] > 0.00001 or (x0-x1)[2] > 0.00001):
x0 = x1
x1 = (E-H).inv()*F*x0 + (E-H).inv()*c
print "\nSolution:"
return [element for element in x1]
def test_matrix_tensor_product():
l1 = zeros(4)
for i in range(16):
l1[i] = 2**i
l2 = zeros(4)
for i in range(16):
l2[i] = i
l3 = zeros(2)
for i in range(4):
l3[i] = i
vec = Matrix([1,2,3])
#test for Matrix known 4x4 matricies
numpyl1 = np.matrix(l1.tolist())
numpyl2 = np.matrix(l2.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, l2]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [l2, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for other known matrix of different dimensions
numpyl2 = np.matrix(l3.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, l3]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [l3, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for non square matrix
numpyl2 = np.matrix(vec.tolist())
numpy_product = np.kron(numpyl1,numpyl2)
args = [l1, vec]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
numpy_product = np.kron(numpyl2,numpyl1)
args = [vec, l1]
sympy_product = matrix_tensor_product(*args)
assert numpy_product.tolist() == sympy_product.tolist()
#test for random matrix with random values that are floats
random_matrix1 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
random_matrix2 = np.random.rand(np.random.rand()*5+1,np.random.rand()*5+1)
numpy_product = np.kron(random_matrix1,random_matrix2)
args = [Matrix(random_matrix1.tolist()),Matrix(random_matrix2.tolist())]
sympy_product = matrix_tensor_product(*args)
assert not (sympy_product - Matrix(numpy_product.tolist())).tolist() > \
(ones((sympy_product.rows,sympy_product.cols))*epsilon).tolist()
#test for three matrix kronecker
sympy_product = matrix_tensor_product(l1,vec,l2)
numpy_product = np.kron(l1,np.kron(vec,l2))
assert numpy_product.tolist() == sympy_product.tolist()
def get_symbolic_vandermonde(p_order, x_vals=None):
r"""Get symbolic Vandermonde matrix of evenly spaced points.
:type p_order: int
:param p_order: The degree of precision for the method.
:type x_vals: list
:param x_vals: (Optional) The list of :math:`x`-values to use. If not
given, defaults to ``p_order + 1`` evenly spaced points
on :math:`\left[0, 1\right]`.
:rtype: tuple
:returns: Pair of vector of powers of :math:`x` and Vandermonde matrix.
Both are type
:class:`sympy.Matrix <sympy.matrices.dense.MutableDenseMatrix>`,
the ``x_vec`` is a row vector with ``p_order + 1`` columns and
the Vandermonde matrix is square of dimension ``p_order + 1``.
"""
x_symb = sympy.Symbol('x')
if x_vals is None:
x_vals = sympy.Matrix(six.moves.xrange(p_order + 1)) / p_order
vand_mat = sympy.zeros(p_order + 1, p_order + 1)
x_vec = sympy.zeros(1, p_order + 1)
for i in six.moves.xrange(p_order + 1):
x_vec[i] = x_symb**i
for j in six.moves.xrange(p_order + 1):
vand_mat[i, j] = x_vals[i]**j
return x_vec, vand_mat
def gen_XdX(atom_list,operator_table,operator_table_dagger,Hcomm,N_atoms_uc):
"""Operate commutation relations to put all the 2nd order term as ckd**ck, cmk**cmkd, cmk**ck and ckd**cmkd form"""
print "gen_XdX"
exclude_list=[]
coeff_list=[]
print N_atoms_uc
N_int = max([max(atom_list[i].neighbors) for i in range(N_atoms_uc)])+1
XdX=sympy.zeros(2*N_atoms_uc)
g=sympy.zeros(2*N_atoms_uc)
for i in range(2*N_atoms_uc):
curr_row=[]
for j in range(2*N_atoms_uc):
mycoeff=operator_table_dagger[i]*operator_table[j]
exclude_list.append(mycoeff)
currcoeff1=coeff(Hcomm[i,j].expand(),mycoeff)
currcoeff2=coeff(Hcomm[j,i].expand(),mycoeff)
print Hcomm[i,j]
print mycoeff
print currcoeff1,currcoeff2
if currcoeff1!=None and currcoeff2!=None:
XdX[i,j]=currcoeff1+currcoeff2
curr_row.append(currcoeff1+currcoeff2)
if i!=j:
g[i,j]=0
else:
if i>=N_atoms_uc:
g[i,j]=-1
else:
g[i,j]=1
print 'XdX done'
return XdX,g
def rne_park_backward(rbtdef, geom, fw_results, ifunc=None):
'''RNE backward pass.'''
V, dV = fw_results
if not ifunc:
ifunc = identity
# extend Tdh_inv so that Tdh_inv[dof] return identity
Tdh_inv = geom.Tdh_inv + [eye(4)]
F = list(range(rbtdef.dof + 1))
F[rbtdef.dof] = zeros((6, 1))
tau = zeros((rbtdef.dof, 1))
fric = frictionforce(rbtdef)
Idrive = driveinertiaterm(rbtdef)
# Backward
for i in range(rbtdef.dof - 1, -1, -1):
Llm = (rbtdef.L[i].row_join(skew(rbtdef.l[i]))).col_join(
(-skew(rbtdef.l[i])).row_join(eye(3) * rbtdef.m[i]))
F[i] = Adjdual(Tdh_inv[i + 1], F[i + 1]) + \
Llm * dV[i] - adjdual(V[i], Llm * V[i])
F[i] = ifunc(F[i])
tau[i] = ifunc((geom.S[i].transpose() * F[i])[0] + fric[i] + Idrive[i])
return tau
def get_linear_system(eqs):
diff_eqs = eqs.substituted_expressions
diff_eq_names = eqs.diff_eq_names
symbols = [Symbol(name) for name in diff_eq_names]
# Coefficients
wildcards = [Wild('c_' + name, exclude=symbols) for name in diff_eq_names]
#Additive constant
constant_wildcard = Wild('c', exclude=symbols)
pattern = reduce(operator.add, [c * s for c, s in zip(wildcards, symbols)])
pattern += constant_wildcard
coefficients = sp.zeros(len(diff_eq_names))
constants = sp.zeros((len(diff_eq_names), 1))
for row_idx, (name, expr) in enumerate(diff_eqs):
s_expr = sympify(expr, locals=dict([(s.name, s) for s in symbols])).expand()
#print s_expr.expand()
pattern_matches = s_expr.match(pattern)
if pattern_matches is None:
raise ValueError(('The expression "%s", defining the variable %s, '
'could not be separated into linear components') %
(str(s_expr), name))
for col_idx in xrange(len(diff_eq_names)):
coefficients[row_idx, col_idx] = pattern_matches[wildcards[col_idx]]
constants[row_idx] = pattern_matches[constant_wildcard]
return (diff_eq_names, coefficients, constants)
def mass_matrix_full(self):
# Returns the mass matrix from above, augmented by kin diff's k_kqdot
if (self._frstar is None) & (self._fr is None):
raise ValueError("Need to compute Fr, Fr* first.")
o = len(self._u)
n = len(self._q)
return ((self._k_kqdot).row_join(zeros(n, o))).col_join((zeros(o, n)).row_join(self.mass_matrix))
def rne_park_forward(rbtdef, geom, ifunc=None):
'''RNE forward pass.'''
if not ifunc:
ifunc = identity
V = list(range(0, rbtdef.dof + 1))
dV = list(range(0, rbtdef.dof + 1))
V[-1] = zeros((6, 1))
dV[-1] = - zeros((3, 1)).col_join(rbtdef.gravityacc)
# Forward
for i in range(rbtdef.dof):
V[i] = ifunc(Adj(geom.Tdh_inv[i], V[i - 1])) + \
ifunc(geom.S[i] * rbtdef.dq[i])
V[i] = ifunc(V[i])
dV[i] = ifunc(geom.S[i] * rbtdef.ddq[i]) + \
ifunc(Adj(geom.Tdh_inv[i], dV[i - 1])) + \
ifunc(adj(ifunc(Adj(geom.Tdh_inv[i], V[i - 1])),
ifunc(geom.S[i] * rbtdef.dq[i])))
dV[i] = ifunc(dV[i])
return V, dV
def GetSymbolicMatrices(func,obsVariablesArray,unknownsArray,rowsForB):
B=sp.zeros(rowsForB*len(func), len(obsVariablesArray)*rowsForB)
# print B
A=sp.zeros(rowsForB*len(func), len(unknownsArray))
V=sp.zeros(len(obsVariablesArray)*rowsForB,1)
X=sp.zeros(len(unknownsArray),1)
#increment by size of function array (ADDING MULTIPLE FUNCTION FUNCTIONALITY)
#loop through fun array with counter, fill in diff/wrt for each funtion while count is less than length of function array
increment=len(func)
vcount=0
#range loop doesnt work so making a variable to use for indexing correct row.. i will be as if func had only 1 funtion
row=0
for i in Range(rowsForB):
for j in Range(len(obsVariablesArray)):
for level in Range(len(func)):
B[row+level,vcount]=diff(func[level],obsVariablesArray[j])
t=symbols('v'+obsVariablesArray[j].name+str(i+1)) #not sure if must be row as well
V[vcount,0]=t
vcount+=1
row+=increment
# pprint (B)
row=0
for i in Range(rowsForB):
for j in Range(len(unknownsArray)):
for level in Range(len(func)):
A[row+level,j]=diff(func[level],unknownsArray[j])
t=symbols('d'+unknownsArray[j].name)
X[j,0]=t
row+=increment
return B,V,A,X
def test_form_2():
symsystem2 = SymbolicSystem(
coordinates,
comb_implicit_rhs,
speeds=speeds,
mass_matrix=comb_implicit_mat,
alg_con=alg_con_full,
output_eqns=out_eqns,
bodies=bodies,
loads=loads,
)
assert symsystem2.coordinates == Matrix([x, y, lam])
assert symsystem2.speeds == Matrix([u, v])
assert symsystem2.states == Matrix([x, y, lam, u, v])
assert symsystem2.alg_con == [4]
inter = comb_implicit_rhs
assert simplify(symsystem2.comb_implicit_rhs - inter) == zeros(5, 1)
assert simplify(symsystem2.comb_implicit_mat - comb_implicit_mat) == zeros(5)
assert set(symsystem2.dynamic_symbols()) == set([y, v, lam, u, x])
assert type(symsystem2.dynamic_symbols()) == tuple
assert set(symsystem2.constant_symbols()) == set([l, g, m])
assert type(symsystem2.constant_symbols()) == tuple
inter = comb_explicit_rhs
symsystem2.compute_explicit_form()
assert simplify(symsystem2.comb_explicit_rhs - inter) == zeros(5, 1)
assert symsystem2.output_eqns == out_eqns
assert symsystem2.bodies == (Pa,)
assert symsystem2.loads == ((P, g * m * N.x),)
def test_n_link_pendulum_on_cart_inputs():
l0, m0 = symbols("l0 m0")
m1 = symbols("m1")
g = symbols("g")
q0, q1, F, T1 = dynamicsymbols("q0 q1 F T1")
u0, u1 = dynamicsymbols("u0 u1")
kane1 = models.n_link_pendulum_on_cart(1)
massmatrix1 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing1 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix1 - kane1.mass_matrix) == zeros(2)
assert simplify(forcing1 - kane1.forcing) == Matrix([0, 0])
kane2 = models.n_link_pendulum_on_cart(1, False)
massmatrix2 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing2 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix2 - kane2.mass_matrix) == zeros(2)
assert simplify(forcing2 - kane2.forcing) == Matrix([0, 0])
kane3 = models.n_link_pendulum_on_cart(1, False, True)
massmatrix3 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing3 = Matrix([[-l0*m1*u1**2*sin(q1)], [g*l0*m1*sin(q1) + T1]])
assert simplify(massmatrix3 - kane3.mass_matrix) == zeros(2)
assert simplify(forcing3 - kane3.forcing) == Matrix([0, 0])
kane4 = models.n_link_pendulum_on_cart(1, True, False)
massmatrix4 = Matrix([[m0 + m1, -l0*m1*cos(q1)],
[-l0*m1*cos(q1), l0**2*m1]])
forcing4 = Matrix([[-l0*m1*u1**2*sin(q1) + F], [g*l0*m1*sin(q1)]])
assert simplify(massmatrix4 - kane4.mass_matrix) == zeros(2)
assert simplify(forcing4 - kane4.forcing) == Matrix([0, 0])
def least_squares_non_verbose(f, psi, Omega, symbolic=True):
"""
Given a function f(x) on an interval Omega (2-list)
return the best approximation to f(x) in the space V
spanned by the functions in the list psi.
"""
N = len(psi) - 1
A = sym.zeros((N+1, N+1))
b = sym.zeros((N+1, 1))
x = sym.Symbol('x')
for i in range(N+1):
for j in range(i, N+1):
integrand = psi[i]*psi[j]
integrand = sym.lambdify([x], integrand)
I = sym.mpmath.quad(integrand, [Omega[0], Omega[1]])
A[i,j] = A[j,i] = I
integrand = psi[i]*f
integrand = sym.lambdify([x], integrand)
I = sym.mpmath.quad(integrand, [Omega[0], Omega[1]])
b[i,0] = I
c = sym.mpmath.lu_solve(A, b) # numerical solve
c = [c[i,0] for i in range(c.rows)]
u = sum(c[i]*psi[i] for i in range(len(psi)))
return u, c
def _form_coefficient_matrices(self):
"""Form the coefficient matrices C_0, C_1, and C_2."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Build up the coefficient matrices C_0, C_1, and C_2
# If there are configuration constraints (l > 0), form C_0 as normal.
# If not, C_0 is I_(nxn). Note that this works even if n=0
if l > 0:
f_c_jac_q = self.f_c.jacobian(self.q)
self._C_0 = (eye(n) - self._Pqd * (f_c_jac_q *
self._Pqd).LUsolve(f_c_jac_q)) * self._Pqi
else:
self._C_0 = eye(n)
# If there are motion constraints (m > 0), form C_1 and C_2 as normal.
# If not, C_1 is 0, and C_2 is I_(oxo). Note that this works even if
# o = 0.
if m > 0:
f_v_jac_u = self.f_v.jacobian(self.u)
temp = f_v_jac_u * self._Pud
if n != 0:
f_v_jac_q = self.f_v.jacobian(self.q)
self._C_1 = -self._Pud * temp.LUsolve(f_v_jac_q)
else:
self._C_1 = zeros(o, n)
self._C_2 = (eye(o) - self._Pud *
temp.LUsolve(f_v_jac_u)) * self._Pui
else:
self._C_1 = zeros(o, n)
self._C_2 = eye(o)
def interpolation(f, psi, points):
"""
Given a function f(x), return the approximation to
f(x) in the space V, spanned by psi, that interpolates
f at the given points. Must have len(points) = len(psi)
"""
N = len(psi) - 1
A = sym.zeros((N+1, N+1))
b = sym.zeros((N+1, 1))
# Wrap psi and f in Python functions rather than expressions
# so that we can evaluate psi at points[i] (alternative to subs?)
psi_sym = psi # save symbolic expression
x = sym.Symbol('x')
psi = [sym.lambdify([x], psi[i]) for i in range(N+1)]
f = sym.lambdify([x], f)
print '...evaluating matrix...'
for i in range(N+1):
for j in range(N+1):
print '(%d,%d)' % (i, j)
A[i,j] = psi[j](points[i])
b[i,0] = f(points[i])
print
print 'A:\n', A, '\nb:\n', b
c = A.LUsolve(b)
# c is a sympy Matrix object, turn to list
c = [sym.simplify(c[i,0]) for i in range(c.shape[0])]
print 'coeff:', c
# u = sym.simplify(sum(c[i,0]*psi_sym[i] for i in range(N+1)))
u = sym.simplify(sum(c[i]*psi_sym[i] for i in range(N+1)))
print 'approximation:', u
return u, c
def inertiamatrix(rbtdef, geom, ifunc=None):
'''Generate mass matrix.'''
if not ifunc:
ifunc = identity
M = zeros((rbtdef.dof, rbtdef.dof))
rbtdeftmp = deepcopy(rbtdef)
rbtdeftmp.gravityacc = zeros((3, 1))
rbtdeftmp.frictionmodel = None
rbtdeftmp.dq = zeros((rbtdeftmp.dof, 1))
for i in range(M.rows):
rbtdeftmp.ddq = zeros((rbtdeftmp.dof, 1))
rbtdeftmp.ddq[i] = 1
geomtmp = Geometry(rbtdeftmp)
fw_results = rne_forward(rbtdeftmp, geomtmp, ifunc)
Mcoli = rne_backward(rbtdeftmp, geomtmp, fw_results, ifunc=ifunc)
# It's done like this since M is symmetric:
M[:, i] = (M[i, :i].T) .col_join(Mcoli[i:, :])
return M
def krivodInterpolate(self, nodes, order, c_fac, x):
"""
Krivodnova interpolation matrix at a particular point
"""
l_I = np.zeros((len(nodes)))
r = sympy.Symbol('r')
van = self.vandermonde(order, nodes)
phi = self.lagrange(nodes)
Np = order + 1
P = sympy.zeros(1, Np)
# Leg from Lag
for i in range(Np):
for j in range(Np):
P[i] = P[i] + van.T[i, j] * phi[j] # V^T . l
P[Np - 1] = c_fac*P[Np - 1]
# Lag from Leg
inv_vanT = np.linalg.inv(van.T)
lag = sympy.zeros(1, Np)
for i in range(Np):
for j in range(Np):
lag[i] = lag[i] + inv_vanT[i, j] * P[j] # (V^T)^-1 . P
for i in range(len(nodes)):
l_I[i] = lag[i].evalf(subs = {r: x})
return l_I
def __init__( self , name ): self.name = name self.linPJac = sympy.zeros( (order,order) ) self.linPVal = sympy.zeros( (order, 1) ) self.B = sympy.zeros ( (order , 1 * len( input_list ) ) ) #Changed for Model3 self.inpVal = sympy.zeros( (len( input_list) , 1) ) self.Bnew = numpy.concatenate( ( numpy.array(self.B), numpy.array(self.linPVal) ) , 1 )
def gen_XdX_old(atom_list,operator_table,operator_table_dagger,Hcomm,N_atoms_uc):
"""Operate commutation relations to put all the 2nd order term as ckd**ck, cmk**cmkd, cmk**ck and ckd**cmkd form"""
print "gen_XdX"
exclude_list=[]
coeff_list=[]
#Hcomm=Hcomm.expand(deep = False)
XdX=sympy.zeros(2*N_atoms_uc)
g=sympy.zeros(2*N_atoms_uc)
for i in range(2*N_atoms_uc):
curr_row=[]
for j in range(2*N_atoms_uc):
mycoeff=operator_table_dagger[i]*operator_table[j]
print mycoeff
exclude_list.append(mycoeff)
currcoeff=Hcomm.coeff(mycoeff)
if currcoeff!=None:
XdX[i,j]=currcoeff
curr_row.append(currcoeff)
if i!=j:
g[i,j]=0
else:
if i>=N_atoms_uc:
g[i,j]=-1
else:
g[i,j]=1
print 'XdX done'
return XdX,g
def test_submatrix_assignment():
m = zeros(4)
m[2:4, 2:4] = eye(2)
assert m == Matrix(((0,0,0,0),
(0,0,0,0),
(0,0,1,0),
(0,0,0,1)))
m[0:2, 0:2] = eye(2)
assert m == eye(4)
m[:,0] = Matrix(4,1,(1,2,3,4))
assert m == Matrix(((1,0,0,0),
(2,1,0,0),
(3,0,1,0),
(4,0,0,1)))
m[:,:] = zeros(4)
assert m == zeros(4)
m[:,:] = ((1,2,3,4),(5,6,7,8),(9, 10, 11, 12),(13,14,15,16))
assert m == Matrix(((1,2,3,4),
(5,6,7,8),
(9, 10, 11, 12),
(13,14,15,16)))
m[0:2, 0] = [0,0]
assert m == Matrix(((0,2,3,4),
(0,6,7,8),
(9, 10, 11, 12),
(13,14,15,16)))
def gen_XdX(atom_list,operator_table,operator_table_dagger,Hcomm,N_atoms_uc):
"""Operate commutation relations to put all the 2nd order term as ckd**ck, cmk**cmkd, cmk**ck and ckd**cmkd form"""
exclude_list=[]
coeff_list=[]
Hcomm=Hcomm.expand()
XdX=sympy.zeros(N_atoms_uc)
g=sympy.zeros(N_atoms_uc)
for i in range(N_atoms_uc):
curr_row=[]
for j in range(N_atoms_uc):
mycoeff=operator_table_dagger[i]*operator_table[j]
exclude_list.append(mycoeff)
currcoeff=coeff(Hcomm,mycoeff)
if currcoeff!=None:
XdX[i,j]=currcoeff
curr_row.append(currcoeff)
if i!=j:
g[i,j]=0
else:
if i>=N_atoms_uc:
g[i,j]=-1
else:
g[i,j]=1
return XdX,g
def interpolation(f, phi, points):
"""
Given a function f(x), return the approximation to
f(x) in the space V, spanned by phi, that interpolates
f at the given points. Must have len(points) = len(phi)
"""
N = len(phi) - 1
A = sm.zeros((N+1, N+1))
b = sm.zeros((N+1, 1))
# Wrap phi and f in Python functions rather than expressions
# so that we can evaluate phi at points[i] (alternative to subs?)
x = sm.Symbol('x')
phi = [sm.lambdify([x], phi[i]) for i in range(N+1)]
f = sm.lambdify([x], f)
print '...evaluating matrix...'
for i in range(N+1):
for j in range(N+1):
print '(%d,%d)' % (i, j)
A[i,j] = phi[j](points[i])
b[i,0] = f(points[i])
print
print 'A:\n', A, '\nb:\n', b
c = A.LUsolve(b)
print 'coeff:', c
u = 0
for i in range(len(phi)):
u += c[i,0]*phi[i](x)
# Alternative:
# u = sum(c[i,0]*phi[i] for i in range(len(phi)))
print 'approximation:', u
return u
def least_squares(f, psi, Omega, symbolic=True, print_latex=False):
"""
Given a function f(x,y) on a rectangular domain
Omega=[[xmin,xmax],[ymin,ymax]],
return the best approximation to f(x,y) in the space V
spanned by the functions in the list psi.
"""
N = len(psi) - 1
A = sym.zeros((N+1, N+1))
b = sym.zeros((N+1, 1))
x, y = sym.symbols('x y')
print '...evaluating matrix...'
for i in range(N+1):
for j in range(i, N+1):
print '(%d,%d)' % (i, j)
integrand = psi[i]*psi[j]
if symbolic:
I = sym.integrate(integrand,
(x, Omega[0][0], Omega[0][1]),
(y, Omega[1][0], Omega[1][1]))
if not symbolic or isinstance(I, sym.Integral):
# Could not integrate symbolically, use numerical int.
print 'numerical integration of', integrand
integrand = sym.lambdify([x,y], integrand)
I = sym.mpmath.quad(integrand,
[Omega[0][0], Omega[0][1]],
[Omega[1][0], Omega[1][1]])
A[i,j] = A[j,i] = I
integrand = psi[i]*f
if symbolic:
I = sym.integrate(integrand,
(x, Omega[0][0], Omega[0][1]),
(y, Omega[1][0], Omega[1][1]))
if not symbolic or isinstance(I, sym.Integral):
# Could not integrate symbolically, use numerical int.
print 'numerical integration of', integrand
integrand = sym.lambdify([x,y], integrand)
I = sym.mpmath.quad(integrand,
[Omega[0][0], Omega[0][1]],
[Omega[1][0], Omega[1][1]])
b[i,0] = I
print
print 'A:\n', A, '\nb:\n', b
if symbolic:
c = A.LUsolve(b) # symbolic solve
# c is a sympy Matrix object, numbers are in c[i,0]
c = [c[i,0] for i in range(c.shape[0])]
else:
c = sym.mpmath.lu_solve(A, b) # numerical solve
print 'coeff:', c
u = sum(c[i]*psi[i] for i in range(len(psi)))
print 'approximation:', u
print 'f:', sym.expand(f)
if print_latex:
print sym.latex(A, mode='plain')
print sym.latex(b, mode='plain')
print sym.latex(c, mode='plain')
return u, c
def _form_permutation_matrices(self):
"""Form the permutation matrices Pq and Pu."""
# Extract dimension variables
l, m, n, o, s, k = self._dims
# Compute permutation matrices
if n != 0:
self._Pq = permutation_matrix(self.q, Matrix([self.q_i, self.q_d]))
if l > 0:
self._Pqi = self._Pq[:, :-l]
self._Pqd = self._Pq[:, -l:]
else:
self._Pqi = self._Pq
self._Pqd = Matrix()
if o != 0:
self._Pu = permutation_matrix(self.u, Matrix([self.u_i, self.u_d]))
if m > 0:
self._Pui = self._Pu[:, :-m]
self._Pud = self._Pu[:, -m:]
else:
self._Pui = self._Pu
self._Pud = Matrix()
# Compute combination permutation matrix for computing A and B
P_col1 = Matrix([self._Pqi, zeros(o + k, n - l)])
P_col2 = Matrix([zeros(n, o - m), self._Pui, zeros(k, o - m)])
if P_col1:
if P_col2:
self.perm_mat = P_col1.row_join(P_col2)
else:
self.perm_mat = P_col1
else:
self.perm_mat = P_col2
def _computeTransitionMeanVar(self):
'''
This is the mean and variance information that we need
for the :math:`\tau`-Leap
'''
if self._transitionJacobian is None or self._hasNewTransition:
self._computeTransitionJacobian()
F = self._transitionJacobian
# holders
mu = sympy.zeros(self._numTransition, 1)
sigma2 = sympy.zeros(self._numTransition, 1)
# we calculate the mean and variance
for i in range(self._numTransition):
for j, eqn in enumerate(self._transitionVector):
mu[i] += F[i,j] * eqn
sigma2[i] += F[i,j] * F[i,j] * eqn
self._transitionMean = mu
self._transitionVar = sigma2
# now we are going to compile them
if self._isDifficult:
self._transitionMeanCompile = self._SC.compileExprAndFormat(self._sp,
self._transitionMean,
modules=modulesH)
self._transitionVarCompile = self._SC.compileExprAndFormat(self._sp,
self._transitionVar,
modules=modulesH)
else:
self._transitionMeanCompile = self._SC.compileExprAndFormat(self._sp, self._transitionMean)
self._transitionVarCompile = self._SC.compileExprAndFormat(self._sp, self._transitionVar)
return None
def regular_completion(matrix):
m,n = matrix.shape
r = matrix.rank()
#~ IPS()
assert m!=n, "There is no regular completion of a square matrix."
if m<n:
assert r==m, "Matrix does not have full row rank."
A, B, V_pi = reshape_matrix_columns(matrix)
zeros = sp.zeros(n-m,m)
ones = sp.eye(n-m)
S = st.col_stack(zeros,ones)
completion = S*V_pi.T
regular_matrix = st.row_stack(matrix,completion)
assert st.rnd_number_rank(regular_matrix)==n, "Regular completion seems to be wrong."
elif n<m:
assert r==n, "Matrix does not have full column rank."
A, B, V_pi = reshape_matrix_columns(matrix.T)
zeros = sp.zeros(m-n,n)
ones = sp.eye(m-n)
S = st.col_stack(zeros,ones)
completion = V_pi*S.T
regular_matrix = st.col_stack(completion,matrix)
assert st.rnd_number_rank(regular_matrix)==m, "Regular completion seems to be wrong."
return completion
def __get_trace_facvar(self, polynomial):
"""Return dense vector representation of a polynomial. This function is
nearly identical to __push_facvar_sparse, but instead of pushing
sparse entries to the constraint matrices, it returns a dense
vector.
"""
facvar = [0] * (self.n_vars + 1)
F = {}
for i in range(self.matrix_var_dim):
for j in range(self.matrix_var_dim):
for key, value in \
polynomial[i, j].as_coefficients_dict().items():
skey = apply_substitutions(key, self.substitutions,
self.pure_substitution_rules)
try:
Fk = F[skey]
except KeyError:
Fk = zeros(self.matrix_var_dim, self.matrix_var_dim)
Fk[i, j] += value
F[skey] = Fk
# This is the tracing part
for key, Fk in F.items():
if key == S.One:
k = 1
else:
k = self.monomial_index[key]
for i in range(self.matrix_var_dim):
for j in range(self.matrix_var_dim):
sym_matrix = zeros(self.matrix_var_dim,
self.matrix_var_dim)
sym_matrix[i, j] = 1
facvar[k+i*self.matrix_var_dim+j] = (sym_matrix*Fk).trace()
facvar = [float(f) for f in facvar]
return facvar
def getKrivodLeft(self, order, nodes, correcFac):
'''
Convert Lagrange poly to normalized Legendre
'''
r = sympy.Symbol('r')
van = self.vandermonde(order, nodes)
phi = self.lagrange(nodes)
Np = order + 1
P = sympy.zeros(1, Np)
# Leg from Lag
for i in range(Np):
for j in range(Np):
P[i] = P[i] + van.T[i, j] * phi[j] # V^T . l
P[Np - 1] = correcFac*P[Np - 1]
# Lag from Leg
inv_vanT = np.linalg.inv(van.T)
lag = sympy.zeros(1, Np)
for i in range(Np):
for j in range(Np):
lag[i] = lag[i] + inv_vanT[i, j] * P[j] # (V^T)^-1 . P
k_l = np.zeros(Np)
wts = np.polynomial.legendre.leggauss(Np)[1]
for i in range(Np):
k_l[i] = -1*lag[i].evalf(subs = {r: -1})/wts[i]
return k_l
def main():
sympy.var('xi, eta, lex, ley, rho')
sympy.var('R')
sympy.var('A11, A12, A16, A22, A26, A66')
sympy.var('B11, B12, B16, B22, B26, B66')
sympy.var('D11, D12, D16, D22, D26, D66')
ONE = sympy.Integer(1)
# shape functions
# - from Reference:
# OCHOA, O. O.; REDDY, J. N. Finite Element Analysis of Composite Laminates. Dordrecht: Springer, 1992.
# bi-linear
Li = lambda xii, etai: ONE / 4. * (1 + xi * xii) * (1 + eta * etai)
# cubic
Hwi = lambda xii, etai: ONE / 16. * (xi + xii)**2 * (xi * xii - 2) * (
eta + etai)**2 * (eta * etai - 2)
Hwxi = lambda xii, etai: -lex / 32. * xii * (xi + xii)**2 * (
xi * xii - 1) * (eta + etai)**2 * (eta * etai - 2)
Hwyi = lambda xii, etai: -ley / 32. * (xi + xii)**2 * (
xi * xii - 2) * etai * (eta + etai)**2 * (eta * etai - 1)
Hwxyi = lambda xii, etai: lex * ley / 64. * xii * (xi + xii)**2 * (
xi * xii - 1) * etai * (eta + etai)**2 * (eta * etai - 1)
Nu = sympy.Matrix([[
Li(-1, -1),
0,
0,
0,
0,
0,
Li(+1, -1),
0,
0,
0,
0,
0,
Li(+1, +1),
0,
0,
0,
0,
0,
Li(-1, +1),
0,
0,
0,
0,
0,
]])
Nv = sympy.Matrix([[
0,
Li(-1, -1),
0,
0,
0,
0,
0,
Li(+1, -1),
0,
0,
0,
0,
0,
Li(+1, +1),
0,
0,
0,
0,
0,
Li(-1, +1),
0,
0,
0,
0,
]])
Nw = sympy.Matrix([[
#u, v, w, , dw/dx , dw/dy, d2w/(dxdy)
0,
0,
Hwi(-1, -1),
Hwxi(-1, -1),
Hwyi(-1, -1),
Hwxyi(-1, -1), # node 1 (-1, -1)
0,
0,
Hwi(+1, -1),
Hwxi(+1, -1),
Hwyi(+1, -1),
Hwxyi(+1, -1), # node 2 (+1, -1)
0,
0,
Hwi(+1, +1),
Hwxi(+1, +1),
Hwyi(+1, +1),
Hwxyi(+1, +1), # node 3 (+1, +1)
0,
0,
Hwi(-1, +1),
Hwxi(-1, +1),
Hwyi(-1, +1),
Hwxyi(-1, +1), # node 4 (-1, +1)
]])
A = sympy.Matrix([[A11, A12, A16], [A12, A22, A26], [A16, A26, A66]])
B = sympy.Matrix([[B11, B12, B16], [B12, B22, B26], [B16, B26, B66]])
D = sympy.Matrix([[D11, D12, D16], [D12, D22, D26], [D16, D26, D66]])
# membrane
Nu_x = (2 / lex) * Nu.diff(xi)
Nu_y = (2 / ley) * Nu.diff(eta)
Nv_x = (2 / lex) * Nv.diff(xi)
Nv_y = (2 / ley) * Nv.diff(eta)
Bm = sympy.Matrix([Nu_x, Nv_y + 1 / R * Nw, Nu_y + Nv_x])
# bending
Nphix = -(2 / lex) * Nw.diff(xi)
Nphiy = -(2 / ley) * Nw.diff(eta)
Nphix_x = (2 / lex) * Nphix.diff(xi)
Nphix_y = (2 / ley) * Nphix.diff(eta)
Nphiy_x = (2 / lex) * Nphiy.diff(xi)
Nphiy_y = (2 / ley) * Nphiy.diff(eta)
Bb = sympy.Matrix([Nphix_x, Nphiy_y, Nphix_y + Nphiy_x])
print('Bm')
for ind, val in np.ndenumerate(Bm):
if val != 0:
print(ind, val)
print('Bb')
for ind, val in np.ndenumerate(Bb):
if val != 0:
print(ind, val)
# Geometric stiffness matrix using Donnell's type of geometric nonlinearity
# (or van Karman shell nonlinear terms)
sympy.var('Nxx, Nyy, Nxy')
Nmatrix = sympy.Matrix([[Nxx, Nxy], [Nxy, Nyy]])
G = sympy.Matrix([(2 / lex) * Nw.diff(xi), (2 / ley) * Nw.diff(eta)])
Kge = sympy.zeros(4 * DOF, 4 * DOF)
Kge[:, :] = (lex * ley) / 4. * G.T * Nmatrix * G
print('Integrands for Kge')
p = Pool(cpu_count)
tmp = []
for ind, integrand in np.ndenumerate(Kge):
tmp.append(integrand)
tmp = p.map(sympy.simplify, tmp)
for i, (ind, integrand) in enumerate(np.ndenumerate(Kge)):
Kge[ind] = tmp[i]
Kg = Kge
def name_ind(i):
if i >= 0 and i < DOF:
return 'c1'
elif i >= DOF and i < 2 * DOF:
return 'c2'
elif i >= 2 * DOF and i < 3 * DOF:
return 'c3'
elif i >= 3 * DOF and i < 4 * DOF:
return 'c4'
else:
raise
print('printing for code')
for ind, val in np.ndenumerate(Kg):
i, j = ind
si = name_ind(i)
sj = name_ind(j)
print(' Kg[%d+%s, %d+%s]' % (i % DOF, si, j % DOF, sj),
'+= wi*(', Kg[ind], ')')
def Hurwitz_sp(Yk):
'''
Funkcja sprawdzajaca stabilnosc transmitancji
INPUT: obiekt sp.Function
OUTPUT: macierz Hurwitza transmitancji(tex syndax),
wektor skladajacy się z wartosci podwyznacznikow macierzy
'''
# Zainicjowanie symboli
M = sp.Function('M')(s)
L = sp.Function('L')(s)
Y = sp.Function('Y')(s)
Y = Yk
#Y=sp.parse_expr(str,Yk)
L, M = sp.fraction(Y) # podział na licznik i mianownik
Ywspolczynniki = sp.Poly(M, s)
Ywspolczynniki = Ywspolczynniki.all_coeffs()
# Rozpoczynam algorytm kryterium Hurwitza
Ystopien = len(Ywspolczynniki) - 1
# Tworze wektory potrzebne do stworzenia macierzy Hurwitza
pTabParz = sp.Matrix([[]])
pTabNParz = sp.Matrix([[]])
pm = 0
pn = 0
for x in range(Ystopien):
pm = x * 2
pn = x * 2 + 1
if pm <= Ystopien:
pTabParz = pTabParz.col_insert(x, sp.Matrix([Ywspolczynniki[pm]]))
else:
pTabParz = pTabParz.col_insert(x, sp.Matrix([0]))
if pn <= Ystopien:
pTabNParz = pTabNParz.col_insert(x,
sp.Matrix([Ywspolczynniki[pn]]))
else:
pTabNParz = pTabNParz.col_insert(x, sp.Matrix([0]))
# Deklaruje macierz Hurwitza jako macierz zer
macierzHurwitza = sp.zeros(Ystopien, Ystopien)
# Algorytm zapusijacy macierz Hurwitza
parzyste = False
if Ystopien == 0:
macierzHurwitza = (sp.ones(1, 1)) / Y
else:
for x in range(Ystopien):
if (parzyste == False):
macierzHurwitza.row_del(x)
macierzHurwitza = macierzHurwitza.row_insert(x, pTabNParz)
pTabNParz = pTabNParz.col_insert(0, sp.Matrix([0]))
pTabNParz.col_del(Ystopien)
parzyste = (not parzyste)
elif (parzyste == True):
macierzHurwitza.row_del(x)
macierzHurwitza = macierzHurwitza.row_insert(x, pTabParz)
pTabParz = pTabParz.col_insert(0, sp.Matrix([0]))
pTabParz.col_del(Ystopien)
parzyste = (not parzyste)
# Algorytm sprawdzajacy czy wszystkie podwyznaczniki sa dodatnie
macierzHurwitzaPomocnicza = macierzHurwitza.copy()
wyznacznikiHurwitza = []
for x in range(Ystopien):
wyznacznikiHurwitza.append(macierzHurwitzaPomocnicza.det())
if macierzHurwitzaPomocnicza.shape[0] > 0:
macierzHurwitzaPomocnicza.col_del(
macierzHurwitzaPomocnicza.shape[0] - 1)
macierzHurwitzaPomocnicza.row_del(
macierzHurwitzaPomocnicza.shape[0] - 1)
# Jesli wszystkie podwyznaczniki sa dodatnie to zmienna stabilnoscHurwitza jest rowna 1,
# w przeciwnym wypadku jest rowna 0
return str(sp.latex(macierzHurwitza)), str(sp.latex(wyznacznikiHurwitza))
def createMatrix(eqs: list, symbols: list) -> sympy.Matrix:
A = sympy.zeros(len(eqs), len(eqs))
for i, symbol in enumerate(symbols, start=0):
for j, eq in enumerate(eqs, start=0):
A[i, j] = sympy.diff(eq, symbol)
return A
linearize = [(sympy.sin(θ), 0), (sympy.cos(θ), 1), (θ.diff(t), 0),
(θ.diff(t)**2, 0), (θ.diff(t, t), 0), (y.diff(t), 0),
(y.diff(t, t), 0)]
A_lin = sympy.simplify(A.subs(linearize))
constants = {g: 9.81, l: 1.0, m_m: 2.0, m_cart: 2.0}
Jacobian = A_lin.subs(constants)
x_0 = sympy.Matrix([1.0, 0.5, 0.3, -0.02, 2.15, 0])
dt = 0.01
timeline = np.arange(0.0, 5, dt)
result = sympy.zeros(len(x_0), len(timeline))
resultA = sympy.zeros(len(x_0), len(timeline))
result[:, 0] = x_0
resultA[:, 0] = x_0
for i in range(len(timeline) - 1):
linearize = [(θ.diff(t, t), resultA[5, i]), (θ.diff(t), resultA[4, i]),
(θ, resultA[3, i]), (x.diff(t, t), 0),
(x.diff(t), resultA[2, i]), (x, resultA[1, i]),
(y.diff(t, t), 0), (y.diff(t), 0)]
result[:, i + 1] = Jacobian * result[:, i] * dt + result[:, i]
resultA[:, i + 1] = A.subs(linearize).subs(constants) * \
resultA[:, i] * dt + resultA[:, i]
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Sep 25 14:18:06 2020
@author: hanjiya
"""
from sympy import Matrix, symbols, zeros
x, y = symbols('x,y')
A = Matrix([[1, 1], [2, -3], [0, 5]])
T = zeros(3, 2)
T = A @ Matrix([[x, y]]).T
print(T)
def calc_lbi_nf_state_eq(self, simplify=False):
"""
calc vectorfields fz, and gz of the Lagrange-Byrnes-Isidori-Normalform
instead of the state xx
"""
n = len(self.tt)
nq = len(self.tau)
np = n - nq
nx = 2 * n
# make sure that the system has the desired structure
B = self.eqns.jacobian(self.tau)
cond1 = B[:np, :] == sp.zeros(np, nq)
cond2 = B[np:, :] == -sp.eye(nq)
if not cond1 and cond2:
msg = "The jacobian of the equations of motion do not have the expected structure: %s"
raise NotImplementedError(msg % str(B))
pp = self.tt[:np, :]
qq = self.tt[np:, :]
uu = self.ttd[:np, :]
vv = self.ttd[np:, :]
ww = st.symb_vector('w1:{0}'.format(np + 1))
assert len(vv) == nq
# state w.r.t normal form
self.zz = st.row_stack(qq, vv, pp, ww)
self.ww = ww
# set the actuated accelearations as new inputs
self.aa = self.ttdd[-nq:, :]
# input vectorfield
self.gz = sp.zeros(nx, nq)
self.gz[nq:2 * nq, :] = sp.eye(
nq) # identity matrix for the active coordinates
# drift vectorfield (will be completed below)
self.fz = sp.zeros(nx, 1)
self.fz[:nq, :] = vv
self.calc_mass_matrix()
if simplify:
self.M.simplify()
M11 = self.M[:np, :np]
M12 = self.M[:np, np:]
d = M11.berkowitz_det()
adj = M11.adjugate()
if simplify:
d = d.simplify()
adj.simplify()
M11inv = adj / d
# defining equation for ww: ww := uu + M11inv*M12*vv
uu_expr = ww - M11inv * M12 * vv
# setting input tau and acceleration to 0 in the equations of motion
C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau))
N = st.time_deriv(M11inv * M12, self.tt)
ww_dot = -M11inv * C1K1.subs(lzip(uu, uu_expr)) + N.subs(
lzip(uu, uu_expr)) * vv
self.fz[2 * nq:2 * nq + np, :] = uu_expr
self.fz[2 * nq + np:, :] = ww_dot
# how the new coordinates are defined:
self.ww_def = uu + M11inv * M12 * vv
if simplify:
self.fz.simplify()
self.gz.simplify()
self.ww_def.simplify()
def genFDStencil(template, diff, order, boundary_diff=None):
"""
This function generates a finite difference stencil from a template matrix
which consists of 1's at gridpoints that you might need to sample during
the finite difference scheme, a -1 at the point at which you are trying to
caluclate the given derivative and 0's everywhere else. It returns a
stencil matrix. Specify the particular differential you want to calculate
with `diff` (x_partial, y_partial) and specify the order you want to
calculate it to with `order`. If you have Neumann boundary conditions,
specify which derivative is fixed with the boundary_diff parameter.
Parameters
----------
template : sympy Matrix
nxn sympy Matrix where n is odd. Represents a template of gridpoints
that you may need to sample to approximate the derivative. Should have
1 at gridpoints you might need to sample, -1 at the point where you are
approximating the derivative, and 0 everywhere else.
diff : tuple of ints
2-element tuple representing the x-degree and y-degree of the
derivative you are approximating.
order : int
The order to which the derivative approximation should be made.
boundary_diff : tuple of ints, optional
Tuple of ints representing which derivative is known at the boundary if
Neumann boundary conditions are being considered. If `None` then it is
assumed there are no boundary conditions necessary.
The default is None.
Returns
-------
stencil : sympy Matrix
Stencil corresponding to the finite difference scheme.
boundary_val : number
Coefficient in front of the fixed boundary value that you need to
subtract from the finite difference scheme.
Examples
--------
>>> import FiniteDifference as fd
>>> import sympy as sy
>>> sy.init_printing(pretty_print=False)
>>> template = sy.Matrix([[1, 1, 1, 1, 1],
... [1, 1, 1, 1, 1],
... [1, 1, -1, 1, 1],
... [1, 1, 1, 1, 1],
... [1, 1, 1, 1, 1]])
>>> diff = (3, 1)
>>> order = 2
>>> stencil, boundary_val = fd.genFDStencil(template, diff, order)
>>> stencil
Matrix([
[-1/10, 1/5, 0, -1/5, 1/10],
[-1/20, 1/10, 0, -1/10, 1/20],
[ 0, 0, 0, 0, 0],
[ 1/20, -1/10, 0, 1/10, -1/20],
[ 1/10, -1/5, 0, 1/5, -1/10]])
>>> boundary_val
0
`genFDStencil` can also deal with Neumann boundary conditions -- here you
need to stipulate which derivative is normal to the boundary using the
optional `boundary_diff` parameter.
>>> import FiniteDifference as fd
>>> import sympy as sy
>>> sy.init_printing(pretty_print=False)
>>> template = sy.Matrix([[0, 0, 0],
... [-1, 1, 1],
... [0, 0, 0]])
>>> diff = (2, 0)
>>> order = 2
>>> boundary_diff = (1, 0)
>>> stencil, boundary_val = fd.genFDStencil(template, diff,
... order, boundary_diff)
>>> stencil
Matrix([
[ 0, 0, 0],
[-7/2, 4, -1/2],
[ 0, 0, 0]])
>>> boundary_val
-3
"""
diff_degree = diff[0] + diff[1]
m, n = template.shape
k = diff_degree + order # total expansion degree
x, y = sy.symbols('x y')
# Find center of template, marked by -1
center = None
for i in range(m):
for j in range(n):
if template[i, j] == -1:
center = (i, j)
if not center:
raise ValueError("Did not set center of template")
# Make list of gridpoints measured from center in the template
# Note that it's in ij index convention
gridpoints = []
for i in range(m):
for j in range(n):
if template[i, j] == 1:
gridpoints.append((j - center[1], center[0] - i))
# Generate standard series to degree k to compare other expansions against
f = sy.exp(x) * sy.exp(y)
q = sy.expand(f.series(x, 0, k).removeO().series(y, 0, k).removeO())
q = sum(term for term in q.args
if sy.degree(term, x) + sy.degree(term, y) < k
and sy.degree(term, x) + sy.degree(term, y) > 0)
q = sy.poly(q, x, y)
# Find place of desired differential, and terms excluded by b.c.
diff_place = q.monoms(order='grlex').index(diff)
if boundary_diff:
excluded_place = q.monoms(order='grlex').index(boundary_diff)
else:
excluded_place = None
# Populate linear system matrix and inhomogeneous vector w/expansion terms
M = sy.zeros(len(gridpoints), q.length())
boundary_diff_vec = sy.zeros(len(gridpoints), 1)
gridpoint_idx = 0
for (i, j) in gridpoints:
f = sy.exp(i * x) * sy.exp(j * y)
p = sy.expand(f.series(x, 0, k).removeO().series(y, 0, k).removeO())
p = sum(term for term in p.args
if sy.degree(term, x) + sy.degree(term, y) < k
and sy.degree(term, x) + sy.degree(term, y) > 0)
p = sy.poly(p, x, y)
taylor_term_idx = 0
for deg in q.monoms(order='grlex'):
if taylor_term_idx == excluded_place:
M[gridpoint_idx, taylor_term_idx] = 0
boundary_diff_vec[gridpoint_idx] = -p.coeff_monomial(deg)
else:
M[gridpoint_idx, taylor_term_idx] = p.coeff_monomial(deg)
taylor_term_idx += 1
gridpoint_idx += 1
# Calc pseudoinverse, check that it inverts desired vector calc stencil
M_inv = M.pinv()
diff_vec = sy.zeros(q.length(), 1)
diff_vec[diff_place] = 1
if M_inv * M * diff_vec == diff_vec:
stencil = template
gridpoint_idx = 0
centerpoint_val = 0
for i in range(m):
for j in range(n):
if template[i, j] == 1:
stencil[i, j] = M_inv[diff_place, gridpoint_idx]
centerpoint_val += M_inv[diff_place, gridpoint_idx]
gridpoint_idx += 1
stencil[center[0], center[1]] = -centerpoint_val
boundary_val = (M_inv * boundary_diff_vec)[diff_place]
else:
print("Could not make a stencil")
stencil = None
boundary_val = None
return stencil, boundary_val
def _form_frstar(self, bl):
"""Form the generalized inertia force.
Computes the vector of the generalized inertia force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
bl : list
A list of all RigidBody's and Particle's in the system.
"""
if not hasattr(bl, '__iter__'):
raise TypeError('Bodies must be supplied in an iterable.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
m = len(udep)
p = o - m
udot = self._udot
udotzero = dict(zip(udot, [0] * o))
# auxiliary speeds
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(zip(uaux, [0] * len(uaux)))
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
# dictionary of qdot's to u's
qdots = dict(zip(self._qdot_u_map.keys(),
self._qdot_u_map.values()))
for k, v in qdots.items():
qdots[k.diff(t)] = v.diff(t)
MM = zeros(o, o)
nonMM = zeros(o, 1)
partials = []
# Fill up the list of partials: format is a list with no. elements
# equal to number of entries in body list. Each of these elements is a
# list - either of length 1 for the translational components of
# particles or of length 2 for the translational and rotational
# components of rigid bodies. The inner most list is the list of
# partial velocities.
for v in bl:
if isinstance(v, RigidBody):
partials += [self._partial_velocity([v.masscenter.vel(N),
v.frame.ang_vel_in(N)],
u, N)]
elif isinstance(v, Particle):
partials += [self._partial_velocity([v.point.vel(N)], u, N)]
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
# This section does 2 things - computes the parts of Fr* that are
# associated with the udots, and the parts that are not associated with
# the udots. This happens for RigidBody and Particle a little
# differently, but similar process overall.
for i, v in enumerate(bl):
if isinstance(v, RigidBody):
M = v.mass.subs(uaz).doit()
I = v.central_inertia.subs(uaz).doit()
for j in range(o):
for k in range(o):
# translational
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() &
partials[i][0][k])
# rotational
temp = (I & partials[i][1][j].subs(uaz).doit())
MM[j, k] += (temp &
partials[i][1][k])
# translational components
nonMM[j] += ( (M.diff(t) *
v.masscenter.vel(N)).subs(uaz).doit() &
partials[i][0][j])
nonMM[j] += (M *
v.masscenter.acc(N).subs(udotzero).subs(uaz).doit()
& partials[i][0][j])
# rotational components
omega = v.frame.ang_vel_in(N).subs(uaz).doit()
nonMM[j] += ((I.dt(v.frame) & omega).subs(uaz).doit() &
partials[i][1][j])
nonMM[j] += ((I &
v.frame.ang_acc_in(N)).subs(udotzero).subs(uaz).doit()
& partials[i][1][j])
nonMM[j] += ((omega ^ (I & omega)).subs(uaz).doit() &
partials[i][1][j])
if isinstance(v, Particle):
M = v.mass.subs(uaz).doit()
for j in range(o):
for k in range(o):
MM[j, k] += M * (partials[i][0][j].subs(uaz).doit() &
partials[i][0][k])
nonMM[j] += M.diff(t) * (v.point.vel(N).subs(uaz).doit() &
partials[i][0][j])
nonMM[j] += (M *
v.point.acc(N).subs(udotzero).subs(uaz).doit() &
partials[i][0][j])
# Negate FRSTAR since Kane defines the inertia forces/torques
# to be negative and we didn't do so above.
MM = MM.subs(qdots).subs(uaz).doit()
nonMM = nonMM.subs(qdots).subs(udotzero).subs(uadz).subs(uaz).doit()
FRSTAR = -(MM * Matrix(udot).subs(uadz) + nonMM)
# For motion constraints, m is the number of constraints
# Really, one should just look at Kane's book for descriptions of this
# process
if m != 0:
FRSTARtilde = FRSTAR[:p, 0]
FRSTARold = FRSTAR[p:o, 0]
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM[:p, :]
MMd = MM[p:o, :]
MM = MMi + self._Ars.T * MMd
self._frstar = FRSTAR
zeroeq = -(self._fr + self._frstar)
zeroeq = zeroeq.subs(udotzero)
self._k_d = MM
self._f_d = zeroeq
return FRSTAR
def __init__(self, label=None):
"""
Constructor for the Core Port-Hamiltonian structure object of pyphs.
Parameter
----------
label: None or string
An optional label string used e.g. for plots (default is None).
Returns
-------
core : Core
A Core Port-Hamiltonian structure object.
"""
# Init label
if label is None:
label = 'phs'
self.label = label
# =====================================================================
# assertions for sympy symbols
self.assertions = {'real': True}
# Ordered list of variables considered as the systems's arguments
self.args_names = ('x', 'dx', 'w', 'u', 'p', 'o')
self.attrstocopy = {('dims', '_xl'), ('dims', '_wl'), 'connectors',
'force_wnl', 'subs', 'M', '_dxH', 'symbs_names',
'exprs_names', 'observers'}
# Names for matrix structures
self.struc_names = ['M', 'J', 'R']
# =====================================================================
# Returned by core.dxH(). If None, returns gradient(core.H, core.x).
self._dxH = None
# names for lists of symbols (x, w, ...)
self.symbs_names = set()
# Expressions names
self.exprs_names = set()
# Init structure
self.M = types.matrix_types[0](sympy.zeros(0))
# List of connectors
self.connectors = list()
# UNORDERED Dictionary of substitution {symbol: value}
self.subs = OrderedDict()
# ORDERED Dictionary of observers {symbol: expr}
self.observers = OrderedDict()
# List of dissipative variable symbols to be ignored in self.reduce_z
self.force_wnl = list()
# =====================================================================
# init tools
self.dims = Dimensions(self)
self.inds = Indices(self)
# init lists of symbols
for name in {'x', 'w', 'u', 'y', 'cu', 'cy', 'p'}:
self.setsymb(name, types.vector_types[0]())
# init functions
self.setexpr('H', sympify(0))
self.setexpr('z', types.vector_types[0]())
# Coefficient matrices for linear parts
self.setexpr('Q', types.matrix_types[0](sympy.zeros(0, 0)))
self.setexpr('Zl', types.matrix_types[0](sympy.zeros(0, 0)))
# init tools
self.dims = Dimensions(self)
self.inds = Indices(self)
# get() and set() for structure matrices
names = ('x', 'w', 'y', 'cy', 'xl', 'xnl', 'wl', 'wnl')
self._struc_getset(names)
# build accessors for nonlinear and linear parts
for name in {'x', 'dx', 'dxH'}:
lnl_accessors = self._gen_lnl_accessors(name, 'x')
setattr(self, name + 'l', lnl_accessors[0])
setattr(self, name + 'nl', lnl_accessors[1])
for name in {'w', 'z'}:
lnl_accessors = self._gen_lnl_accessors(name, 'w')
setattr(self, name + 'l', lnl_accessors[0])
setattr(self, name + 'nl', lnl_accessors[1])
tmplist = line.split() # split a row into a list
# print(tmplist)
if tmplist == []: # skip empty rows
continue
if tmplist[0][0] == '#': # skip comment rows
continue
if tmplist[0][0:6] == "STAGES": # number of RC stages
stages = int(tmplist[1])
print("stages = " + str(stages))
continue
# start reading actual data
# (1st column is stage number)
c_list.append(tmplist[1]) # Cth on the 2nd column
r_list.append(tmplist[2]) # Rth on the 3rd column
FosterMat = sympy.zeros(stages, 3) # Final results will be stored here.
CauerMat = sympy.zeros(stages, 3) # Input data will be stored here
for i in range(stages):
CauerMat[i, 0] = sympy.Rational(c_list[i])
# By default, reduced the accuracy level by not Rationalizing Rth.
CauerMat[i, 1] = sympy.Rational(r_list[i]) if rational_rth else r_list[i]
CauerMat[i, 2] = CauerMat[i, 0] * CauerMat[i, 1]
# ### As shown in the FosterMatSample3x3, variables line up in ascending order.
# Cf1 and Rf1 pair represents the first stage of the Foster model.
# They are next to Junction.
# So as the Cc1 and Rc1 of the Cauer model.
def test_Metric_einstein():
(coords, t, r, th, ph, schw, g, mu, nu) = _generate_schwarzschild()
G = g.einstein
assert zeros(4).equals(G.as_array())
def test_Metric_ricci_tensor():
(coords, t, r, th, ph, schw, g, mu, nu) = _generate_schwarzschild()
R = g.ricci_tensor
assert zeros(4).equals(R.as_array())
def external_inputs(self):
"""SymPy dx1-matrix: Return the vector of external inputs."""
u = zeros(self.nr_pools, 1)
for k, val in self.input_fluxes.items():
u[k] = val
return u
def test_u1():
lambd = symbols("lambd")
actual_1 = Circuit().u1(lambd)[0].run(backend="sympy_unitary")
expected_1 = Circuit().rz(lambd)[0].run_with_sympy_unitary()
assert simplify(actual_1 - expected_1) == zeros(2)
def external_outputs(self):
"""SymPy dx1-matrix: Return the vector of external outputs."""
o = zeros(self.nr_pools, 1)
for k, val in self.output_fluxes.items():
o[k] = val
return o
print "\n\nTesting unknown(symbol) (one-arg form)"
print "Unknown a: ", ua.symbol
fs = ' unknown object element "solved" FAIL'
assert(ua.solved == False), fs
print "a is solved: ", ua.solved , ' (Expect False)'
print "Unknown b: ", ub.symbol
ub.solved = True
print "b is solved: ", ub.solved, ' (Expect True)'
assert(ub.solved == True), fs
##Test matrix_equation class
print "\n\nTesting matrix_equation(T1,T2) class"
T1 = ik_lhs()
T2 = sp.zeros(5)
T2[1,1] = a # note: a = atan2(b,c) above
T2[1,2] = a+b
T2[2,2] = sp.sin(c)
T2[2,3] = l_1*sp.sin(d) + 2*l_2*sp.cos(d)
T2[3,1] = c+sp.cos(c)*l_1
sp.pprint(T2)
tme = kc.matrix_equation(T1,T2)
print ''
print "Mat eqn 1,2: ", tme.Td[1,2], " '=' ", tme.Ts[1,2], "(not a kequation type!)"
print ''
sp.var('e22 ')
def timeit_Matrix_zeronm():
zeros(100, 100)
def test_subs_Matrix():
z = zeros(2)
assert (x * y).subs({x: z, y: 0}) == z
assert (x * y).subs({y: z, x: 0}) == 0
assert (x * y).subs({y: z, x: 0}, simultaneous=True) == z
assert (x + y).subs({x: z, y: z}) == z
def _form_frstar(self, bl):
"""Form the generalized inertia force.
Computes the vector of the generalized inertia force vector.
Used to compute E.o.M. in the form Fr + Fr* = 0.
Parameters
==========
bl : list
A list of all RigidBody's and Particle's in the system.
"""
if not isinstance(bl, (list, tuple)):
raise TypeError('Bodies must be supplied in a list.')
if self._fr == None:
raise ValueError('Calculate Fr first, please.')
t = dynamicsymbols._t
N = self._inertial
self._bodylist = bl
u = self._u # all speeds
udep = self._udep # dependent speeds
o = len(u)
p = o - len(udep)
udot = self._udot
udotzero = dict(zip(udot, [0] * len(udot)))
uaux = self._uaux
uauxdot = [diff(i, t) for i in uaux]
# dictionary of auxiliary speeds which are equal to zero
uaz = dict(zip(uaux, [0] * len(uaux)))
# dictionary of derivatives of auxiliary speeds which are equal to zero
uadz = dict(zip(uauxdot, [0] * len(uauxdot)))
# Form R*, T* for each body or particle in the list
# This is stored as a list of tuples [(r*, t*),...]
# Each tuple is for a body or particle
# Within each rs is a tuple and ts is a tuple
# These have the same structure: ([list], value)
# The list is the coefficients of rs/ts wrt udots, value is everything
# else in the expression
# Partial velocities are stored as a list of tuple; a tuple for each
# body
# Each tuple has two elements, lists which represent the partial
# velocity for each ur; The first list is translational partial
# velocities, the second list is rotational translational velocities
MM = zeros(o, o)
nonMM = zeros(o, 1)
rsts = []
partials = []
for i, v in enumerate(bl): # go through list of bodies, particles
if isinstance(v, RigidBody):
om = v.frame.ang_vel_in(N).subs(uadz).subs(uaz) # ang velocity
omp = v.frame.ang_vel_in(N) # ang velocity, for partials
alp = v.frame.ang_acc_in(N).subs(uadz).subs(uaz) # ang acc
ve = v.masscenter.vel(N).subs(uadz).subs(uaz) # velocity
vep = v.masscenter.vel(N) # velocity, for partials
acc = v.masscenter.acc(N).subs(uadz).subs(uaz) # acceleration
m = (v.mass).subs(uadz).subs(uaz)
I, P = v.inertia
I = I.subs(uadz).subs(uaz)
if P != v.masscenter:
# redefine I about the center of mass
# have I S/O, want I S/S*
# I S/O = I S/S* + I S*/O; I S/S* = I S/O - I S*/O
f = v.frame
d = v.masscenter.pos_from(P)
I -= inertia_of_point_mass(m, d, f)
templist = []
# One could think of r star as a collection of coefficients of
# the udots plus another term. What we do here is get all of
# these coefficients and store them in a list, then we get the
# "other" term and put the list and other term in a tuple, for
# each body/particle. The same is done for t star. The reason
# for this is to not let the expressions get too large; so we
# keep them seperate for as long a possible
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-I & alp.diff(w, N))
other = -((I.dt(v.frame) & om) + (I & alp.subs(udotzero)) +
(om ^ (I & om)))
ts = (templist, other)
tl1 = []
tl2 = []
# calculates the partials only once and stores them for later
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(omp.diff(w, N))
partials.append((tl1, tl2))
elif isinstance(v, Particle):
ve = v.point.vel(N).subs(uadz).subs(uaz)
vep = v.point.vel(N)
acc = v.point.acc(N).subs(uadz).subs(uaz)
m = v.mass.subs(uadz).subs(uaz)
templist = []
# see above note
for j, w in enumerate(udot):
templist.append(-m * acc.diff(w, N))
other = -m.diff(t) * ve - m * acc.subs(udotzero)
rs = (templist, other)
# We make an empty t star here so that way the later code
# doesn't care whether its operating on a body or particle
ts = ([0] * len(u), 0)
tl1 = []
tl2 = []
# calculates the partials only once, makes 0's for angular
# partials so the later code is body/particle indepedent
for j, w in enumerate(u):
tl1.append(vep.diff(w, N))
tl2.append(0)
partials.append((tl1, tl2))
else:
raise TypeError('The body list needs RigidBody or '
'Particle as list elements.')
rsts.append((rs, ts))
# Use R*, T* and partial velocities to form FR*
FRSTAR = zeros(o, 1)
# does this for each body in the list
for i, v in enumerate(rsts):
rs, ts = v # unpact r*, t*
vps, ops = partials[i] # unpack vel. partials, ang. vel. partials
# Computes the mass matrix entries from r*, there are from the list
# in the rstar tuple
ii = 0
for x in vps:
for w in rs[0]:
MM[ii] += w & x
ii += 1
# Computes the mass matrix entries from t*, there are from the list
# in the tstar tuple
ii = 0
for x in ops:
for w in ts[0]:
MM[ii] += w & x
ii += 1
# Non mass matrix entries from rstar, from the other in the rstar
# tuple
for j, w in enumerate(vps):
nonMM[j] += w & rs[1]
# Non mass matrix entries from tstar, from the other in the tstar
# tuple
for j, w in enumerate(ops):
nonMM[j] += w & ts[1]
FRSTAR = MM * Matrix(udot) + nonMM
# For motion constraints, m is the number of constraints
# Really, one should just look at Kane's book for descriptions of this
# process
if len(self._udep) != 0:
FRSTARtilde = FRSTAR.extract(range(p), [0])
FRSTARold = FRSTAR.extract(range(p, o), [0])
FRSTARtilde += self._Ars.T * FRSTARold
FRSTAR = FRSTARtilde
MMi = MM.extract(range(p), range(o))
MMd = MM.extract(range(p, o), range(o))
MM = MMi + self._Ars.T * MMd
self._frstar = FRSTAR
zeroeq = self._fr + self._frstar
zeroeq = zeroeq.subs(udotzero)
self._k_d = MM
self._f_d = zeroeq
return FRSTAR
def tick(self, tick):
test_number = tick.blackboard.get('test_number')
assert(test_number in [1, 2]), ' BAD TEST NUMBER'
if(test_number == 1):
# set up bb data for testing
Td = ik_lhs() # basic LHS template for TEST
Ts = sp.zeros(4)
Td[0,3] = sp.cos(th_1)*Px + sp.sin(th_1)*Py
Ts[0,3] = a_3*sp.cos(th_23) - d_4*sp.sin(th_23) + a_2 * sp.cos(th_2)
Td[2,3] = -Pz
Ts[2,3] = a_3*sp.sin(th_23) + d_4*sp.cos(th_23) + a_2 * sp.sin(th_2)
testm = matrix_equation(Td,Ts)
R = Robot()
R.mequation_list = [testm]
ud1 = unknown(th_1)
uth2 = unknown(th_2)
uth3 = unknown(th_3)
uth23 = unknown(th_23)
uth4 = unknown(th_4)
uth5 = unknown(th_5)
variables = [ud1, uth2, uth23, uth3, uth4, uth5]
R.generate_solution_nodes(variables) #for the solution graph
ud1.solutions.append(a_3)
ud1.nsolutions = 1
ud1.set_solved(R,variables) # needed for this test
#uth23.set_solved(R,variables) # needed for this test
R.sum_of_angles_transform(variables) # should add th_23=th_2+th_3 to list
[L1, L2, L3p] = R.scan_for_equations(variables) # lists of 1unk and 2unk equations
tick.blackboard.set('eqns_1u', L1)
tick.blackboard.set('eqns_2u', L2)
tick.blackboard.set('eqns_3pu', L3p)
tick.blackboard.set('unknowns',variables)
tick.blackboard.set('Robot',R)
return b3.SUCCESS
if(test_number == 2):
#
# The famous Puma 560 (solved in Craig)
#
robot = 'Puma'
[dh_Puma560, vv_Puma, params_puma, unk_Puma] = robot_params('Puma')
dh = dh_Puma560
vv = vv_Puma
variables = unk_Puma
################## (all robots) ######################
## make sure each unknown knows its position (index)
i = 0
for u in variables :
u.n = i
i+=1
print 'Testing x2z2solver with Puma Kinematics'
# read kinematic model from pickle file / or compute it from scratch
[M, R, variables ] = kinematics_pickle(robot, dh, params_puma, vv, variables )
## check the pickle in case DH params were changed in robot_params making the
# pickle obsolete.
check_the_pickle(dh_Puma560, M.DH)
R.name = 'Puma x2z2 Test Robot'
# set th_1 to solved
variables [0].solutions.append(a_3)
variables [0].nsolutions = 1
variables [0].set_solved(R,variables ) # needed for this test
R.sum_of_angles_transform(variables ) # should add th_23=th_2+th_3 to list
[L1, L2, L3p] = R.scan_for_equations(variables ) # lists of 1unk and 2unk equations
tick.blackboard.set('eqns_1u', L1)
tick.blackboard.set('eqns_2u', L2)
tick.blackboard.set('eqns_3pu', L3p)
tick.blackboard.set('unknowns', variables )
tick.blackboard.set('Robot', R)
return b3.SUCCESS
import sympy as sp
T, tau = sp.symbols('T, tau')
sigma_a, sigma_b = sp.symbols('sigma_a, sigma_b')
shape = 4
A = sp.MatrixSymbol('A', shape, shape)
A = sp.Matrix([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
zeros = sp.zeros(shape, shape)
G = sp.MatrixSymbol('G', shape, shape)
G = sp.Matrix([[0, 0], [1, 0], [0, 0], [0, 1]])
D = sp.diag(*[sigma_a, sigma_b])
myvar = sp.exp((T - tau) * A) * G * D * G.T * sp.exp((T - tau) * A.T)
Q = sp.simplify(sp.integrate(myvar, (tau, 0, T)))
vanloan = sp.BlockMatrix([[-A, G * D * G.T], [zeros, A.T]]) * T
vanloan_exp = sp.exp(sp.Matrix(vanloan))
Q2 = vanloan_exp[shape:, shape:].T * vanloan_exp[:shape, shape:]
def __init__(self, **kwargs):
super(Config, self).__init__(N_JOINTS=1,
N_LINKS=1,
ROBOT_NAME='onelink',
**kwargs)
self._T = {} # dictionary for storing calculated transforms
self.JOINT_NAMES = ['joint0']
self.REST_ANGLES = np.array([np.pi / 2.0])
# create the inertia matrices for each link
self._M_LINKS.append(np.diag([1.0, 1.0, 1.0, 0.02, 0.02,
0.02])) # link0
# the joints don't weigh anything
self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)]
# segment lengths associated with each joint
self.L = np.array([
[0.0, 0.0, 0.05], # from origin to l0 COM
[0.0, 0.0, 0.05], # from l0 COM to j0
[0.22, 0.0, 0.0], # from j0 to l1 COM
[0.0, 0.0, .15]
]) # from l1 COM to EE
# ---- Transform Matrices ----
# Transform matrix : origin -> link 0
# account for axes change and offsets
self.Torgl0 = sp.Matrix([[1, 0, 0, self.L[0, 0]],
[0, 1, 0, self.L[0, 1]],
[0, 0, 1, self.L[0, 2]], [0, 0, 0, 1]])
# Transform matrix : link 0 -> joint 0
# account for axes change and offsets
self.Tl0j0 = sp.Matrix([[1, 0, 0, self.L[1, 0]],
[0, 0, -1, self.L[1, 1]],
[0, 1, 0, self.L[1, 2]], [0, 0, 0, 1]])
# Transform matrix : joint 0 -> link 1
# account for rotation due to q
self.Tj0l1a = sp.Matrix([[sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0],
[sp.sin(self.q[0]),
sp.cos(self.q[0]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for change of axes and offsets
self.Tj0l1b = sp.Matrix([[0, 0, 1, self.L[2, 0]],
[0, 1, 0, self.L[2, 1]],
[-1, 0, 0, self.L[2, 2]], [0, 0, 0, 1]])
self.Tj0l1 = self.Tj0l1a * self.Tj0l1b
# Transform matrix : link 1 -> end-effector
self.Tl1ee = sp.Matrix([[1, 0, 0, self.L[3,
0]], [0, 1, 0, self.L[3, 1]],
[0, 0, 1, self.L[3, 2]], [0, 0, 0, 1]])
# orientation part of the Jacobian (compensating for angular velocity)
self.J_orientation = [self._calc_T('joint0')[:3, :3] * self._KZ
] # joint 0 orientation
print 'Qb: ',Qb
QsT = Matrix([B.transpose(),\
(A*B).transpose(),\
(A**2*B).transpose(),\
(A**3*B).transpose()])
Qs = QsT.transpose()
print 'Qs: ',Qs
print 'A: ',A
print 'B: ',B
print 'C: ',C
'''
Nachweis der Steuerbarkeit des erweiterten Zustandsraummodells
'''
Aquer = A.col_join(C).row_join(sp.zeros(5,1))
Bquer = B.col_join(sp.zeros(1,1))
Cquer = C.row_join(sp.zeros(1,1))
'''
Übertragungsfunktion des Linearisierten Systems
'''
G = C*(sp.eye(4)*s - A)**-1*B
# Steuerbarkeitsmatrix berechnen
n = 5
Qsquer = sp.Matrix()
for i in range(5):
q = (Aquer**i)*Bquer
if len(Qsquer) == 0:
def __init__(self, scheme):
# TODO: add source terms
t, x, y, z = sp.symbols("t x y z", type='real')
consm = list(scheme.consm.keys())
nconsm = len(scheme.consm)
self.consm = sp.Matrix(consm)
self.dim = scheme.dim
space = [x, y, z]
LA = scheme.symb_la
if not LA:
LA = scheme.la
func = []
for i in range(nconsm):
func.append(sp.Function('f{}'.format(i))(t, x, y, z)) #pylint: disable=not-callable
func = sp.Matrix(func)
sublist = [(i, j) for i, j in zip(consm, func)]
sublist_inv = [(j, i) for i, j in zip(consm, func)]
eq_func = sp.Matrix(scheme.EQ[nconsm:]).subs(sublist)
s = sp.Matrix(scheme.s[nconsm:])
all_vel = scheme.stencil.get_all_velocities()
Lambda = []
for i in range(all_vel.shape[1]):
vd = LA * sp.diag(*all_vel[:, i])
Lambda.append(scheme.M * vd * scheme.invM)
phi1 = sp.zeros(s.shape[0], 1) #pylint: disable=unsubscriptable-object
sigma = sp.diag(*s).inv() - sp.eye(len(s)) / 2
gamma_1 = sp.zeros(nconsm, 1)
self.coeff_order1 = []
for dim, lambda_ in enumerate(Lambda):
A, B = sp.Matrix([lambda_[:nconsm, :nconsm]
]), sp.Matrix([lambda_[:nconsm, nconsm:]])
C, D = sp.Matrix([lambda_[nconsm:, :nconsm]
]), sp.Matrix([lambda_[nconsm:, nconsm:]])
self.coeff_order1.append(A * func + B * eq_func)
alltogether(self.coeff_order1[-1], nsimplify=True)
for i in range(nconsm):
gamma_1[i] += sp.Derivative(self.coeff_order1[-1][i],
space[dim])
dummy = -C * func - D * eq_func
alltogether(dummy, nsimplify=True)
for i in range(dummy.shape[0]):
phi1[i] += sp.Derivative(dummy[i], space[dim])
self.coeff_order2 = [[sp.zeros(nconsm) for _ in range(scheme.dim)]
for _ in range(scheme.dim)]
for dim, lambda_ in enumerate(Lambda):
A, B = sp.Matrix([lambda_[:nconsm, :nconsm]
]), sp.Matrix([lambda_[:nconsm, nconsm:]])
meq = sp.Matrix(scheme.EQ[nconsm:])
jac = meq.jacobian(consm)
jac = jac.subs(sublist)
delta1 = jac * gamma_1
phi1_ = phi1 + delta1
sphi1 = B * sigma * phi1_
sphi1 = sphi1.doit()
alltogether(sphi1, nsimplify=True)
for i in range(scheme.dim):
for jc in range(nconsm):
for ic in range(nconsm):
self.coeff_order2[dim][i][
ic, jc] += sphi1[ic].expand().coeff(
sp.Derivative(func[jc], space[i]))
for ic, c in enumerate(self.coeff_order1):
self.coeff_order1[ic] = c.subs(sublist_inv).doit()
for ic, c in enumerate(self.coeff_order2):
for jc, cc in enumerate(c):
self.coeff_order2[ic][jc] = cc.subs(sublist_inv).doit()
def generate_symbolic_model(T, U, tt, F, simplify=True, **kwargs):
"""
T: kinetic energy
U: potential energy
tt: sequence of independent deflection variables ("theta")
F: external forces
simplify: determines whether the equations of motion should be simplified
(default=True)
kwargs: optional assumptions like 'real=True'
"""
n = len(tt)
for theta_i in tt:
assert isinstance(theta_i, sp.Symbol)
F = sp.Matrix(F)
if F.shape[0] == 1:
# convert to column vector
F = F.T
if not F.shape == (n, 1):
msg = "Vector of external forces has the wrong length. Should be " + \
str(n) + " but is %i!" % F.shape[0]
raise ValueError(msg)
# introducing symbols for the derivatives
tt = sp.Matrix(tt)
ttd = st.time_deriv(tt, tt, **kwargs)
ttdd = st.time_deriv(tt, tt, order=2, **kwargs)
#Lagrange function
L = T - U
if not T.atoms().intersection(ttd) == set(ttd):
raise ValueError('Not all velocity symbols do occur in T')
# partial derivatives of L
L_diff_tt = st.jac(L, tt)
L_diff_ttd = st.jac(L, ttd)
#prov_deriv_symbols = [ttd, ttdd]
# time-depended_symbols
tds = list(tt) + list(ttd)
L_diff_ttd_dt = st.time_deriv(L_diff_ttd, tds, **kwargs)
#lagrange equation 2nd kind
model_eq = sp.zeros(n, 1)
for i in range(n):
model_eq[i] = L_diff_ttd_dt[i] - L_diff_tt[i] - F[i]
# create object of model
mod = SymbolicModel() # model_eq, qs, f, D)
mod.eqns = model_eq
mod.extforce_list = F
reduced_F = sp.Matrix([s for s in F if st.is_symbol(s)])
mod.F = reduced_F
mod.tau = reduced_F
# coordinates velocities and accelerations
mod.tt = tt
mod.ttd = ttd
mod.ttdd = ttdd
mod.qs = tt
mod.qds = ttd
mod.qdds = ttdd
# also store kinetic and potential energy
mod.T = T
mod.U = U
if simplify:
mod.eqns.simplify()
return mod
def python_string_to_sympy(string_expression: tuple_of(str), x_symb: (Matrix, MatrixSymbol, None), mu_symb: (Matrix, MatrixSymbol, None)):
sympy_expression = zeros(len(string_expression), 1)
for (i, si) in enumerate(string_expression):
sympy_expression[i] = sympify(si, locals={"x": x_symb, "mu": mu_symb})
return ImmutableMatrix(sympy_expression)
def mat_zeros(dim):
return zeros(dim)
U[:, i], X[:, i] = u0[0:nu].transpose(), x0.transpose()
u = self.optimise(
u0,
x0,
i,
)
u0 = u
u = u[0:nu].reshape(nu, -1)
x0 = x0.reshape(len(x0), -1)
x0 = self.A @ x0 + self.B @ u
return X, U
if __name__ == '__main__':
mpc = mpc_opt()
pos = sp.zeros(3, len(mpc.t))
x0, u0, = np.array([[0.0], [0.0], [0.0]]), np.array([[0.4], [0.2]])
cg = mpc.cost_gradient(np.tile(u0, (mpc.N, 1)), x0, 1)
cons = mpc.constraints(np.tile(u0, (mpc.N, 1)), x0, 1)
X, U = mpc.get_state_and_input(u0, x0)
plt.figure(1)
plt.plot(X[0, :], '--r')
plt.plot(X[1, :], '--b')
plt.plot(X[2, :], '--g')
# plt.ylim(-2, 2)
plt.xlabel('time')
plt.title('states')
NormalSlave = custom_sympy_fe_utilities.DefineDofDependencyMatrix(
NormalSlave, u1_var)
DOperator = custom_sympy_fe_utilities.DefineDofDependencyMatrix(
DOperator, u12_var)
MOperator = custom_sympy_fe_utilities.DefineDofDependencyMatrix(
MOperator, u12_var)
# Defining the normal NormalGap
Dx1Mx2 = DOperator * x1 - MOperator * x2
Dw1Mw2 = DOperator * w1 - MOperator * w2
for node in range(nnodes):
NormalGap[node] = -Dx1Mx2.row(node).dot(NormalSlave.row(node))
# Define dofs & test function vector
dofs = sympy.Matrix(sympy.zeros(number_dof, 1))
testfunc = sympy.Matrix(sympy.zeros(number_dof, 1))
count = 0
for i in range(0, nnodes_master):
for k in range(0, dim):
dofs[count] = u2[i, k]
testfunc[count] = w2[i, k]
count += 1
for i in range(0, nnodes):
for k in range(0, dim):
dofs[count] = u1[i, k]
testfunc[count] = w1[i, k]
count += 1
for i in range(0, nnodes):
dofs[count] = LMNormal[i]
testfunc[count] = wLMNormal[i]
def parse_dynare_text(txt, add_model=True, full_output=False, debug=False):
'''
Imports the content of a modfile into the current interpreter scope
'''
# here we call "instruction group", a string finishing by a semicolon
# an "instruction group" can have several lines
# a line can be
# - a comment //...
# - an old-style tag //$...
# - a new-style tag [key1='value1',..]
# - macro-instruction @#...
# A Modfile contains several blocks (in this order) :
# - an initblock defining variables, exovariables, parameters, initialization
# inside the initblock the order of declaration doesn't matter
# - a model block with two special lines (model; end;)
# - optional blocks (like endval, shocks)
# seperated by free matlab instructions in any order;
# - all other instructions are ignored
otxt = txt
otxt = otxt.replace("\r\n", "\n")
otxt = otxt.replace("^", "**")
# first, we remove end-of-line comments : they are definitely lost
regex = re.compile("(.+)//[^#](.*)")
def remove_end_comment(line):
res = regex.search(line)
if res:
l = res.groups(1)[0]
return (l)
else:
return line
txt = str.join("\n", map(remove_end_comment, otxt.split("\n")))
name_regex = re.compile("//\s*fname\s*=\s*'(.*)'")
m = name_regex.search(txt)
if m:
fname = m.group(1)
else:
fname = None
instruction_groups = [Instruction_group(s) for s in txt.split(";")]
instructions = [ig.instruction for ig in instruction_groups]
if debug:
print('Elementary instructions')
for i in instruction_groups:
print(i)
try:
imodel = [
re.compile('model(\(.*\)|)').match(e) is not None
for e in instructions
]
imodel = imodel.index(True)
#imodel = instructions.index("model") #this doesn't work for "MODEL"
iend = instructions.index("end")
model_block = instruction_groups[imodel:(iend + 1)]
init_block = instruction_groups[0:imodel]
except:
raise Exception('Model block could not be found.')
next_instructions = instructions[(iend + 1):]
next_instruction_groups = instruction_groups[(iend + 1):]
if 'initval' in next_instructions:
iinitval = next_instructions.index('initval')
iend = next_instructions.index('end', iinitval)
matlab_block_1 = next_instruction_groups[0:iinitval]
initval_block = next_instruction_groups[iinitval:(iend + 1)]
next_instruction_groups = next_instruction_groups[(iend + 1):]
next_instructions = next_instructions[(iend + 1):]
else:
initval_block = None
matlab_block_1 = None
if 'endval' in next_instructions:
iendval = next_instructions.index('endval')
iend = next_instructions.index('end', iendval)
matlab_block_2 = next_instruction_groups[0:iendval]
endval_block = next_instruction_groups[iendval:(iend + 1)]
next_instruction_groups = next_instruction_groups[(iend + 1):]
next_instructions = next_instructions[(iend + 1):]
else:
endval_block = None
matlab_block_2 = None
# TODO : currently shocks block needs to follow initval, this restriction should be removed
if 'shocks' in next_instructions:
ishocks = next_instructions.index('shocks')
iend = next_instructions.index('end', ishocks)
matlab_block_3 = next_instruction_groups[0:ishocks]
shocks_block = next_instruction_groups[ishocks:(iend + 1)]
next_instruction_groups = next_instruction_groups[(iend + 1):]
next_instructions = next_instructions[(iend + 1):]
else:
shocks_block = None
matlab_block_3 = None
try:
init_regex = re.compile("(parameters |var |varexo |)(.*)")
var_names = []
varexo_names = []
parameters_names = []
declarations = {}
for ig in init_block:
if ig.instruction != '':
m = init_regex.match(ig.instruction)
if not m:
raise Exception("Unexpected instruction in init block : " +
str(ig.instruction))
if m.group(1) == '':
[lhs, rhs] = m.group(2).split("=")
lhs = lhs.strip()
rhs = rhs.strip()
declarations[lhs] = rhs
else:
arg = m.group(2).replace(",", " ")
names = [vn.strip() for vn in arg.split()]
if m.group(1).strip() == 'var':
dest = var_names
elif m.group(1).strip() == 'varexo':
dest = varexo_names
elif m.group(1).strip() == 'parameters':
dest = parameters_names
for n in names:
if not n in dest:
dest.append(n)
else:
raise Exception(
"symbol %s has already been defined".format(n))
except Exception as e:
raise Exception('Init block could not be read : ' + str(e))
# the following instruction set the variables "variables","shocks","parameters"
variables = []
for vn in var_names:
v = Variable(vn)
variables.append(v)
shocks = []
for vn in varexo_names:
s = Shock(vn)
shocks.append(s)
parameters = []
for vn in parameters_names:
p = Parameter(vn)
parameters.append(p)
parse_dict = dict()
for v in variables + shocks + parameters:
parse_dict[v.name] = v
special_symbols = [
sympy.exp, sympy.log, sympy.sin, sympy.cos, sympy.atan, sympy.tan
]
for s in special_symbols:
parse_dict[str(s)] = s
parse_dict['sqrt'] = sympy.sqrt
# Read parameters values
parameters_values = {}
for p in declarations:
try:
rhs = eval(declarations[p], parse_dict)
except Exception as e:
Exception("Impossible to evaluate parameter value : " + str(e))
try:
lhs = eval(p, parse_dict)
except Exception as e:
# here we could declare p
raise e
parameters_values[lhs] = rhs
# Now we read the model block
model_tags = model_block[0].tags
equations = []
for ig in model_block[1:-1]:
if ig.instruction != '':
teq = ig.instruction.replace('^', "**")
if '=' in teq:
teqlhs, teqrhs = teq.split("=")
else:
teqlhs = teq
teqrhs = '0'
eqlhs = eval(teqlhs, parse_dict)
eqrhs = eval(teqrhs, parse_dict)
eq = Equation(eqlhs, eqrhs)
eq.tags.update(ig.tags)
# if eq.tags.has_key('name'):
# eq.tags[] = ig.tags['name']
equations.append(eq)
# Now we read the initval block
init_values = {}
if initval_block != None:
for ig in initval_block[1:-1]:
if len(ig.instruction.strip()) > 0:
try:
[lhs, rhs] = ig.instruction.split("=")
except Exception as e:
print(ig.instruction)
raise e
init_values[eval(lhs, parse_dict)] = eval(rhs, parse_dict)
# Now we read the endval block
# I don't really care about the endval block !
end_values = {}
if endval_block != None:
for ig in endval_block[1:-1]:
[lhs, rhs] = ig.instruction.split("=")
end_values[eval(lhs)] = eval(rhs)
# Now we read the shocks block
covariances = None
if shocks_block != None:
covariances = sympy.zeros(len(shocks))
regex1 = re.compile("var (.*?),(.*?)=(.*)|var (.*?)=(.*)")
for ig in shocks_block[1:-1]:
m = regex1.match(ig.instruction)
if not m:
raise Exception("unrecognized instruction in block shocks : " +
str(ig.instruction))
if m.group(1) != None:
varname1 = m.group(1).strip()
varname2 = m.group(2).strip()
value = m.group(3).strip().replace("^", "**")
elif m.group(4) != None:
varname1 = m.group(4).strip()
varname2 = varname1
value = m.group(5).strip().replace("^", "**")
i = varexo_names.index(varname1)
j = varexo_names.index(varname2)
covariances[i, j] = eval(value, parse_dict)
covariances[j, i] = eval(value, parse_dict)
calibration = {}
calibration.update(parameters_values)
calibration.update(init_values)
symbols = {
'variables': variables,
'shocks': shocks,
'parameters': parameters
}
from trash.dolo.symbolic.model import SModel
model = SModel({'dynare_block': equations}, symbols, calibration,
covariances)
return model
