def test_latex_printer():
r = Function('r')('t')
assert VectorLatexPrinter().doprint(r ** 2) == "r^{2}"
r2 = Function('r^2')('t')
assert VectorLatexPrinter().doprint(r2.diff()) == r'\dot{r^{2}}'
ra = Function('r__a')('t')
assert VectorLatexPrinter().doprint(ra.diff().diff()) == r'\ddot{r^{a}}'
def test_simple():
sympy.var('x, y, r')
u = Function('u')(x, y)
w = Function('w')(x, y)
f = Function('f')(x, y)
e = (u.diff(x) + 1./2*w.diff(x,x)**2)*f.diff(x,y) \
+ w.diff(x,y)*f.diff(x,x)
return Vexpr(e, u, w)
def idiff(eq, y, x, n=1):
"""Return ``dy/dx`` assuming that ``eq == 0``.
Parameters
==========
y : the dependent variable or a list of dependent variables (with y first)
x : the variable that the derivative is being taken with respect to
n : the order of the derivative (default is 1)
Examples
========
>>> from sympy.abc import x, y, a
>>> from sympy.geometry.util import idiff
>>> circ = x**2 + y**2 - 4
>>> idiff(circ, y, x)
-x/y
>>> idiff(circ, y, x, 2).simplify()
-(x**2 + y**2)/y**3
Here, ``a`` is assumed to be independent of ``x``:
>>> idiff(x + a + y, y, x)
-1
Now the x-dependence of ``a`` is made explicit by listing ``a`` after
``y`` in a list.
>>> idiff(x + a + y, [y, a], x)
-Derivative(a, x) - 1
See Also
========
sympy.core.function.Derivative: represents unevaluated derivatives
sympy.core.function.diff: explicitly differentiates wrt symbols
"""
if is_sequence(y):
dep = set(y)
y = y[0]
elif isinstance(y, Symbol):
dep = set([y])
else:
raise ValueError("expecting x-dependent symbol(s) but got: %s" % y)
f = dict([(s, Function(
s.name)(x)) for s in eq.atoms(Symbol) if s != x and s in dep])
dydx = Function(y.name)(x).diff(x)
eq = eq.subs(f)
derivs = {}
for i in range(n):
yp = solve(eq.diff(x), dydx)[0].subs(derivs)
if i == n - 1:
return yp.subs([(v, k) for k, v in f.items()])
derivs[dydx] = yp
eq = dydx - yp
dydx = dydx.diff(x)
def idiff(eq, y, x, n=1):
"""Return ``dy/dx`` assuming that ``eq == 0``.
Parameters
==========
y : the dependent variable or a list of dependent variables (with y first)
x : the variable that the derivative is being taken with respect to
n : the order of the derivative (default is 1)
Examples
========
>>> from sympy.abc import x, y, a
>>> from sympy.geometry.util import idiff
>>> circ = x**2 + y**2 - 4
>>> idiff(circ, y, x)
-x/y
>>> idiff(circ, y, x, 2).simplify()
-(x**2 + y**2)/y**3
Here, ``a`` is assumed to be independent of ``x``:
>>> idiff(x + a + y, y, x)
-1
Now the x-dependence of ``a`` is made explicit by listing ``a`` after
``y`` in a list.
>>> idiff(x + a + y, [y, a], x)
-Derivative(a, x) - 1
See Also
========
sympy.core.function.Derivative: represents unevaluated derivatives
sympy.core.function.diff: explicitly differentiates wrt symbols
"""
if is_sequence(y):
dep = set(y)
y = y[0]
elif isinstance(y, Symbol):
dep = set([y])
else:
raise ValueError("expecting x-dependent symbol(s) but got: %s" % y)
f = dict([(s, Function(s.name)(x)) for s in eq.free_symbols
if s != x and s in dep])
dydx = Function(y.name)(x).diff(x)
eq = eq.subs(f)
derivs = {}
for i in range(n):
yp = solve(eq.diff(x), dydx)[0].subs(derivs)
if i == n - 1:
return yp.subs([(v, k) for k, v in f.items()])
derivs[dydx] = yp
eq = dydx - yp
dydx = dydx.diff(x)
def test_noncommutative_issue_15131():
x = Symbol('x', commutative=False)
t = Symbol('t', commutative=False)
fx = Function('Fx', commutative=False)(x)
ft = Function('Ft', commutative=False)(t)
A = Symbol('A', commutative=False)
eq = fx * A * ft
eqdt = eq.diff(t)
assert eqdt.args[-1] == ft.diff(t)
def test_noncommutative_issue_15131():
x = Symbol("x", commutative=False)
t = Symbol("t", commutative=False)
fx = Function("Fx", commutative=False)(x)
ft = Function("Ft", commutative=False)(t)
A = Symbol("A", commutative=False)
eq = fx * A * ft
eqdt = eq.diff(t)
assert eqdt.args[-1] == ft.diff(t)
def test_noncommutative_issue_15131():
x = Symbol('x', commutative=False)
t = Symbol('t', commutative=False)
fx = Function('Fx', commutative=False)(x)
ft = Function('Ft', commutative=False)(t)
A = Symbol('A', commutative=False)
eq = fx * A * ft
eqdt = eq.diff(t)
assert eqdt.args[-1] == ft.diff(t)
def test_lambdify_Derivative_arg_issue_16468():
f = Function('f')(x)
fx = f.diff()
assert lambdify((f, fx), f + fx)(10, 5) == 15
assert eval(lambdastr((f, fx), f / fx))(10, 5) == 2
raises(SyntaxError, lambda: eval(lambdastr(
(f, fx), f / fx, dummify=False)))
assert eval(lambdastr((f, fx), f / fx, dummify=True))(10, 5) == 2
assert eval(lambdastr((fx, f), f / fx, dummify=True))(S(10), 5) == S.Half
assert lambdify(fx, 1 + fx)(41) == 42
assert eval(lambdastr(fx, 1 + fx, dummify=True))(41) == 42
def test_lambdify_Derivative_arg_issue_16468():
f = Function('f')(x)
fx = f.diff()
assert lambdify((f, fx), f + fx)(10, 5) == 15
assert eval(lambdastr((f, fx), f/fx))(10, 5) == 2
raises(SyntaxError, lambda:
eval(lambdastr((f, fx), f/fx, dummify=False)))
assert eval(lambdastr((f, fx), f/fx, dummify=True))(10, 5) == 2
assert eval(lambdastr((fx, f), f/fx, dummify=True))(10, 5) == S.Half
assert lambdify(fx, 1 + fx)(41) == 42
assert eval(lambdastr(fx, 1 + fx, dummify=True))(41) == 42
def solve_symbolically():
t = symbols('t')
m1, m2, l1, l2, g = symbols('m1 m2 l1 l2 g')
theta1 = Function('theta1')(t)
theta2 = Function('theta2')(t)
x1 = l1 * sin(theta1)
y1 = -l1 * cos(theta1)
x2 = x1 + l2 * sin(theta2)
y2 = y1 - l2 * cos(theta2)
d2x1 = x1.diff(t, 2)
d2y1 = y1.diff(t, 2)
d2x2 = x2.diff(t, 2)
d2y2 = y2.diff(t, 2)
d2theta1 = theta1.diff(t, 2)
d2theta2 = theta2.diff(t, 2)
LHS = -cos(theta1) * (m1 * d2x1 + m2 * d2x2)
RHS = sin(theta1) * (m1 * d2y1 + m2 * d2y2 + m2 * g + m1 * g)
eqn_1 = LHS - RHS
LHS = cos(theta2) * m2 * d2x2
RHS = -sin(theta2) * (m2 * g + m2 * d2y2)
eqn_2 = LHS - RHS
system = [eqn_1, eqn_2]
soln = solve(system, d2theta1, d2theta2)
pprint(soln[d2theta1])
print('\n' * 1)
pprint(soln[d2theta2])
def test_solve_for_functions_derivatives():
t = Symbol('t')
x = Function('x')(t)
y = Function('y')(t)
a11, a12, a21, a22, b1, b2 = symbols('a11,a12,a21,a22,b1,b2')
soln = solve([a11 * x + a12 * y - b1, a21 * x + a22 * y - b2], x, y)
assert soln == {
y: (a11 * b2 - a21 * b1) / (a11 * a22 - a12 * a21),
x: (a22 * b1 - a12 * b2) / (a11 * a22 - a12 * a21)
}
assert solve(x - 1, x) == [1]
assert solve(3 * x - 2, x) == [Rational(2, 3)]
soln = solve([
a11 * x.diff(t) + a12 * y.diff(t) - b1,
a21 * x.diff(t) + a22 * y.diff(t) - b2
], x.diff(t), y.diff(t))
assert soln == {
y.diff(t): (a11 * b2 - a21 * b1) / (a11 * a22 - a12 * a21),
x.diff(t): (a22 * b1 - a12 * b2) / (a11 * a22 - a12 * a21)
}
assert solve(x.diff(t) - 1, x.diff(t)) == [1]
assert solve(3 * x.diff(t) - 2, x.diff(t)) == [Rational(2, 3)]
eqns = set((3 * x - 1, 2 * y - 4))
assert solve(eqns, set((x, y))) == {x: Rational(1, 3), y: 2}
x = Symbol('x')
f = Function('f')
F = x**2 + f(x)**2 - 4 * x - 1
assert solve(F.diff(x), diff(f(x), x)) == [(2 - x) / f(x)]
# Mixed cased with a Symbol and a Function
x = Symbol('x')
y = Function('y')(t)
soln = solve(
[a11 * x + a12 * y.diff(t) - b1, a21 * x + a22 * y.diff(t) - b2], x,
y.diff(t))
assert soln == {
y.diff(t): (a11 * b2 - a21 * b1) / (a11 * a22 - a12 * a21),
x: (a22 * b1 - a12 * b2) / (a11 * a22 - a12 * a21)
}
def test_solve_for_functions_derivatives():
t = Symbol('t')
x = Function('x')(t)
y = Function('y')(t)
a11,a12,a21,a22,b1,b2 = symbols('a11,a12,a21,a22,b1,b2')
soln = solve([a11*x + a12*y - b1, a21*x + a22*y - b2], x, y)
assert soln == {
x : (a22*b1 - a12*b2)/(a11*a22 - a12*a21),
y : (a11*b2 - a21*b1)/(a11*a22 - a12*a21),
}
assert solve(x - 1, x) == [1]
assert solve(3*x - 2, x) == [Rational(2, 3)]
soln = solve([a11*x.diff(t) + a12*y.diff(t) - b1, a21*x.diff(t) +
a22*y.diff(t) - b2], x.diff(t), y.diff(t))
assert soln == { y.diff(t) : (a11*b2 - a21*b1)/(a11*a22 - a12*a21),
x.diff(t) : (a22*b1 - a12*b2)/(a11*a22 - a12*a21) }
assert solve(x.diff(t)-1, x.diff(t)) == [1]
assert solve(3*x.diff(t)-2, x.diff(t)) == [Rational(2,3)]
eqns = set((3*x - 1, 2*y-4))
assert solve(eqns, set((x,y))) == { x : Rational(1, 3), y: 2 }
x = Symbol('x')
f = Function('f')
F = x**2 + f(x)**2 - 4*x - 1
assert solve(F.diff(x), diff(f(x), x)) == [-((x - 2)/f(x))]
# Mixed cased with a Symbol and a Function
x = Symbol('x')
y = Function('y')(t)
soln = solve([a11*x + a12*y.diff(t) - b1, a21*x +
a22*y.diff(t) - b2], x, y.diff(t))
assert soln == { y.diff(t) : (a11*b2 - a21*b1)/(a11*a22 - a12*a21),
x : (a22*b1 - a12*b2)/(a11*a22 - a12*a21) }
def modeling(linearized=True):
t = sp.Symbol('t') # time
params = sp.symbols('m0, m1, J1, a1, l1, g, d0, d1') # system parameters
m0, m1, J1, a1, l1, g, d0, d1 = params # J = 4/3*m1*l1**2
'''
http://www.derongliu.org/adp/adp-cdrom/Barto1983.pdf:
params_values = [(m0, 1.0), (m1, 0.1), (J1, 0.133), (a1, 0.5),
(l1, 1), (g, 9.81), (d0, 5*1e-4), (d1, 2*1e-6)]
https://github.com/TUD-RST/pytutorials/blob/master/03-Jupyter-Notebook-And-SymPy/sim/01_notebook.ipynb:
params_values = [(m0, 3.34), (m1, 0.3583), (J1, 0.0379999),
(l1, 0.5), (a1, 0.43), (g, 9.81), (d0, 0.1), (d1, 0.006588)]
https://www.acin.tuwien.ac.at/file/publications/cds/pre_post_print/glueck2013.pdf
params_values = [(m0, 3.34), (m1, 0.876), (J1, 0.013), (a1, 0.215), (l1, 0.323), (g, 9.81),
(d0, 0.1), (d1, 0.215)]
'''
params_values = [(m0, 3.34), (m1, 0.3583), (J1, 0.0379999), (l1, 0.5),
(a1, 0.43), (g, 9.81), (d0, 0.1), (d1, 0.006588)]
# force
F = sp.Symbol('F')
# generalized coordinates
q0_t = Function('q0')(t)
dq0_t = q0_t.diff(t)
ddq0_t = q0_t.diff(t, 2)
q1_t = Function('q1')(t)
dq1_t = q1_t.diff(t)
ddq1_t = q1_t.diff(t, 2)
# position vectors
p0 = sp.Matrix([q0_t, 0])
p1 = sp.Matrix([q0_t - a1 * sin(q1_t), a1 * cos(q1_t)])
# velocity vectors
dp0 = p0.diff(t)
dp1 = p1.diff(t)
# kinetic energy T
T0 = m0 / 2 * (dp0.T * dp0)[0]
T1 = (m1 * (dp1.T * dp1)[0] + J1 * dq1_t**2) / 2
T = T0 + T1
# potential energy V
V = m1 * g * p1[1]
# lagrangian L
L = T - V
L = L.expand()
L = sp.trigsimp(L)
# Lagrange equations of the second kind
# d/dt(dL/d(dq_i/dt)) - dL/dq_i = Q_i
Q0 = F - d0 * dq0_t
Q1 = -d1 * dq1_t
Eq0 = L.diff(dq0_t, t) - L.diff(q0_t) - Q0 # = 0
Eq1 = L.diff(dq1_t, t) - L.diff(q1_t) - Q1 # = 0
# equations of motion
Eq = sp.Matrix([Eq0, Eq1])
# partial linerization / acceleration as input, not force/torque
# new input - acceleration of the cart
a = sp.Function('a')(t)
# replace ddq0 with a
Eq_a = Eq.subs(ddq0_t, a)
# solve for F
sol = sp.solve(Eq_a, F)
Fa = sol[F] # F(a)
# partial linearization
Eq_lin = Eq.subs(F, Fa)
# solve for ddq/dt
ddq_t = sp.Matrix([ddq0_t, ddq1_t])
if linearized:
ddq = sp.solve(Eq_lin, ddq_t)
else:
ddq = sp.solve(Eq, ddq_t)
# state space model
# functions of x, u
x1_t = sp.Function('x1')(t)
x2_t = sp.Function('x2')(t)
x3_t = sp.Function('x3')(t)
x4_t = sp.Function('x4')(t)
x_t = sp.Matrix([x1_t, x2_t, x3_t, x4_t])
u_t = sp.Function('u')(t)
# symbols of x, u
x1, x2, x3, x4, u = sp.symbols("x1, x2, x3, x4, u")
xx = [x1, x2, x3, x4]
# replace generalized coordinates with states
if linearized:
xu_subs = [(dq0_t, x3_t), (dq1_t, x4_t), (q0_t, x1_t), (q1_t, x2_t),
(a, u_t)]
else:
xu_subs = [(dq0_t, x3_t), (dq1_t, x4_t), (q0_t, x1_t), (q1_t, x2_t),
(F, u_t)]
# first order ODE (right hand side)
dx_t = sp.Matrix([x3_t, x4_t, ddq[ddq0_t], ddq[ddq1_t]])
dx_t = dx_t.subs(xu_subs)
# linearized dynamics
A = dx_t.jacobian(x_t)
B = dx_t.diff(u_t)
# symbolic expressions of A and B with parameter values
Asym = A.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
Bsym = B.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
dx_t_sym = dx_t.subs(list(zip(x_t, xx))).subs(u_t, u).subs(
params_values) # replacing all symbolic functions with symbols
print(dx_t_sym)
if linearized:
lin = '_lin'
else:
lin = ''
with open('c_files/cart_pole' + lin + '_ode.p', 'wb') as opened_file:
pickle.dump(dx_t_sym, opened_file)
with open('c_files/cart_pole' + lin + '_A.p', 'wb') as opened_file:
pickle.dump(Asym, opened_file)
with open('c_files/cart_pole' + lin + '_B.p', 'wb') as opened_file:
pickle.dump(Bsym, opened_file)
dxdt, A, B = load_existing()
return dxdt, A, B
def _test_f():
# FIXME: we get infinite recursion here:
x = Symbol("x")
f = Function("f")
assert residue(f(x) / x**5, x, 0) == f.diff(x, 4) / 24
# symbols
t = Symbol('t', positive=True)
zeta = Symbol('\zeta', positive=True)
omegan = Symbol('\omega_n', positive=True)
omegad = Symbol('\omega_d', positive=True)
epsilon = Symbol(r'\varepsilon', positive=True)
tn = Symbol('t_n', positive=True)
P0 = Symbol('P0')
m = Symbol('m', positive=True)
u0 = 0
v0 = 0
# unknown function
u = Function('u')(t)
# solving ODE (mass-normalized EOM)
f = P0*sympy.DiracDelta(t-tn)
ics = {u.subs(t, 0): u0,
u.diff(t).subs(t, 0): v0,
}
sol = dsolve(u.diff(t, t) + 2*zeta*omegan*u.diff(t) + omegan**2*u - f/m, ics=ics)
display(sympy.simplify(sol.rhs))
from sympy.plotting import plot
plot(sol.rhs.subs({omegan: 10, zeta: 0.1, tn: 3, P0: 1, m: 3}), (t, 0, 10),
adaptive=False,
nb_of_points=1000,
ylabel='$u(t)$')
def modeling(linearized=True):
t = sp.Symbol('t') # time
params = sp.symbols(
'm0, m1, m2, J1, J2, l1, l2, g, d0, d1, d2') # system parameters
m0, m1, m2, J1, J2, l1, l2, g, d0, d1, d2 = params
params_values = [(m0, 3.34), (m1, 0.3583), (m2, 0.5383), (J1, 0.0379999),
(J2, 0.12596), (l1, 0.5), (l2, 0.75), (g, 9.81),
(d0, 0.1), (d1, 0.006588), (d2, 0.006588)]
# force
F = sp.Symbol('F')
# generalized coordinates
q0_t = Function('q0')(t)
dq0_t = q0_t.diff(t)
ddq0_t = q0_t.diff(t, 2)
q1_t = Function('q1')(t)
dq1_t = q1_t.diff(t)
ddq1_t = q1_t.diff(t, 2)
q2_t = Function('q2')(t)
dq2_t = q2_t.diff(t)
ddq2_t = q2_t.diff(t, 2)
# position vectors
p0 = sp.Matrix([q0_t, 0])
p1 = sp.Matrix([q0_t - 0.5 * l1 * sin(q1_t), 0.5 * l1 * cos(q1_t)])
p2 = sp.Matrix([q0_t - 0.5 * l2 * sin(q2_t), 0.5 * l2 * cos(q2_t)])
# velocity vectors
dp0 = p0.diff(t)
dp1 = p1.diff(t)
dp2 = p2.diff(t)
# kinetic energy T
T0 = m0 / 2 * (dp0.T * dp0)[0]
T1 = (m1 * (dp1.T * dp1)[0] + J1 * dq1_t**2) / 2
T2 = (m2 * (dp2.T * dp2)[0] + J2 * dq2_t**2) / 2
T = T0 + T1 + T2
# potential energy V
V = m1 * g * p1[1] + m2 * g * p2[1]
# lagrangian L
L = T - V
# Lagrange equations of the second kind
# d/dt(dL/d(dq_i/dt)) - dL/dq_i = Q_i
Q0 = F - d0 * dq0_t
Q1 = -d1 * dq1_t
Q2 = -d2 * dq2_t
Eq0 = L.diff(dq0_t, t) - L.diff(q0_t) - Q0 # = 0
Eq1 = L.diff(dq1_t, t) - L.diff(q1_t) - Q1 # = 0
Eq2 = L.diff(dq2_t, t) - L.diff(q2_t) - Q2 # = 0
# equations of motion
Eq = sp.Matrix([Eq0, Eq1, Eq2])
# partial linerization / acceleration as input, not force/torque
# new input - acceleration of the cart
a = sp.Function('a')(t)
# replace ddq0 with a
Eq_a = Eq.subs(ddq0_t, a)
# solve for F
sol = sp.solve(Eq_a, F)
Fa = sol[F] # F(a)
# partial linearization
Eq_lin = Eq.subs(F, Fa)
# solve for ddq/dt
ddq_t = sp.Matrix([ddq0_t, ddq1_t, ddq2_t])
if linearized:
ddq = sp.solve(Eq_lin, ddq_t)
else:
ddq = sp.solve(Eq, ddq_t)
# state space model
# functions of x, u
x1_t = sp.Function('x1')(t)
x2_t = sp.Function('x2')(t)
x3_t = sp.Function('x3')(t)
x4_t = sp.Function('x4')(t)
x5_t = sp.Function('x5')(t)
x6_t = sp.Function('x6')(t)
x_t = sp.Matrix([x1_t, x2_t, x3_t, x4_t, x5_t, x6_t])
u_t = sp.Function('u')(t)
# symbols of x, u
x1, x2, x3, x4, x5, x6, u = sp.symbols("x1, x2, x3, x4, x5, x6, u")
xx = [x1, x2, x3, x4, x5, x6]
# replace generalized coordinates with states
if linearized:
xu_subs = [(dq0_t, x4_t), (dq1_t, x5_t), (dq2_t, x6_t), (q0_t, x1_t),
(q1_t, x2_t), (q2_t, x3_t), (a, u_t)]
else:
xu_subs = [(dq0_t, x4_t), (dq1_t, x5_t), (dq2_t, x6_t), (q0_t, x1_t),
(q1_t, x2_t), (q2_t, x3_t), (F, u_t)]
# first order ODE (right hand side)
dx_t = sp.Matrix([x4_t, x5_t, x6_t, ddq[ddq0_t], ddq[ddq1_t], ddq[ddq2_t]])
dx_t = dx_t.subs(xu_subs)
# linearized dynamics
A = dx_t.jacobian(x_t)
B = dx_t.diff(u_t)
# symbolic expressions of A and B with parameter values
Asym = A.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
Bsym = B.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
dx_t_sym = dx_t.subs(list(zip(x_t, xx))).subs(u_t, u).subs(
params_values) # replacing all symbolic functions with symbols
print(dx_t_sym)
if linearized:
lin = '_lin'
else:
lin = ''
with open('c_files/cart_pole_double_parallel' + lin + '_ode.p',
'wb') as opened_file:
pickle.dump(dx_t_sym, opened_file)
with open('c_files/cart_pole_double_parallel' + lin + '_A.p',
'wb') as opened_file:
pickle.dump(Asym, opened_file)
with open('c_files/cart_pole_double_parallel' + lin + '_B.p',
'wb') as opened_file:
pickle.dump(Bsym, opened_file)
# RHS as callable function
dxdt, A, B = load_existing()
return dxdt, A, B
def test_cylinder_clpt():
'''Test case where the functional corresponds to the internal energy of
a cylinder using the Classical Laminated Plate Theory (CLPT)
'''
from sympy import Matrix
sympy.var('x, y, r')
sympy.var('B11, B12, B16, B21, B22, B26, B61, B62, B66')
sympy.var('D11, D12, D16, D21, D22, D26, D61, D62, D66')
# displacement field
u = Function('u')(x, y)
v = Function('v')(x, y)
w = Function('w')(x, y)
# stress function
f = Function('f')(x, y)
# laminate constitute matrices B represents B*, see Jones (1999)
B = Matrix([[B11, B12, B16],
[B21, B22, B26],
[B61, B62, B66]])
# D represents D*, see Jones (1999)
D = Matrix([[D11, D12, D16],
[D12, D22, D26],
[D16, D26, D66]])
# strain-diplacement equations
e = Matrix([[u.diff(x) + 1./2*w.diff(x)**2],
[v.diff(y) + 1./r*w + 1./2*w.diff(y)**2],
[u.diff(y) + v.diff(x) + w.diff(x)*w.diff(y)]])
k = Matrix([[ -w.diff(x, x)],
[ -w.diff(y, y)],
[-2*w.diff(x, y)]])
# representing the internal forces using the stress function
N = Matrix([[ f.diff(y, y)],
[ f.diff(x, x)],
[ -f.diff(x, y)]])
functional = N.T*V(e) - N.T*B*V(k) + k.T*D.T*V(k)
return Vexpr(functional, u, v, w)
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
# unknown function
u = Function('u')(t)
# solving ODE with initial conditions
u0 = 0.4
v0 = 2
k = 150
m = 2
ics = {u.subs(t, 0): u0, u.diff(t).subs(t, 0): v0}
sol = dsolve(m * u.diff(t, t) + k * u, ics=ics)
import matplotlib
matplotlib.use('TkAgg')
from sympy.plotting import plot
p1 = plot(sol.rhs, (t, 0, 1), xlabel='$t$', ylabel='$u(t)$')
# unknown function
u = Function('u')(t)
# assumed values
wn = 5.
u0 = 1
v0 = 0
wf = wn * r
f0 = 10.
# 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)
import matplotlib
matplotlib.use('TkAgg')
from sympy.plotting import plot3d
subs = {zeta: 0.2}
p1 = plot3d(
sol.rhs.subs(subs),
(t, 0, 10),
(r, 0.1, 2),
show=False,
nb_of_points_x=250,
nb_of_points_y=250,
pprint(eq2(u(x, y), v(x, y)))
print "\nSolution:"
u_hat = 1 - (exp(k*x)+exp(-k*x)) / (exp(k)+exp(-k))
pprint(Eq(Function("u_hat")(x), u_hat))
#test the Boundary Conditions:
assert u_hat.subs(x, -1) == 0
assert u_hat.subs(x, 1) == 0
u = cos(x*pi/2)*cos(y*pi/2)
v = u_hat*u_hat.subs(x, y)
print "u:"
pprint(u)
print "u:", ccode(u)
print "d u/d x:", ccode(u.diff(x))
print "d u/d y:", ccode(u.diff(y))
print "v:"
pprint(v)
print "v:", ccode(v)
print "d v/d x:", ccode(v.diff(x))
print "d v/d y:", ccode(v.diff(y))
print "-"*80
print "RHS, for f(u)=u:"
# delete this redefinition to get nonlinear f(u):
def f(u):
return u
e1 = eq1(u, v, f)
e2 = eq2(u, v)
print "g1:"
pretty_print(Eq(psi_2, Integral(-G * der_m / r1, (r1, r, R))))
p2 = integrate(-G * der_m / r1, (r1, r, R))
p2 = simplify(p2)
eq_psi = eq_psi.subs(psi_2, p2)
pretty_print(simplify(eq_psi))
pretty_print('''Then: ''')
psi = solve(eq_psi, psi)[0]
# assert edge case for r = R
assert psi.subs(r, R) + G * M / R == 0
C = Function('C')(r)
pretty_print(Eq(C, -Derivative(Function('psi')(r), r)))
eq_C = Eq(C, -psi.diff(r))
pretty_print(eq_C)
C = solve(eq_C, C)[0]
# assert edge case for r = R
assert C.subs(r, R) + G * M / R**2 == 0
# suppose we wave such unis that GM /R **3 = 1, and R = 1
# plot potential `psi` vs distance to the center, r:
rs = np.linspace(0., 1.)
psi = psi.subs({G: 1., M: 1., R: 1.})
f_psi = sym.lambdify(r, psi)
plt.plot(rs, f_psi(rs))
from IPython.display import display
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
m = Symbol('m', positive=True)
k = Symbol('k', positive=True)
c = Symbol('c', positive=True)
# unknown function
u = Function('u')(t)
# solving ODE
sol = dsolve(m * u.diff(t, t) + c * u.diff(t) + k * u)
# sol.lhs ==> u(t)
# sol.rhs ==> solution
display(sol.rhs)
from sympy import Function, Symbol, symbols, Derivative, preview, simplify, collect, Wild
from sympy import diff, ln, sin, pprint, sqrt, latex, Integral
import sympy as sym
x, y, z, t = symbols('x y z t')
f = Function('f')
F = Function('F')(x, y, z)
g = Function('g', real=True)(ln(F))
result = diff(sin(x), x)
print(result)
print(f(x * x).diff(x))
print(g.diff(x))
aaa = g.diff(x, 2)
# aaa._args[1] * aaa._args[0]
# init_printing()
pprint(Integral(sqrt(1 / x), x), use_unicode=True)
print(latex(Integral(sqrt(1 / x), x)))
print("\n\n")
pprint(g.diff((x, 1), (y, 0)), use_unicode=True)
# pprint(g.diff((x, 2),(y, 2)), use_unicode=True)
pprint(sym.diff(sym.tan(x), x))
pprint(sym.diff(g, x))
print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
from sympy import Derivative as D, collect, Function
g3=g3.subs(sin(phi)**2, 1-cos(phi)**2).expand().simplify()
print r'%\alpha^{2} + \beta^{2} + \gamma^{2} = ', latex(g3)
x_hat = Matrix([
rho * sin(theta) * cos(phi),
rho * sin(theta) * sin(phi),
rho * cos(theta)])
psi = Function("psi")
psi =exp(I*omega*t) * exp(-I*g2)
print "\Psi =", latex(psi)
#Derivatives of the wave function of the coordinates
dpsidx=psi.diff(x)
dpsidx=dpsidx.subs(psi,'Psi').expand().simplify()
print "d \Psi / dx =", latex(dpsidx)
dpsidy=psi.diff(y)
dpsidy=dpsidy.subs(psi,'Psi').expand().simplify()
print "d \Psi / dy =", latex(dpsidy)
dpsidz=psi.diff(z)
dpsidz=dpsidz.subs(psi,'Psi').expand().simplify()
print "d \Psi / dz =", latex(dpsidz)
########### Maxwell's Equations ###################
#A,B,C1,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R = symbols('A B C1 D E F G H I J K L M N O P Q R', cls=Function)
omega, mu, epsilon, muz, epsilonz = symbols('omega mu epsilon mu_z epsilon_z', integer=True)
# Vertical indexs: Hx Hy Hz Ex Ey Ez
pprint(eq2(u(x, y), v(x, y)))
print "\nSolution:"
u_hat = 1 - (exp(k * x) + exp(-k * x)) / (exp(k) + exp(-k))
pprint(Eq(Function("u_hat")(x), u_hat))
#test the Boundary Conditions:
assert u_hat.subs(x, -1) == 0
assert u_hat.subs(x, 1) == 0
u = cos(x * pi / 2) * cos(y * pi / 2)
v = u_hat * u_hat.subs(x, y)
print "u:"
pprint(u)
print "u:", ccode(u)
print "d u/d x:", ccode(u.diff(x))
print "d u/d y:", ccode(u.diff(y))
print "v:"
pprint(v)
print "v:", ccode(v)
print "d v/d x:", ccode(v.diff(x))
print "d v/d y:", ccode(v.diff(y))
print "-" * 80
print "RHS, for f(u)=u:"
# delete this redefinition to get nonlinear f(u):
def f(u):
return u
from sympy import Function, Symbol, symbols, Derivative, preview, simplify, collect, Wild
from sympy import diff, ln, sin, pprint, sqrt, latex, Integral
import sympy as sym
x, y, z, t = symbols('x y z t')
f = Function('f')
F = Function('F')(x, y, z)
g = Function('g', real=True)(ln(F))
result = diff(sin(x), x)
print(result)
print(f(x * x).diff(x))
print(g.diff(x))
aaa = g.diff(x, 2)
# aaa._args[1] * aaa._args[0]
# init_printing()
pprint(Integral(sqrt(1 / x), x), use_unicode=True)
print(latex(Integral(sqrt(1 / x), x)))
print("\n\n")
pprint(g.diff((x, 1), (y, 0)), use_unicode=True)
# pprint(g.diff((x, 2),(y, 2)), use_unicode=True)
pprint(sym.diff(sym.tan(x), x))
pprint(sym.diff(g, x))
print("xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx")
from sympy import Derivative as D, collect, Function
from IPython.display import display
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
omegan = Symbol('\omega_n', positive=True)
zeta = Symbol('\zeta')
# unknown function
u = Function('u')(t)
# solving ODE
sol = dsolve(u.diff(t, t) + 2*zeta*omegan*u.diff(t) + omegan**2*u)
# sol.lhs ==> u(t)
# sol.rhs ==> solution
display(sol.rhs.simplify())
display()
sympy.display(sol.rhs.simplify())
Lagrange formalism
@author: Topher
"""
import sympy as sp
from sympy import sin,cos,Function
t = sp.Symbol('t')
params = sp.symbols('M , G , J , J_ball , R')
M , G , J , J_ball , R = params
# ball position r
r_t = Function('r')(t)
d_r_t = r_t.diff(t)
dd_r_t = r_t.diff(t,2)
# beam angle theta
theta_t = Function('theta')(t)
d_theta_t = theta_t.diff(t)
dd_theta_t = theta_t.diff(t,2)
# torque of the beam
tau = Function('tau')
# kinetic energy
T = ((M + J_ball/R**2)*d_r_t**2 + (J + M*r_t**2 + J_ball)*d_theta_t**2)/2
# potential energy
V = M*G*r_t*sin(theta_t)
# lagrange function
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
zeta = Symbol('\zeta', constant=True, positive=True)
# unknown function
u = Function('u')(t)
# assumed values
u0 = 1
v0 = 0
wn = 4.
wd = wn * sympy.sqrt(1 - zeta**2)
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, ics=ics)
import matplotlib
matplotlib.use('TkAgg')
from sympy.plotting import plot3d
p1 = plot3d(
sol.rhs,
(t, 0, 10),
(zeta, 0.05, 0.7),
show=False,
nb_of_points_x=500,
nb_of_points_y=10,
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
wn = Symbol('\omega_n', positive=True)
zeta = Symbol('\zeta')
# unknown function
u = Function('u')(t)
# solving ODE
sol = dsolve(u.diff(t, t) + 2 * zeta * wn * u.diff(t) + wn**2 * u)
# sol.lhs ==> u(t)
# sol.rhs ==> solution
print(sol.rhs.simplify())
print()
sympy.print_latex(sol.rhs.simplify())
q9 = asin(dot(H[1], A[2])) # Front wheel yaw relative to rear wheel yaw
q10 = asin(dot(E[2], N[3])) # Front assembly lean
front_pitch = asin(-dot(E[1], g3)) # Front assembly pitch
"""
# Expressions for E[1] and E[3] measure numbers of g3
g31_expr = dot(g3, E[1])
g33_expr = dot(g3, E[3])
num1 = g3_num.dict[E[1]]
num2 = g3_num.dict[E[3]]
den = g3_den
# Time derivatives of expressions for E[1] and E[3] measure numbers of g3
g31_expr_dt = (num1.diff(t)*den - num1*den.diff(t))/den**2
g33_expr_dt = (num2.diff(t)*den - num2*den.diff(t))/den**2
g3_dict = {g31: g31_expr, g33: g33_expr, g31.diff(t): g31_expr_dt,
g33.diff(t): g33_expr_dt}
g3_symbol_dict = {g31: g31_s, g33: g33_s, g31.diff(t): g31d_s, g33.diff(t):
g33d_s}
# Position vector from front wheel center to front wheel contact point
# g31 and g33 are Sympy Functions which are 'unknown' functions of time.
fo_fn = Vector({E[1]: rf*g31, E[3]: rf*g33, N[3]: rft})
# Locate rear wheel center
CO = N.O.locate('CO', -rrt*N[3] -rr*B[3], C)
# Locate mass center of rear assembly (rear wheel, rear frame and rider)
CDO = CO.locate('CDO', l1*D[1] + l2*D[3], D, mass=mcd)
# Locate top of steer axis
DE = CO.locate('DE', lr*D[1], D)
# Locate front wheel center
def test_cylinder_clpt():
'''Test case where the functional corresponds to the internal energy of
a cylinder using the Classical Laminated Plate Theory (CLPT)
'''
from sympy import Matrix
sympy.var('x, y, r')
sympy.var('B11, B12, B16, B21, B22, B26, B61, B62, B66')
sympy.var('D11, D12, D16, D21, D22, D26, D61, D62, D66')
# displacement field
u = Function('u')(x, y)
v = Function('v')(x, y)
w = Function('w')(x, y)
# stress function
f = Function('f')(x, y)
# laminate constitute matrices B represents B*, see Jones (1999)
B = Matrix([[B11, B12, B16], [B21, B22, B26], [B61, B62, B66]])
# D represents D*, see Jones (1999)
D = Matrix([[D11, D12, D16], [D12, D22, D26], [D16, D26, D66]])
# strain-diplacement equations
e = Matrix([[u.diff(x) + 1. / 2 * w.diff(x)**2],
[v.diff(y) + 1. / r * w + 1. / 2 * w.diff(y)**2],
[u.diff(y) + v.diff(x) + w.diff(x) * w.diff(y)]])
k = Matrix([[-w.diff(x, x)], [-w.diff(y, y)], [-2 * w.diff(x, y)]])
# representing the internal forces using the stress function
N = Matrix([[f.diff(y, y)], [f.diff(x, x)], [-f.diff(x, y)]])
functional = N.T * e - N.T * B * k + 1. / 2 * k.T * D.T * k
return Vexpr(functional, u, v, w)
#u3p = Function("u3p")(t)
#u4p = Function("u4p")(t)
#u5p = Function("u5p")(t)
P_NC_CO = r2*A[3] - r1*B[3]
#print "P_NC_CO> = ", P_NC_CO
P_NO_CO = q1*N[1] + q2*N[2] + P_NC_CO
V_CN_N = au1*A[1] + au2*A[2] + au3*A[3]
#print "V_CN_N> = ", V_CN_N
V_CO_N = V_CN_N + cross(C.get_omega(N), P_NC_CO)
#print "V_CO_N> = ", V_CO_N
qdots = [q3.diff(t), q4.diff(t), q5.diff(t), au1, au2, au3, u3.diff(t), \
u4.diff(t), u5.diff(t)]
us = [u3, u4, u5, au1, au2, au3, u3p, u4p, u5p]
gen_speeds = dict(zip(qdots, us))
C.set_omega(C.get_omega(N).subs(gen_speeds), N, force = True)
V_CO_N = V_CO_N.subs(gen_speeds)
V_CN_N = V_CN_N.subs(gen_speeds)
WC = coeff(C.get_omega(N), us[:-3])
VC = coeff(V_CO_N, us[:-3])
VCN = coeff(V_CN_N, us[:-3])
#print WC
# Program 05d: The Lindstedt-Poincare Method
# Deriving the order epsilon equations.
# See Example 9.
from sympy import collect, expand, Function, Symbol
x0 = Function('x0')
x1 = Function('x1')
x2 = Function('x2')
x = Function('x')
t = Symbol('t')
eps = Symbol('eps')
w1 = Symbol('w1')
w2 = Symbol('w2')
x = x0(t) + eps * x1(t) + eps**2 * x2(t)
expr = (1 + eps * w1 + eps**2 * w2)**2 * x.diff(t, t) + x - eps * x**3
expr = expand(expr)
expr = collect(expr, eps)
print(expr)
def _test_f():
# FIXME: we get infinite recursion here:
f = Function("f")
assert residue(f(x)/x**5, x, 0) == f.diff(x, 4)/24
def modeling(linearized=True):
t = sp.Symbol('t') # time
params = sp.symbols(
'm0, m1, J0, J1, l0, l1, g, d0, d1') # system parameters
m0, m1, J0, J1, l0, l1, g, d0, d1 = params
params_values = [(m0, 0.3583), (m1, 0.3583), (J0, 0.0379999),
(J1, 0.0379999), (l0, 0.5), (l1, 0.5), (g, 9.81),
(d0, 0.006588), (d1, 0.006588)]
# torque
tau = sp.Symbol('tau')
# generalized coordinates
q0_t = Function('q0')(t)
dq0_t = q0_t.diff(t)
ddq0_t = q0_t.diff(t, 2)
q1_t = Function('q1')(t)
dq1_t = q1_t.diff(t)
ddq1_t = q1_t.diff(t, 2)
# position vectors of COMs
p0 = sp.Matrix([-0.5 * l0 * sin(q0_t), 0.5 * l0 * cos(q0_t)])
p1 = 2 * p0 + sp.Matrix(
[-0.5 * l1 * sin(q0_t + q1_t), 0.5 * l1 * cos(q0_t + q1_t)])
# velocity vectors
dp0 = p0.diff(t)
dp1 = p1.diff(t)
# kinetic energy T
T0 = (m0 * (dp0.T * dp0)[0] + J0 * dq0_t**2) / 2
T1 = (m1 * (dp1.T * dp1)[0] + J1 * (dq0_t + dq1_t)**2) / 2
T = T0 + T1
# potential energy V
V = m0 * g * p0[1] + m1 * g * p1[1]
# lagrangian L
L = T - V
L = L.expand()
L = sp.trigsimp(L)
# Lagrange equations of the second kind
# d/dt(dL/d(dq_i/dt)) - dL/dq_i = Q_i
Q0 = -d0 * dq0_t
Q1 = tau - d1 * dq1_t
Eq0 = L.diff(dq0_t, t) - L.diff(q0_t) - Q0 # = 0
Eq1 = L.diff(dq1_t, t) - L.diff(q1_t) - Q1 # = 0
# equations of motion
Eq = sp.Matrix([Eq0, Eq1])
# partial linerization / acceleration as input, not force/torque
# new input - acceleration of the cart
a = sp.Function('a')(t)
# replace ddq0 with a
Eq_a = Eq.subs(ddq1_t, a)
# solve for F
sol = sp.solve(Eq_a, tau)
tau_a = sol[tau] # F(a)
# partial linearization
Eq_lin = Eq.subs(tau, tau_a)
# solve for ddq/dt
ddq_t = sp.Matrix([ddq0_t, ddq1_t])
if linearized:
ddq = sp.solve(Eq_lin, ddq_t)
else:
ddq = sp.solve(Eq, ddq_t)
# state space model
# functions of x, u
x1_t = sp.Function('x1')(t)
x2_t = sp.Function('x2')(t)
x3_t = sp.Function('x3')(t)
x4_t = sp.Function('x4')(t)
x_t = sp.Matrix([x1_t, x2_t, x3_t, x4_t])
u_t = sp.Function('u')(t)
# symbols of x, u
x1, x2, x3, x4, u = sp.symbols("x1, x2, x3, x4, u")
xx = [x1, x2, x3, x4]
# replace generalized coordinates with states
if linearized:
xu_subs = [(dq0_t, x3_t), (dq1_t, x4_t), (q0_t, x1_t), (q1_t, x2_t),
(a, u_t)]
else:
xu_subs = [(dq0_t, x3_t), (dq1_t, x4_t), (q0_t, x1_t), (q1_t, x2_t),
(tau, u_t)]
# first order ODE (right hand side)
dx_t = sp.Matrix([x3_t, x4_t, ddq[ddq0_t], ddq[ddq1_t]])
dx_t = dx_t.subs(xu_subs)
# linearized dynamics
A = dx_t.jacobian(x_t)
B = dx_t.diff(u_t)
# symbolic expressions of A and B with parameter values
Asym = A.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
Bsym = B.subs(list(zip(x_t, xx))).subs(u_t, u).subs(params_values)
dx_t_sym = dx_t.subs(list(zip(x_t, xx))).subs(u_t, u).subs(
params_values) # replacing all symbolic functions with symbols
print(dx_t_sym)
if linearized:
lin = '_lin'
else:
lin = ''
with open('c_files/acrobot' + lin + '_ode.p', 'wb') as opened_file:
pickle.dump(dx_t_sym, opened_file)
with open('c_files/acrobot' + lin + '_A.p', 'wb') as opened_file:
pickle.dump(Asym, opened_file)
with open('c_files/acrobot' + lin + '_B.p', 'wb') as opened_file:
pickle.dump(Bsym, opened_file)
dxdt, A, B = load_existing()
return dxdt, A, B
# Simplifying
g3 = g3.subs(sin(phi)**2, 1 - cos(phi)**2).expand().simplify()
print r'%\alpha^{2} + \beta^{2} + \gamma^{2} = ', latex(g3)
x_hat = Matrix([
rho * sin(theta) * cos(phi), rho * sin(theta) * sin(phi), rho * cos(theta)
])
psi = Function("psi")
psi = exp(I * omega * t) * exp(-I * g2)
print "\Psi =", latex(psi)
#Derivatives of the wave function of the coordinates
dpsidx = psi.diff(x)
dpsidx = dpsidx.subs(psi, 'Psi').expand().simplify()
print "d \Psi / dx =", latex(dpsidx)
dpsidy = psi.diff(y)
dpsidy = dpsidy.subs(psi, 'Psi').expand().simplify()
print "d \Psi / dy =", latex(dpsidy)
dpsidz = psi.diff(z)
dpsidz = dpsidz.subs(psi, 'Psi').expand().simplify()
print "d \Psi / dz =", latex(dpsidz)
########### Maxwell's Equations ###################
#A,B,C1,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R = symbols('A B C1 D E F G H I J K L M N O P Q R', cls=Function)
omega, mu, epsilon, muz, epsilonz = symbols('omega mu epsilon mu_z epsilon_z',
integer=True)
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
k = Symbol('k', positive=True)
m = Symbol('m', positive=True)
tn = Symbol('t_n', positive=True)
P0 = Symbol('P0', positive=True)
u0 = Symbol('u_0')
v0 = Symbol('v_0')
omegan = Symbol('\omega_n')
# unknown function
u = Function('u')(t)
# solving ODE
f = P0*sympy.DiracDelta(t - tn)
ics = {u.subs(t, 0): u0,
u.diff(t).subs(t, 0): v0,
}
sol = dsolve(m*u.diff(t, t) + k*u - f, ics=ics)
display(sympy.simplify(sol.rhs))
#expr = sympy.sqrt(k/m)
#display(sympy.simplify(sol.rhs.expand().subs({expr: omegan})))
#expr2 = sympy.sqrt(k*m)
#expr3 = 1/expr
#display(sympy.simplify(sol.rhs.expand().subs({expr: omegan, expr2: m*omegan, expr3: 1/omegan})))
def lagrange_model():
'''
'''
t = sp.Symbol('t')
params = sp.symbols(
'm_0, m_1, m_2, m_3, J_1, J_2, J_3, l_1, l_2, l_3, a_1, a_2, a_3, g, d_1, d_2, d_3'
)
m0, m1, m2, m3, J1, J2, J3, l1, l2, l3, a1, a2, a3, g, d1, d2, d3 = params
F = sp.Symbol('F')
q1t = Function('q1')(t)
q2t = Function('q2')(t)
q3t = Function('q3')(t)
q4t = Function('q4')(t)
q1dt = q1t.diff(t)
q2dt = q2t.diff(t)
q3dt = q3t.diff(t)
q4dt = q4t.diff(t)
q1ddt = q1t.diff(t, 2)
q2ddt = q2t.diff(t, 2)
q3ddt = q3t.diff(t, 2)
q4ddt = q4t.diff(t, 2)
P1 = sp.Matrix([q4t + a1 * sin(q1t), +a1 * cos(q1t)])
P2 = sp.Matrix(
[q4t + l1 * sin(q1t) + a2 * sin(q2t), l1 * cos(q1t) + a2 * cos(q2t)])
P3 = sp.Matrix([
q4t + l1 * sin(q1t) + l2 * sin(q2t) + a3 * sin(q3t),
+l1 * cos(q1t) + l2 * cos(q2t) + a3 * cos(q3t)
])
P1dt = P1.diff(t)
P2dt = P2.diff(t)
P3dt = P3.diff(t)
# Kinetische Energie vom Schlitten
T0 = 0.5 * m0 * q4dt**2
# Kinetische Energie vom Pendel1
T1 = 0.5 * m1 * (P1dt.T * P1dt)[0] + 0.5 * J1 * q1dt**2
# Kinetische Energie vom Pendel2
T2 = 0.5 * m2 * (P2dt.T * P2dt)[0] + 0.5 * J2 * q2dt**2
# Kinetische Energie vom Pendel3
T3 = 0.5 * m3 * (P3dt.T * P3dt)[0] + 0.5 * J3 * q3dt**2
# Gesamt Kinetische Energie
T = T0 + T1 + T2 + T3
# Gesamt Potentielle Energie
U = g * (m1 * P1[1] + m2 * P2[1] + m3 * P3[1])
# Geschwindigkeitsabhängige (viskose) Reibungen
R = 0.5 * d1 * q1dt**2 + 0.5 * d2 * (q2dt - q1dt)**2 + 0.5 * d3 * (q3dt -
q2dt)**2
#Lagrange-Funktion
L = T - U
L = L.expand()
L = sp.trigsimp(L)
# --- Lagrange-Gleichungen ---
# Hilfsterme:
L_d_q1 = L.diff(q1t)
L_d_q2 = L.diff(q2t)
L_d_q3 = L.diff(q3t)
L_d_q4 = L.diff(q4t)
L_d_q1dt = L.diff(q1dt)
L_d_q2dt = L.diff(q2dt)
L_d_q3dt = L.diff(q3dt)
L_d_q4dt = L.diff(q4dt)
DL_d_q1dt = L_d_q1dt.diff(t)
DL_d_q2dt = L_d_q2dt.diff(t)
DL_d_q3dt = L_d_q3dt.diff(t)
DL_d_q4dt = L_d_q4dt.diff(t)
R_d_q1dt = R.diff(q1dt)
R_d_q2dt = R.diff(q2dt)
R_d_q3dt = R.diff(q3dt)
R_d_q4dt = R.diff(q4dt)
Eq1 = DL_d_q1dt - L_d_q1 + R_d_q1dt
Eq2 = DL_d_q2dt - L_d_q2 + R_d_q2dt
Eq3 = DL_d_q3dt - L_d_q3 + R_d_q3dt
Eq4 = DL_d_q4dt - L_d_q4 + R_d_q4dt - F
Eq = sp.Matrix([Eq1, Eq2, Eq3, Eq4])
# Symbole für Zeitfunktionen einsetzen (für bessere Übersicht)
# Symbole anlegen
q_symbs = sp.symbols(
"q1, q2, q3, q4, q1d, q2d, q3d, q4d, q1dd, q2dd, q3dd, q4dd")
q1, q2, q3, q4, q1d, q2d, q3d, q4d, q1dd, q2dd, q3dd, q4dd = q_symbs
# Substitution festlegen
# Auchtung: Reihenfolge ist wichtig:
# Mit höchsten Ableitungen beginnen, weil sonst die Funktionen innerhalb dieser
# Ableitungen ersetzt werden
subs_list = [(q1ddt, q1dd), (q2ddt, q2dd), (q3ddt, q3dd), (q4ddt, q4dd),
(q1dt, q1d), (q2dt, q2d), (q3dt, q3d), (q4dt, q4d), (q1t, q1),
(q2t, q2), (q3t, q3), (q4t, q4)]
#Substitution durchführen
Eq = Eq.subs(subs_list)
qq = sp.Matrix([q1, q2, q3, q4])
# Vektor der Beschleunigungen
acc = sp.Matrix([q1dd, q2dd, q3dd, q4dd])
# Massenmatrix
MM = Eq.jacobian(acc)
acc0 = list(zip(acc, [0, 0, 0, 0]))
rest = Eq.subs(
acc0) # Alles was übrig bleibt, wenn Beschleunigungen 0 sind
rest.simplify()
a = sp.Symbol('a') # neuer virtueller Eingang
MM3x3 = MM[:-1, :-1]
# Inverse der Massenmatrix bilden (Abkürzung einführen)
MM3x3det = MM3x3.det()
D = sp.Symbol("D")
M3x3inv = MM3x3.adjugate() / D
phi_dd_show = -M3x3inv * (a * MM[:-1, -1] + rest[:-1, 0])
phi_dd = phi_dd_show.subs(D, MM3x3det)
dot_state = sp.Matrix([q1d, q2d, q3d, q4d] + phi_dd[:] + [a])
params_values = [(m0, 3.34), (m1, 0.8512), (m2, 0.8973), (m3, 0.5519),
(J1, 0.01980194), (J2, 0.02105375), (J3, 0.01818537),
(l1, 0.32), (l2, 0.419), (l3, 0.485), (a1, 0.20001517),
(a2, 0.26890449), (a3, 0.21666087), (g, 9.81),
(d1, 0.00715294), (d2, 1.9497e-06), (d3, 0.00164642)]
phi_dd = phi_dd.subs(dict(params_values))
dot_state = sp.Matrix([q1d, q2d, q3d, q4d] + phi_dd[:] + [a])
qdd_part_lin_num = list(dot_state)
q1, q2, q3, q4, q1d, q2d, q3d, q4d = q_symbs[:-4]
q1dd_expr, q2dd_expr, q3dd_expr, q4dd_expr = qdd_part_lin_num[-4:]
q1dd_fnc = sp.lambdify([q1, q2, q3, q4, q1d, q2d, q3d, q4d, a], q1dd_expr,
'numpy')
q2dd_fnc = sp.lambdify([q1, q2, q3, q4, q1d, q2d, q3d, q4d, a], q2dd_expr,
'numpy')
q3dd_fnc = sp.lambdify([q1, q2, q3, q4, q1d, q2d, q3d, q4d, a], q3dd_expr,
'numpy')
q4dd_fnc = sp.lambdify([q1, q2, q3, q4, q1d, q2d, q3d, q4d, a], q4dd_expr,
'numpy')
def state_eq_lagrange(xx, aa):
q4, q1, q2, q3, q4d, q1d, q2d, q3d = xx
a, = aa
qdd1 = q1dd_fnc(q1, q2, q3, q4, q1d, q2d, q3d, q4d, a)
qdd2 = q2dd_fnc(q1, q2, q3, q4, q1d, q2d, q3d, q4d, a)
qdd3 = q3dd_fnc(q1, q2, q3, q4, q1d, q2d, q3d, q4d, a)
return np.array([q4d, q1d, q2d, q3d, a, qdd1, qdd2, qdd3])
return state_eq_lagrange
from matplotlib import pyplot
from scipy import stats
from sympy import Function, Symbol, S, oo, I, sin, Heaviside, Piecewise, Interval, plot, log
%matplotlib inline
# %%
x = Symbol('x')
f = Function('f')
g = Function('g')(x)
f
f(x)
g
f(x).diff(x)
g.diff(x)
# %%
class h(Function):
@classmethod
def eval(cls, x):
if x.is_Number:
if x is S.Zero:
return S.One
elif x is S.Infinity:
return S.Zero
def _eval_is_real(self):
import sympy
from sympy import Function, dsolve, Symbol
# symbols
t = Symbol('t', positive=True)
m = Symbol('m', positive=True)
k = Symbol('k', positive=True)
F0 = Symbol('F0', positive=True)
wf = Symbol('\omega_f', positive=True)
# unknown function
u = Function('u')(t)
# solving ODE
F = F0 * sympy.sin(wf * t)
sol = dsolve(m * u.diff(t, t) + k * u - F)
# sol.lhs ==> u(t)
# sol.rhs ==> solution
print(sol.rhs)
wn = sympy.Symbol('\omega_n')
expr = sol.rhs.subs(sympy.sqrt(k / m), wn)
print(expr)
@author: Topher
"""
from __future__ import division
import sympy as sp
from sympy import sin,cos,Function
t = sp.Symbol('t')
params = sp.symbols('M , G , J , J_ball , R')
M , G , J , J_ball , R = params
# ball position r
r_t = Function('r')(t)
d_r_t = r_t.diff(t)
dd_r_t = r_t.diff(t,2)
# beam angle theta
theta_t = Function('theta')(t)
d_theta_t = theta_t.diff(t)
dd_theta_t = theta_t.diff(t,2)
# torque of the beam
tau = Function('tau')
# kinetic energy
T = ((M + J_ball/R**2)*d_r_t**2 + (J + M*r_t**2 + J_ball)*d_theta_t**2)/2
# potential energy
V = M*G*r_t*sin(theta_t)
# lagrange function
