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))