def jacobian(twist, q):
c4 = cos(q[2])
s4 = sin(q[2])
c34 = cos(q[1] + q[2])
s34 = sin(q[1] + q[2])
jb4 = sym.Matrix([-1, -82.3, 94.65])
jb3 = sym.Matrix([-1, jb4[1] + 392.25 * c4, jb4[2] - 392.25 * s4])
# jb3 = sym.Matrix([0, -1, 0, 392.25*c4-82.3, 0, 94.65-392.25*s4])
jb2 = sym.Matrix([-1, jb3[1] + 425 * c34, jb3[2] - 425 * s34])
# jb2 = sym.Matrix([0, -1, 0, 392.25*c4-82.3+425*c34, 0, 94.65-392.25*s4-425*s34])
jcb = sym.Matrix([[jb2, jb3, jb4]])**(-1)
sym.print_latex(jcb * twist)
return q
from sympy import diff, Matrix
from sympy import symbols
from sympy import exp
from sympy import print_latex
def sigmoid(_x):
return 1 / (1 + exp(-_x))
def fz2(w2_1, w2_2, b2, a_1, a_2):
return w2_1 * a_1 + w2_2 * a_2 + b2
z2_1, z2_2 = symbols("z2_{1} z2_{2}")
a_1, a_2 = symbols("a_1, a_2")
y_1, y_2 = symbols("y_1, y_2")
w2_1, w2_2, b2 = symbols("w2_{1}, w2_{2}, b2")
print_latex(fz2)
d = diff(fz2(w2_1, w2_2, b2, a_1, a_2), a_1)
print_latex(d)
Z2 = Matrix([z2_1, z2_2])
Y = Matrix([y_1, y_2])
W2 = Matrix([w2_1, w2_2])
dZ2 = Z2 - Y
print_latex(dZ2.T.dot(W2))
# %% Normal SumSin Diff
#sumsin2 = 1/sin(x/2)*sin((n+1)*x/2)*sin(n*x/2)
#sumsin2 = sympy.simplify(sumsin2)
#
#print('sumsin2')
#sympy.print_latex(sympy.simplify(sumsin2))
#
#print('diff sumsin2 dx1')
#sympy.print_latex(sympy.simplify(sympy.diff(sumsin2, x1)))
#print('diff sumsin2 dx2')
#sympy.print_latex(sympy.simplify(sympy.diff(sumsin2, x2)))
# %% Normal SumCos Diff
x = sumcos(deps_dummy, n)
print('SumCos:')
sympy.print_latex(sympy.simplify(x))
print('diff SumCos:')
sympy.print_latex(sympy.simplify(sympy.diff(x, deps_dummy)))
# %% Sym Diff d
xbarx = symbols('xb_x', real=True)
xbary = symbols('xb_y', real=True)
xbar = [xbarx, xbary]
# %% Distance Euler Formulation
d = Ecalc_d(xbar, dx(x1, x2), deps(x1, x2), n)
print('Distance to goal -- Euler')
# Constants
A1, L1, L2, L3 = sp.symbols("A1 L1 L2 L3")
t = sp.symbols("t")
# Variables
beta, x, y, a, b = sp.symbols("beta x y a b", cls=sp.Function)
alpha = sp.acos(
((L2**2) + ((b(t) - a(t))**2) - (L1**2)) / (2 * L2 * (b(t) - a(t))))
beta = sp.acos(
((L1**2) + ((b(t) - a(t))**2) - (L2**2)) / (2 * L1 * (b(t) - a(t))))
x = b(t) - L3 * sp.cos(beta + A1)
y = L3 * sp.sin(beta + A1)
sp.print_latex(beta)
sp.print_latex(x)
sp.print_latex(y)
# sp.pprint(sp.diff(x, t))
# print(sp.diff(x, t))
# print("\n\n-----------------\n\n")
# sp.pprint(sp.simplify(sp.diff(y, t)))
# print(sp.simplify(sp.diff(y, t)))
dxdt = sp.simplify(sp.diff(x, t))
dydt = sp.simplify(sp.diff(y, t))
sp.print_latex(dxdt)
sp.print_latex(dydt)
# Manual substitution
Yinv = sp.simplify(Y**-1) #sp.print_latex(Yinv) temp = sp.simplify(Yinv * b) #sp.print_latex(temp) result = sp.integrate(temp, t) #sp.print_latex(result) # sp.pretty_print(result) # sp.pretty_print(sp.expand(Y * result)) # sp.print_latex(sp.expand(Y * result)) ### Ansatz yp = a_1 * sp.exp(t) + a_2 * t * sp.exp(t) + a_3 * t**2 * sp.exp(t) #sp.print_latex(yp) dyp = sp.expand(sp.diff(yp, t)) #sp.pretty_print(dyp) #sp.print_latex(dyp) dddyp = sp.expand(sp.diff(yp, t, 3)) #sp.print_latex(dddyp) lhs = dddyp - 3 * dyp + 2 * yp sp.pretty_print(lhs) sp.print_latex(lhs)
# sp.print_latex(sol)
# term3 = -7 * E**2 * t**4 + 28 * E * t **2 - 432
# sp.pretty_print(term3)
# term3 = sp.simplify(term3)
# sp.pretty_print(term3)
# eq1 = sp.Eq(dy_tilde, (- 3 * y_tilde - 4 * id) / (4 * y_tilde + 3 * id))
# sp.pretty_print(eq1)
# eq2 = sp.Eq(y_hat, (- 3 * y_tilde - 4 * id) / (4 * y_tilde + 3 * id))
# sp.pretty_print(eq2)
haty = (-3 * y_tilde(t) - 4 * t) / (4 * y_tilde(t) + 3 * t)
sp.pretty_print(haty)
sp.print_latex(haty)
tilde_y = sp.solve(sp.Eq(haty, y_hat(t)), y_tilde(t))[0]
sp.pretty_print(tilde_y)
sp.print_latex(tilde_y)
tilde_y_prime = sp.simplify(sp.diff(tilde_y, t))
sp.pretty_print(tilde_y_prime)
sp.print_latex(tilde_y_prime)
sol = sp.simplify(
sp.solve(sp.Eq(tilde_y_prime, y_hat(t)),
y_hat(t).diff(t))[0])
sp.pretty_print(sol)
sp.print_latex(sol)
[0, 0, 0, 1]])
Ltil = sym.Matrix([[gam, bet * gam, 0, 0], [bet * gam, gam, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
G = sym.Matrix([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
# contravariant boost
F1 = L.transpose() * Fcont * L
F1.simplify()
F2 = L * Fcont * L.transpose()
F2.simplify()
print('contravariant')
print(r'\begin{equation}')
sym.print_latex(F1)
print(r'\end{equation}')
print(r'\begin{equation}')
sym.print_latex(F2)
print(r'\end{equation}')
# covariant boost
F1 = Ltil.transpose() * Fcov * Ltil
F1.simplify()
F2 = Ltil * Fcov * Ltil.transpose()
F2.simplify()
print('contravariant')
# sp.pretty_print(y_p)
# #sp.print_latex(y_p)
# ddy = sp.diff(y_p, t, t)
# print("y_p^primeprime = ")
# sp.pretty_print(ddy)
# lhs = sp.simplify(ddy + y_p)
# print("lhs = ")
# sp.pretty_print(lhs)
# # sp.print_latex(sp.expand(lhs))
### c)
y = sp.symbols('y', cls=sp.Function)
diffeq = sp.Eq(y(t).diff(t, t) - y(t), t * sp.exp(-t))
sp.pretty_print(diffeq)
solc = sp.expand(sp.dsolve(diffeq, y(t)))
#sp.pretty_print(solc)
#sp.print_latex(solc)
y_pc = a_1 * t * sp.exp(-t) + a_2 * t**2 * sp.exp(-t)
sp.print_latex(y_pc)
yc_primeprime = sp.expand(sp.diff(y_pc, t, t))
sp.print_latex(yc_primeprime)
sp.print_latex(yc_primeprime - y_pc)
si = 'c1' if i < 3 else 'c2'
sj = 'c1' if j < 3 else 'c2'
print(' K[%d+%s, %d+%s]' % (i % 3, si, j % 3, sj), '+=', K[ind])
if leg_poly:
print('M integrated with Legendre polynomial')
else:
print('M integrated with Hermitian polynomial')
print('printing Me for assignment report')
for ind, val in np.ndenumerate(Me):
i, j = ind
if val == 0:
continue
print('M_e[%d, %d] =' % (i + 1, j + 1), end='')
sympy.print_latex(Me[ind].subs('le', 'l_e'))
print(r'\\')
print('printing Me_lumped for assignment report')
for ind, val in np.ndenumerate(Me_lumped):
i, j = ind
if val == 0:
continue
print('M_e[%d, %d] =' % (i + 1, j + 1), end='')
sympy.print_latex(Me_lumped[ind].subs('le', 'l_e'))
print(r'\\')
M = simplify(R.T * Me * R)
M_lumped = simplify(R.T * Me_lumped * R)
print('printing M for code')
J11 = (1 - k1 * u * v * (2 + u) / ((1 + u)**2)) / c
J12 = (-k1 * u**2 / (1 + u)) / c
J31 = k2 * k3 * v * (2 * u + u**2) / (1 + u)**2
J32 = (-k2 * (1 - k3 * u**2 / (1 + u)))
# print sp.print_latex(J11)
# print sp.print_latex(J12)
# print sp.print_latex(J31)
# print sp.print_latex(J32)
# characteristic_polynomial = (J11 - lam)*(-lam)*(c-lam) + J31*J12*(c-lam) + 0 - (J11 - lam)*J32
#
# print sp.print_latex(characteristic_polynomial)
# eigs_2 = sp.solve(characteristic_polynomial, lam)
# sp.print_latex(eigs_2[0])
# print (sp.expand(eigs_2[0])).evalf()
# sp.print_latex(eigs_2[1])
# print (sp.expand(eigs_2[1])).evalf()
# sp.print_latex(eigs_2[2])
# print (sp.expand(eigs_2[2])).evalf()
characteristic_polynomial_o = (1 / c - lam) * (-lam) * (c - lam) - (
1 / c - lam) * (-k2)
print sp.print_latex(characteristic_polynomial_o)
eigs_2_o = sp.solve(characteristic_polynomial_o, lam)
sp.print_latex(eigs_2_o[0])
print(sp.expand(eigs_2_o[0])).evalf()
sp.print_latex(eigs_2_o[1])
print(sp.expand(eigs_2_o[1])).evalf()
sp.print_latex(eigs_2_o[2])
print(sp.expand(eigs_2_o[2])).evalf()
y0 = sp.Matrix([[y01], [y02]])
print("y_0 =")
sp.pretty_print(y0)
b = sp.Matrix([[t * sp.exp(t**2)], [sp.exp(t**2)]])
b = b.subs(t, s)
print("b(s) = ")
sp.pretty_print(b)
Y = sp.simplify(Y)
print("Y(t) = ")
sp.pretty_print(Y)
Y_inv = sp.simplify(Y**-1)
sp.print_latex(y0)
Y_inv = Y_inv.subs(t, s)
print("Y^{-1}(s) = ")
sp.pretty_print(Y_inv)
A = sp.simplify(Y_inv * b)
print("Y^{-1}(s) b(s) =")
sp.pretty_print(A)
B = sp.simplify(sp.integrate(A, (s, t_0, t)))
print("\int_{t_0}^t Y^{-1}(s) b(s) ds =")
sp.pretty_print(B)
C = sp.simplify(Y * B)
print("Y(t) * \int_{t_0}^t Y^{-1}(s) b(s) ds =")
sp.pretty_print(C)
import sympy as sp
tau, t = sp.symbols('\tau, t')
y = sp.symbols('y', cls=sp.Function)
diffeq = sp.Eq(
2 * t**3 * y(t).diff(t, 3) + 10 * t**2 * y(t).diff(t, 2) -
4 * t * y(t).diff(t) - 20 * y(t), 0)
sp.pretty_print(diffeq)
sol = sp.expand(sp.dsolve(diffeq, y(t)))
sp.pretty_print(sol)
sp.print_latex(sol)
diffeq2 = sp.Eq(
2 * sp.exp(tau)**3 * y(tau).diff(tau, 3) +
10 * sp.exp(tau)**2 * y(tau).diff(tau, 2) -
4 * sp.exp(tau) * y(tau).diff(tau) - 20 * y(tau), 0)
sp.pretty_print(diffeq2)
sol2 = sp.expand(sp.dsolve(diffeq2, y(tau)))
sp.pretty_print(sol2)
for _ in range(10):
TtList.append((eval("t" + str(_)), TatList[_][1]))
############T(t)################
Tt = sympy.Piecewise(*TtList) # T(t,v,tau,t1,t2,t3,t4)
#############T(T) numpy数值计算函数#################
TatF = sympy.lambdify([t, v, tau, T1, T2, T3, T4], Tat, "numpy")
TtF = sympy.lambdify([t, v, tau, T1, T2, T3, T4], Tt, "numpy")
if __name__ == "__main__":
sympy.print_latex(
Tat.subs({
v: sympy.Rational(7, 6),
T1: 175,
T2: 195,
T3: 235,
T4: 255
})) # 将T(t)使用latex表示
import matplotlib.pyplot as plt
# plt.switch_backend('FltkAgg')
fullLength = 25 + 25 + 11 * 30.5 + 10 * 5
# [ 79.92383957, 166.75268173, 197.07910538, 225 , 258.30924988]
vNp = 70 / 60
tNp = numpy.arange(0, fullLength / vNp, 0.1)
tauNp = 47.1
t1Np = 175
t2Np = 195
t3Np = 235
t4Np = 255
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
wn = Symbol('\omega_n', positive=True)
zeta = Symbol('\zeta')
f0 = Symbol('f_0', positive=True)
wf = Symbol('\omega_f', positive=True)
u0 = Symbol('u_0', constant=True)
v0 = Symbol('v_0', constant=True)
# unknown function
u = Function('u')(t)
# solving ODE
f = f0 * sympy.cos(wf * t)
ics = {
u.subs(t, 0): u0,
u.diff(t).subs(t, 0): v0,
}
sol = dsolve(u.diff(t, t) + 2 * zeta * wn * u.diff(t) + wn**2 * u - f, ics=ics)
# sol.lhs ==> u(t)
# sol.rhs ==> solution
print(sol.rhs.simplify())
sympy.print_latex(sol.rhs.simplify())