def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
A = ReferenceFrame('A')
assert CoordinateSym('Ax', A, 0) == A[0]
assert CoordinateSym('Ax', A, 1) == A[1]
assert CoordinateSym('Ax', A, 2) == A[2]
raises(ValueError, lambda: CoordinateSym('Ax', A, 3))
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
assert isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym)
assert A.variable_map(A) == {A[0]: A[0], A[1]: A[1], A[2]: A[2]}
assert A[0].frame == A
B = A.orientnew('B', 'Axis', [q, A.z])
assert B.variable_map(A) == {
B[2]: A[2],
B[1]: -A[0] * sin(q) + A[1] * cos(q),
B[0]: A[0] * cos(q) + A[1] * sin(q)
}
assert A.variable_map(B) == {
A[0]: B[0] * cos(q) - B[1] * sin(q),
A[1]: B[0] * sin(q) + B[1] * cos(q),
A[2]: B[2]
}
assert time_derivative(B[0], A) == -A[0] * sin(q) * qd + A[1] * cos(q) * qd
assert time_derivative(B[1], A) == -A[0] * cos(q) * qd - A[1] * sin(q) * qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0] * cos(q) + A[1] * sin(q)
assert express(B[1], A, variables=True) == -A[0] * sin(q) + A[1] * cos(q)
assert express(B[2], A, variables=True) == A[2]
assert time_derivative(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == A[1] * qd * A.x - A[0] * qd * A.y
assert time_derivative(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == -B[1] * qd * B.x + B[0] * qd * B.y
assert express(B[0]*B[1]*B[2], A, variables=True) == \
A[2]*(-A[0]*sin(q) + A[1]*cos(q))*(A[0]*cos(q) + A[1]*sin(q))
assert (time_derivative(B[0] * B[1] * B[2], A) -
(A[2] * (-A[0]**2 * cos(2 * q) - 2 * A[0] * A[1] * sin(2 * q) +
A[1]**2 * cos(2 * q)) * qd)).trigsimp() == 0
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == \
(B[0]*cos(q) - B[1]*sin(q))*A.x + (B[0]*sin(q) + \
B[1]*cos(q))*A.y + B[2]*A.z
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A, variables=True) == \
A[0]*A.x + A[1]*A.y + A[2]*A.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == \
(A[0]*cos(q) + A[1]*sin(q))*B.x + \
(-A[0]*sin(q) + A[1]*cos(q))*B.y + A[2]*B.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B, variables=True) == \
B[0]*B.x + B[1]*B.y + B[2]*B.z
N = B.orientnew('N', 'Axis', [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew('C', 'Axis', [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert mapping[A[0]] == 2*C[0]*cos(q)/3 + C[0]/3 - 2*C[1]*sin(q + pi/6)/3 +\
C[1]/3 - 2*C[2]*cos(q + pi/3)/3 + C[2]/3
assert mapping[A[1]] == -2*C[0]*cos(q + pi/3)/3 + \
C[0]/3 + 2*C[1]*cos(q)/3 + C[1]/3 - 2*C[2]*sin(q + pi/6)/3 + C[2]/3
assert mapping[A[2]] == -2*C[0]*sin(q + pi/6)/3 + C[0]/3 - \
2*C[1]*cos(q + pi/3)/3 + C[1]/3 + 2*C[2]*cos(q)/3 + C[2]/3
def dt(self, otherframe):
"""
Returns a Vector which is the time derivative of
the self Vector, taken in frame otherframe.
Calls the global time_derivative method
Parameters
==========
otherframe : ReferenceFrame
The frame to calculate the time derivative in
"""
from sympy.physics.vector import time_derivative
return time_derivative(self, otherframe)
def dt(self, otherframe):
"""
Returns a Vector which is the time derivative of
the self Vector, taken in frame otherframe.
Calls the global time_derivative method
Parameters
==========
otherframe : ReferenceFrame
The frame to calculate the time derivative in
"""
from sympy.physics.vector import time_derivative
return time_derivative(self, otherframe)
def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
A = ReferenceFrame('A')
assert CoordinateSym('Ax', A, 0) == A[0]
assert CoordinateSym('Ax', A, 1) == A[1]
assert CoordinateSym('Ax', A, 2) == A[2]
raises(ValueError, lambda: CoordinateSym('Ax', A, 3))
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
assert isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym) and \
isinstance(A[0], CoordinateSym)
assert A.variable_map(A) == {A[0]:A[0], A[1]:A[1], A[2]:A[2]}
assert A[0].frame == A
B = A.orientnew('B', 'Axis', [q, A.z])
assert B.variable_map(A) == {B[2]: A[2], B[1]: -A[0]*sin(q) + A[1]*cos(q),
B[0]: A[0]*cos(q) + A[1]*sin(q)}
assert A.variable_map(B) == {A[0]: B[0]*cos(q) - B[1]*sin(q),
A[1]: B[0]*sin(q) + B[1]*cos(q), A[2]: B[2]}
assert time_derivative(B[0], A) == -A[0]*sin(q)*qd + A[1]*cos(q)*qd
assert time_derivative(B[1], A) == -A[0]*cos(q)*qd - A[1]*sin(q)*qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0]*cos(q) + A[1]*sin(q)
assert express(B[1], A, variables=True) == -A[0]*sin(q) + A[1]*cos(q)
assert express(B[2], A, variables=True) == A[2]
assert time_derivative(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == A[1]*qd*A.x - A[0]*qd*A.y
assert time_derivative(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == - B[1]*qd*B.x + B[0]*qd*B.y
assert express(B[0]*B[1]*B[2], A, variables=True) == \
A[2]*(-A[0]*sin(q) + A[1]*cos(q))*(A[0]*cos(q) + A[1]*sin(q))
assert (time_derivative(B[0]*B[1]*B[2], A) -
(A[2]*(-A[0]**2*cos(2*q) -
2*A[0]*A[1]*sin(2*q) +
A[1]**2*cos(2*q))*qd)).trigsimp() == 0
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A) == \
(B[0]*cos(q) - B[1]*sin(q))*A.x + (B[0]*sin(q) + \
B[1]*cos(q))*A.y + B[2]*A.z
assert express(B[0]*B.x + B[1]*B.y + B[2]*B.z, A, variables=True) == \
A[0]*A.x + A[1]*A.y + A[2]*A.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B) == \
(A[0]*cos(q) + A[1]*sin(q))*B.x + \
(-A[0]*sin(q) + A[1]*cos(q))*B.y + A[2]*B.z
assert express(A[0]*A.x + A[1]*A.y + A[2]*A.z, B, variables=True) == \
B[0]*B.x + B[1]*B.y + B[2]*B.z
N = B.orientnew('N', 'Axis', [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew('C', 'Axis', [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert mapping[A[0]] == 2*C[0]*cos(q)/3 + C[0]/3 - 2*C[1]*sin(q + pi/6)/3 +\
C[1]/3 - 2*C[2]*cos(q + pi/3)/3 + C[2]/3
assert mapping[A[1]] == -2*C[0]*cos(q + pi/3)/3 + \
C[0]/3 + 2*C[1]*cos(q)/3 + C[1]/3 - 2*C[2]*sin(q + pi/6)/3 + C[2]/3
assert mapping[A[2]] == -2*C[0]*sin(q + pi/6)/3 + C[0]/3 - \
2*C[1]*cos(q + pi/3)/3 + C[1]/3 + 2*C[2]*cos(q)/3 + C[2]/3
def get_torque_from_L(L,q,qd):
round_L_round_q = sympy.zeros(len(q),1);
i = 0;
for q_i in q:
round_L_round_q_i = [];
round_L_round_q_i = sympy.simplify(sympy.diff(L,q_i));
round_L_round_q[i] = sympy.Matrix(round_L_round_q_i);
i+=1;
d_dt_round_L_round_qd = sympy.zeros(len(qd),1);
i = 0;
for qd_i in qd:
round_L_round_qd_i = [];
d_dt_round_L_round_qd_i = [];
round_L_round_qd_i = sympy.diff(L,qd_i);
d_dt_round_L_round_qd_i = sympy.simplify(time_derivative(round_L_round_qd_i,N));
d_dt_round_L_round_qd[i] = sympy.Matrix(d_dt_round_L_round_qd_i);
i+=1;
tau = d_dt_round_L_round_qd - round_L_round_q
return tau
B1.orient(B0, 'Axis', [THETA_1, B0.z])
B2 = ReferenceFrame('B2')
# Referencial móvel: theta_2 em relação a B1.z
B2.orient(B1, 'Axis', [THETA_2, B1.z])
# Vetores Posição entre os Pontos
# Vetor Nulo
B0_R_OA = Vector(0)
# Vetor que liga os pontos A e B expresso no referencial móvel B1
B1_R_AB = L_1 * B1.x
# Vetor que liga os pontos B e C expresso no referencial móvel B2
B2_R_BC = L_2 * B2.x
# Cinemática do ponto A em relação ao referencial B0
R_A = B0_R_OA
V_A = time_derivative(R_A, B0)
A_A = time_derivative(V_A, B0)
# Cinemática do ponto B em relação ao referencial B0
R_B = R_A + B1_R_AB.express(B0)
V_B = time_derivative(R_B, B0)
A_B = time_derivative(V_B, B0)
# Cinemática do ponto C em relação ao referencial B0
R_C = R_B.express(B0) + B2_R_BC.express(B0)
V_C = (time_derivative(R_C, B0))
A_C = (time_derivative(V_C, B0))
# Simplificação dos Resultados
R_A = (R_A.to_matrix(B0)).applyfunc(trigsimp)
V_A = (V_A.to_matrix(B0)).applyfunc(trigsimp)
# symbolic form
# alpha = symbols('a')
A1, A2 = symbols('A1 A2')
B1, B2 = symbols('B1 B2')
C2 = symbols('C2')
# Define interface offset (alpha)
alpha = 0.25 + A1 * t * sin(B1 * N[0]) + A2 * sin(B2 * N[0] + C2 * t)
# Define the solution equation (eta)
xi = (N[1] - alpha) / sqrt(2 * kappa)
eta_sol = (1 - tanh(xi)) / 2
eq_sol = simplify(
time_derivative(eta_sol, N) + 4 * eta_sol * (eta_sol - 1) *
(eta_sol - 0.5) - divergence(kappa * gradient(eta_sol, N), N))
# substitute coefficient values
parameters = ((kappa, params['kappa']), (A1, 0.0075), (B1, 8.0 * pi),
(A2, 0.03), (B2, 22.0 * pi), (C2, 0.0625 * pi))
subs = [sub.subs(parameters) for sub in (eq_sol, eta_sol)]
# generate FiPy lambda functions
from sympy.utilities.lambdify import lambdify
(eq_fp, eta_fp) = [
lambdify((N[0], N[1], t), sub, modules=fp.numerix) for sub in subs
[theta_1, B0.y]) # Referencial móvel: theta_1 em relação a B0.y
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis',
[theta_2, B1.z]) # Referencial móvel: theta_2 em relação a B1.z
B3 = ReferenceFrame('B3')
B3.orient(B2, 'Axis',
[theta_3, B2.z]) # Referencial móvel: theta_3 em relação a B2.z
# Vetores Posição entre os Pontos
B0_r_OA = Vector(0) # Vetor Nulo
B2_r_AB = l_1 * B2.x # Vetor que liga os pontos A e B expresso no referencial móvel B2
B3_r_BC = l_2 * B3.x # Vetor que liga os pontos B e C expresso no referencial móvel B3
# Cinemática do ponto A em relação ao referencial B0
r_A = B0_r_OA
v_A = time_derivative(r_A, B0)
a_A = time_derivative(v_A, B0)
# Cinemática do ponto B em relação ao referencial B0
r_B = r_A + B2_r_AB.express(B0)
v_B = time_derivative(r_B, B0)
a_B = time_derivative(v_B, B0)
# Cinemática do ponto C em relação ao referencial B0
r_C = B3_r_BC.express(B0)
v_C = (time_derivative(r_C, B0))
a_C = (time_derivative(v_C, B0))
# Simplifcação dos Resultados
r_A = (r_A.to_matrix(B0)).applyfunc(trigsimp)
v_A = (v_A.to_matrix(B0)).applyfunc(trigsimp)
def test_time_derivative():
#The use of time_derivative for calculations pertaining to scalar
#fields has been tested in test_coordinate_vars in test_essential.py
A = ReferenceFrame('A')
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
d = A.x | A.x
assert time_derivative(d, B) == (-qd) * (A.y | A.x) + \
(-qd) * (A.x | A.y)
d1 = A.x | B.y
assert time_derivative(d1, A) == -qd * (A.x | B.x)
assert time_derivative(d1, B) == -qd * (A.y | B.y)
d2 = A.x | B.x
assert time_derivative(d2, A) == qd * (A.x | B.y)
assert time_derivative(d2, B) == -qd * (A.y | B.x)
d3 = A.x | B.z
assert time_derivative(d3, A) == 0
assert time_derivative(d3, B) == -qd * (A.y | B.z)
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2)
C = B.orientnew('C', 'Axis', [q4, B.x])
v1 = q1 * A.z
v2 = q2 * A.x + q3 * B.y
v3 = q1 * A.x + q2 * A.y + q3 * A.z
assert time_derivative(B.x, C) == 0
assert time_derivative(B.y, C) == -q4d * B.z
assert time_derivative(B.z, C) == q4d * B.y
assert time_derivative(v1, B) == q1d * A.z
assert time_derivative(v1, C) == - q1*sin(q)*q4d*A.x + \
q1*cos(q)*q4d*A.y + q1d*A.z
assert time_derivative(v2, A) == q2d * A.x - q3 * qd * B.x + q3d * B.y
assert time_derivative(v2, C) == q2d*A.x - q2*qd*A.y + \
q2*sin(q)*q4d*A.z + q3d*B.y - q3*q4d*B.z
assert time_derivative(v3, B) == (q2*qd + q1d)*A.x + \
(-q1*qd + q2d)*A.y + q3d*A.z
assert time_derivative(d, C) == - qd*(A.y|A.x) + \
sin(q)*q4d*(A.z|A.x) - qd*(A.x|A.y) + sin(q)*q4d*(A.x|A.z)
def test_time_derivative():
#The use of time_derivative for calculations pertaining to scalar
#fields has been tested in test_coordinate_vars in test_essential.py
A = ReferenceFrame('A')
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
d = A.x | A.x
assert time_derivative(d, B) == (-qd) * (A.y | A.x) + \
(-qd) * (A.x | A.y)
d1 = A.x | B.y
assert time_derivative(d1, A) == - qd*(A.x|B.x)
assert time_derivative(d1, B) == - qd*(A.y|B.y)
d2 = A.x | B.x
assert time_derivative(d2, A) == qd*(A.x|B.y)
assert time_derivative(d2, B) == - qd*(A.y|B.x)
d3 = A.x | B.z
assert time_derivative(d3, A) == 0
assert time_derivative(d3, B) == - qd*(A.y|B.z)
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2)
C = B.orientnew('C', 'Axis', [q4, B.x])
v1 = q1 * A.z
v2 = q2*A.x + q3*B.y
v3 = q1*A.x + q2*A.y + q3*A.z
assert time_derivative(B.x, C) == 0
assert time_derivative(B.y, C) == - q4d*B.z
assert time_derivative(B.z, C) == q4d*B.y
assert time_derivative(v1, B) == q1d*A.z
assert time_derivative(v1, C) == - q1*sin(q)*q4d*A.x + \
q1*cos(q)*q4d*A.y + q1d*A.z
assert time_derivative(v2, A) == q2d*A.x - q3*qd*B.x + q3d*B.y
assert time_derivative(v2, C) == q2d*A.x - q2*qd*A.y + \
q2*sin(q)*q4d*A.z + q3d*B.y - q3*q4d*B.z
assert time_derivative(v3, B) == (q2*qd + q1d)*A.x + \
(-q1*qd + q2d)*A.y + q3d*A.z
assert time_derivative(d, C) == - qd*(A.y|A.x) + \
sin(q)*q4d*(A.z|A.x) - qd*(A.x|A.y) + sin(q)*q4d*(A.x|A.z)
def test_coordinate_vars():
"""Tests the coordinate variables functionality"""
A = ReferenceFrame("A")
assert CoordinateSym("Ax", A, 0) == A[0]
assert CoordinateSym("Ax", A, 1) == A[1]
assert CoordinateSym("Ax", A, 2) == A[2]
raises(ValueError, lambda: CoordinateSym("Ax", A, 3))
q = dynamicsymbols("q")
qd = dynamicsymbols("q", 1)
assert (isinstance(A[0], CoordinateSym)
and isinstance(A[0], CoordinateSym)
and isinstance(A[0], CoordinateSym))
assert A.variable_map(A) == {A[0]: A[0], A[1]: A[1], A[2]: A[2]}
assert A[0].frame == A
B = A.orientnew("B", "Axis", [q, A.z])
assert B.variable_map(A) == {
B[2]: A[2],
B[1]: -A[0] * sin(q) + A[1] * cos(q),
B[0]: A[0] * cos(q) + A[1] * sin(q),
}
assert A.variable_map(B) == {
A[0]: B[0] * cos(q) - B[1] * sin(q),
A[1]: B[0] * sin(q) + B[1] * cos(q),
A[2]: B[2],
}
assert time_derivative(B[0], A) == -A[0] * sin(q) * qd + A[1] * cos(q) * qd
assert time_derivative(B[1], A) == -A[0] * cos(q) * qd - A[1] * sin(q) * qd
assert time_derivative(B[2], A) == 0
assert express(B[0], A, variables=True) == A[0] * cos(q) + A[1] * sin(q)
assert express(B[1], A, variables=True) == -A[0] * sin(q) + A[1] * cos(q)
assert express(B[2], A, variables=True) == A[2]
assert (time_derivative(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == A[1] * qd * A.x - A[0] * qd * A.y)
assert (time_derivative(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == -B[1] * qd * B.x + B[0] * qd * B.y)
assert express(B[0] * B[1] * B[2], A, variables=True) == A[2] * (
-A[0] * sin(q) + A[1] * cos(q)) * (A[0] * cos(q) + A[1] * sin(q))
assert (time_derivative(B[0] * B[1] * B[2], A) -
(A[2] * (-A[0]**2 * cos(2 * q) - 2 * A[0] * A[1] * sin(2 * q) +
A[1]**2 * cos(2 * q)) * qd)).trigsimp() == 0
assert (express(B[0] * B.x + B[1] * B.y + B[2] * B.z,
A) == (B[0] * cos(q) - B[1] * sin(q)) * A.x +
(B[0] * sin(q) + B[1] * cos(q)) * A.y + B[2] * A.z)
assert (express(B[0] * B.x + B[1] * B.y + B[2] * B.z, A,
variables=True) == A[0] * A.x + A[1] * A.y + A[2] * A.z)
assert (express(A[0] * A.x + A[1] * A.y + A[2] * A.z,
B) == (A[0] * cos(q) + A[1] * sin(q)) * B.x +
(-A[0] * sin(q) + A[1] * cos(q)) * B.y + A[2] * B.z)
assert (express(A[0] * A.x + A[1] * A.y + A[2] * A.z, B,
variables=True) == B[0] * B.x + B[1] * B.y + B[2] * B.z)
N = B.orientnew("N", "Axis", [-q, B.z])
assert N.variable_map(A) == {N[0]: A[0], N[2]: A[2], N[1]: A[1]}
C = A.orientnew("C", "Axis", [q, A.x + A.y + A.z])
mapping = A.variable_map(C)
assert (mapping[A[0]] == 2 * C[0] * cos(q) / 3 + C[0] / 3 -
2 * C[1] * sin(q + pi / 6) / 3 + C[1] / 3 -
2 * C[2] * cos(q + pi / 3) / 3 + C[2] / 3)
assert (mapping[A[1]] == -2 * C[0] * cos(q + pi / 3) / 3 + C[0] / 3 +
2 * C[1] * cos(q) / 3 + C[1] / 3 - 2 * C[2] * sin(q + pi / 6) / 3 +
C[2] / 3)
assert (mapping[A[2]] == -2 * C[0] * sin(q + pi / 6) / 3 + C[0] / 3 -
2 * C[1] * cos(q + pi / 3) / 3 + C[1] / 3 + 2 * C[2] * cos(q) / 3 +
C[2] / 3)