def CreateArm(self):
armVector = self.counterweightPivot.pos_from(self.projectilePivot)
armLength = armVector.magnitude()
inertiaProduct = self.armMass * armLength * armLength / 12.0
armInertia = inertia(self.armFrame, 0, inertiaProduct, inertiaProduct)
self.arm = RigidBody('A', self.armCenter, self.armFrame, self.armMass, (armInertia, self.armCenter))
self._bodyList.append(self.arm)
def __init__(self, linkage, name, joint, parent):
"""TODO
"""
super(DynamicLink, self).__init__(linkage, name)
self._joint = joint
# Give the joint access to the link.
self._joint._link = self
self._parent = parent
# Create rigid body.
mass = symbols(name + '_mass')
frame = ReferenceFrame(name + '_frame')
self._origin = Point(name + '_origin')
# TODO explain what this method call does.
joint._orient(name, parent.frame, parent.origin, frame, self._origin)
# All expressed in body frame.
masscenter = self._origin.locatenew(
name + '_masscenter',
symbols(name + '_mcx') * frame.x +
symbols(name + '_mcy') * frame.y +
symbols(name + '_mcz') * frame.z)
masscenter.set_vel(frame, 0)
masscenter.v2pt_theory(self._origin, linkage.root.frame, frame)
inertia = functions.inertia(frame, symbols(name + '_ixx'),
symbols(name + '_iyy'),
symbols(name + '_izz'),
symbols(name + '_ixy'),
symbols(name + '_iyz'),
symbols(name + '_izx'))
# TODO allow specification of non-central inertia
self._rigidbody = RigidBody(name + '_rigidbody', masscenter, frame,
mass, (inertia, masscenter))
def __init__(self, linkage, name, joint, parent):
"""TODO
"""
super(DynamicLink, self).__init__(linkage, name)
self._joint = joint
# Give the joint access to the link.
self._joint._link = self
self._parent = parent
# Create rigid body.
mass = symbols(name + '_mass')
frame = ReferenceFrame(name + '_frame')
self._origin = Point(name + '_origin')
# TODO explain what this method call does.
joint._orient(name, parent.frame, parent.origin, frame, self._origin)
# All expressed in body frame.
masscenter = self._origin.locatenew(name + '_masscenter',
symbols(name + '_mcx') * frame.x +
symbols(name + '_mcy') * frame.y +
symbols(name + '_mcz') * frame.z)
masscenter.set_vel(frame, 0)
masscenter.v2pt_theory(self._origin, linkage.root.frame, frame)
inertia = functions.inertia(frame,
symbols(name + '_ixx'),
symbols(name + '_iyy'),
symbols(name + '_izz'),
symbols(name + '_ixy'),
symbols(name + '_iyz'),
symbols(name + '_izx'))
# TODO allow specification of non-central inertia
self._rigidbody = RigidBody(name + '_rigidbody',
masscenter, frame, mass, (inertia, masscenter))
def parallel_axis(self, point):
"""Returns the inertia dyadic of the body with respect to another
point.
Parameters
==========
point : sympy.physics.vector.Point
The point to express the inertia dyadic about.
Returns
=======
inertia : sympy.physics.vector.Dyadic
The inertia dyadic of the rigid body expressed about the provided
point.
"""
# circular import issue
from sympy.physics.mechanics.functions import inertia
a, b, c = self.masscenter.pos_from(point).to_matrix(self.frame)
I = self.mass * inertia(
self.frame,
b**2 + c**2,
c**2 + a**2,
a**2 + b**2,
-a * b,
-b * c,
-a * c,
)
return self.central_inertia + I
def mass(self):
# Define inertia of arm
inertia_product = self.arm_mass * self.arm_length * self.arm_length / 12
armInertia = inertia(self.armFrame, 0, inertia_product, inertia_product)
# Define Rigid Body and Particles
self.arm = RigidBody('A', self.centerPoint, self.armFrame, self.arm_mass, (armInertia, self.centerPoint))
self.counterweight = Particle( 'D', self.counterweightPoint, self.counterweight_mass)
self.projectile = Particle( 'B', self.projectilePoint, self.projectile_mass)
# Pontos e Centros de Massa
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, l_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, r_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, r_2 * B2.x)
CM_2.v2pt_theory(O, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_zz)
E_1 = RigidBody('Elo_1', CM_1, B1, m_1, (I_1, CM_1)) # Elo 1
I_2 = inertia(B1, 0, 0, I_1_zz)
E_2 = RigidBody('Elo_2', CM_2, B2, m_2, (I_2, CM_2)) # Elo 2
# Energia Potencial
P_1 = -m_1 * g * B0.z
r_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = r_1_CM.dot(P_1)
P_2 = -m_2 * g * B0.z
r_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = r_2_CM.dot(P_2)
# Forças/Momentos Generalizados
FL = [(B1, tau_1 * B0.z), (B2, tau_2 * B0.z)]
# Método de Lagrange
m, g = symbols('m g')
I_xx, I_yy, I_zz = symbols('I_{xx}, I_{yy}, I_{zz}')
# Referenciais
B0 = ReferenceFrame('B0') # Referencial Inercial
B1 = B0.orientnew('B1', 'Axis', [theta, B0.z]) # Referencial móvel: theta_1 em relação a B0.z
# Pontos e Centros de Massa
O = Point('O') # Origem
O.set_vel(B0, 0)
CM = Point('CM') # Centro de Massa
CM.set_pos(O, r * B1.x)
CM.v2pt_theory(O, B0, B1)
# Corpos Rígidos
I = inertia(B1, I_xx, I_yy, I_zz) # Tensor de Inércia
E = RigidBody('E', CM, B1, m, (I, O)) # Elo 1
# Energia Potencial
P = - m * g * B0.y
r_CM = (r * B1.x).express(B0)
E.potential_energy = r_CM.dot(P)
# Forças/Momentos Generalizados
FL = [(B1, tau * B0.z)]
# Método de Lagrange
L = Lagrangian(B0, E).simplify()
LM = LagrangesMethod(L, [theta], frame=B0, forcelist = FL)
L_eq = LM.form_lagranges_equations()
pprint(L_eq)
rhs = LM.rhs()
def solve_dynamics(self):
"""Calcula a solução da dinâmica do manipulador e já coloca em
formato pronto para ser calculado """
# Variáveis Simbólicas do problema
theta_1, theta_2 = dynamicsymbols('theta_1 theta_2')
dtheta_1, dtheta_2 = dynamicsymbols('theta_1 theta_2', 1)
tau_1, tau_2 = symbols('tau_1 tau_2')
symb_dynamics = (theta_1, theta_2, dtheta_1, dtheta_2, tau_1, tau_2)
l_1, l_2 = symbols('l_1 l_2', positive = True)
r_1, r_2 = symbols('r_1 r_2', positive = True)
m_1, m_2, g = symbols('m_1 m_2 g')
I_zz_1, I_zz_2 = symbols('I_{1zz} I_{2zz}')
symb_params = (l_1, l_2, r_1, r_2, I_zz_1, I_zz_2, m_1, m_2, g)
# Nome do arquivo salvo com a equação do movimento
here = os.path.dirname(os.path.realpath(__file__))
file_name = (here + "\\models\\scara_dynamics_" +
"".join(str(e) + "_" for e in self.num_params) + ".data")
try:
rhs = dill.load(open(file_name, "rb"))
self.rhs_lambdified = rhs
return
except:
print("Modelo dinâmico ainda não foi calculado para esses parâmetros. Aguarde...")
initial_time = time.time()
# Referenciais
B0 = ReferenceFrame('B0') # Referencial Parado
B1 = ReferenceFrame('B1')
B1.orient(B0, 'Axis', [theta_1, B0.z]) # Referencial móvel: theta_1 em relação a B0.z
B2 = ReferenceFrame('B2')
B2.orient(B1, 'Axis', [theta_2, B1.z]) # Referencial móvel: theta_2 em relação a B1.z
# Pontos e Centros de Massa
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, l_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, r_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, r_2 * B2.x)
CM_2.v2pt_theory(A, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_zz_1)
E_1 = RigidBody('Elo_1', CM_1, B1, m_1, (I_1, CM_1)) # Elo 1
I_2 = inertia(B2, 0, 0, I_zz_2)
E_2 = RigidBody('Elo_2', CM_2, B2, m_2, (I_2, CM_2)) # Elo 2)
# Energia Potencial
P_1 = -m_1 * g * B0.y
r_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = r_1_CM.dot(P_1)
P_2 = -m_2 * g * B0.y
r_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = r_2_CM.dot(P_2)
# Forças/Momentos Generalizados
FL = [(B1, tau_1 * B1.z), (B2, tau_2 * B2.z)]
# Método de Lagrange
L = Lagrangian(B0, E_1, E_2).simplify()
LM = LagrangesMethod(L, [theta_1, theta_2], frame=B0, forcelist=FL)
motion_eq = LM.form_lagranges_equations()
rhs = LM.rhs()
# Salva as Equação em formato fácil de se obter Solução já com os Parâmetros
dummys = [Dummy() for i in symb_dynamics]
rhs = msubs(rhs, dict(zip(symb_params, self.num_params)))
dummydict = dict(zip(symb_dynamics, dummys))
rhs = msubs(rhs, dummydict)
# Lambdify as equações
for f in rhs:
self.rhs_lambdified.append(lambdify(dummys, f))
# Salva as equações lambidificadas e já com as constantes subtituídas no disco
os.makedirs(os.path.dirname(file_name), exist_ok=True)
dill.dump(self.rhs_lambdified, open(file_name, "wb"))
print("Modelo calculado e salvo em " + file_name + ".Tempo: " +
str(time.time() - initial_time))
# Pontos e Centros de Massa
O = Point('O')
O.set_vel(B0, 0)
A = Point('A')
A.set_pos(O, L_1 * B1.x)
A.v2pt_theory(O, B0, B1)
CM_1 = Point('CM_1')
CM_1.set_pos(O, R_1 * B1.x)
CM_1.v2pt_theory(O, B0, B1)
CM_2 = Point('CM_2')
CM_2.set_pos(A, R_2 * B2.x)
CM_2.v2pt_theory(O, B0, B2)
# Corpos Rígidos
I_1 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 1
E_1 = RigidBody('Elo_1', CM_1, B1, M_1, (I_1, CM_1))
I_2 = inertia(B1, 0, 0, I_1_ZZ)
# Elo 2
E_2 = RigidBody('Elo_2', CM_2, B2, M_2, (I_2, CM_2))
# Energia Potencial
P_1 = -M_1 * G * B0.z
R_1_CM = CM_1.pos_from(O).express(B0)
E_1.potential_energy = R_1_CM.dot(P_1)
P_2 = -M_2 * G * B0.z
R_2_CM = CM_2.pos_from(O).express(B0).simplify()
E_2.potential_energy = R_2_CM.dot(P_2)
# Forças/Momentos Generalizados
def solve_dynamics(self):
"""Calcula a solução da dinâmica do manipulador e já coloca em
formato pronto para ser calculado """
# Variáveis Simbólicas do problema
theta = dynamicsymbols('theta')
dtheta = dynamicsymbols('theta', 1)
tau = symbols('tau')
symb_dynamics = (theta, dtheta, tau)
l = symbols('l', positive=True)
r = symbols('r', positive=True)
m, g = symbols('m g')
I_xx, I_yy, I_zz = symbols('I_{xx}, I_{yy}, I_{zz}')
symb_params = (l, r, I_zz, m, g)
# Nome do arquivo salvo com a equação do movimento
here = os.path.dirname(os.path.realpath(__file__))
file_name = (here + "\\models\\rotatingarm_dynamics_" +
"".join(str(e) + "_" for e in self.num_params) + ".data")
# Verifica se a solução já foi calculada antes
try:
rhs = dill.load(open(file_name, "rb"))
self.rhs_lambdified = rhs
return
except:
print(
"Modelo dinâmico ainda não foi calculado para esses parâmetros. Aguarde..."
)
initial_time = time.time()
# Referenciais
B0 = ReferenceFrame('B0') # Referencial Inercial
B1 = B0.orientnew(
'B1', 'Axis',
[theta, B0.z]) # Referencial móvel: theta_1 em relação a B0.z
# Pontos e Centros de Massa
O = Point('O') # Origem
O.set_vel(B0, 0)
CM = Point('CM') # Centro de Massa
CM.set_pos(O, r * B1.x)
CM.v2pt_theory(O, B0, B1)
# Corpos Rígidos
E = RigidBody('E', CM, B1, m, (inertia(B1, I_xx, I_yy, I_zz), O))
# Energia Potencial
P = -m * g * B0.y
r_CM = (r * B1.x).express(B0)
E.potential_energy = r_CM.dot(P)
# Forças/Momentos Generalizados
FL = [(B1, tau * B0.z)]
# Método de Lagrange
L = Lagrangian(B0, E).simplify()
LM = LagrangesMethod(L, [theta], frame=B0, forcelist=FL)
motion_eq = LM.form_lagranges_equations()
rhs = LM.rhs()
# Salva as Equação em formato fácil de se obter Solução já com os Parâmetros
dummys = [Dummy() for i in symb_dynamics]
rhs = msubs(rhs, dict(zip(symb_params, self.num_params)))
dummydict = dict(zip(symb_dynamics, dummys))
rhs = msubs(rhs, dummydict)
# Lambdify as equações
for f in rhs:
self.rhs_lambdified.append(lambdify(dummys, f))
# Salva as equações lambidificadas e já com as constantes subtituídas no disco
os.makedirs(os.path.dirname(file_name), exist_ok=True)
dill.dump(self.rhs_lambdified, open(file_name, "wb"))
print("Modelo calculado e salvo em " + file_name + ".Tempo: " +
str(time.time() - initial_time))
