| 123456789101112131415161718192021222324252627282930313233343536 |
- import sympy.physics.mechanics as _me
- import sympy as _sm
- import math as m
- import numpy as _np
- q1, q2 = _me.dynamicsymbols('q1 q2')
- q1_d, q2_d = _me.dynamicsymbols('q1_ q2_', 1)
- q1_dd, q2_dd = _me.dynamicsymbols('q1_ q2_', 2)
- l, m, g = _sm.symbols('l m g', real=True)
- frame_n = _me.ReferenceFrame('n')
- point_pn = _me.Point('pn')
- point_pn.set_vel(frame_n, 0)
- theta1 = _sm.atan(q2/q1)
- frame_a = _me.ReferenceFrame('a')
- frame_a.orient(frame_n, 'Axis', [theta1, frame_n.z])
- particle_p = _me.Particle('p', _me.Point('p_pt'), _sm.Symbol('m'))
- particle_p.point.set_pos(point_pn, q1*frame_n.x+q2*frame_n.y)
- particle_p.mass = m
- particle_p.point.set_vel(frame_n, (point_pn.pos_from(particle_p.point)).dt(frame_n))
- f_v = _me.dot((particle_p.point.vel(frame_n)).express(frame_a), frame_a.x)
- force_p = particle_p.mass*(g*frame_n.x)
- dependent = _sm.Matrix([[0]])
- dependent[0] = f_v
- velocity_constraints = [i for i in dependent]
- u_q1_d = _me.dynamicsymbols('u_q1_d')
- u_q2_d = _me.dynamicsymbols('u_q2_d')
- kd_eqs = [q1_d-u_q1_d, q2_d-u_q2_d]
- forceList = [(particle_p.point,particle_p.mass*(g*frame_n.x))]
- kane = _me.KanesMethod(frame_n, q_ind=[q1,q2], u_ind=[u_q2_d], u_dependent=[u_q1_d], kd_eqs = kd_eqs, velocity_constraints = velocity_constraints)
- fr, frstar = kane.kanes_equations([particle_p], forceList)
- zero = fr+frstar
- f_c = point_pn.pos_from(particle_p.point).magnitude()-l
- config = _sm.Matrix([[0]])
- config[0] = f_c
- zero = zero.row_insert(zero.shape[0], _sm.Matrix([[0]]))
- zero[zero.shape[0]-1] = config[0]
|
