def test_particle(): m, m2, v1, v2, v3, r, g, h = symbols('m m2 v1 v2 v3 r g h') P = Point('P') P2 = Point('P2') p = Particle('pa', P, m) assert p.mass == m assert p.point == P # Test the mass setter p.mass = m2 assert p.mass == m2 # Test the point setter p.point = P2 assert p.point == P2 # Test the linear momentum function N = ReferenceFrame('N') O = Point('O') P2.set_pos(O, r * N.y) P2.set_vel(N, v1 * N.x) assert p.linear_momentum(N) == m2 * v1 * N.x assert p.angular_momentum(O, N) == -m2 * r *v1 * N.z P2.set_vel(N, v2 * N.y) assert p.linear_momentum(N) == m2 * v2 * N.y assert p.angular_momentum(O, N) == 0 P2.set_vel(N, v3 * N.z) assert p.linear_momentum(N) == m2 * v3 * N.z assert p.angular_momentum(O, N) == m2 * r * v3 * N.x P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z) assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z) p.set_potential_energy(m * g * h) assert p.potential_energy == m * g * h # TODO make the result not be system-dependent assert p.kinetic_energy( N) in [m2*(v1**2 + v2**2 + v3**2)/2, m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2]
def test_particle(): m, m2, v1, v2, v3, r = symbols('m m2 v1 v2 v3 r') P = Point('P') P2 = Point('P2') p = Particle('pa', P, m) assert p.mass == m assert p.point == P # Test the mass setter p.mass = m2 assert p.mass == m2 # Test the point setter p.point = P2 assert p.point == P2 # Test the linear momentum function N = ReferenceFrame('N') O = Point('O') P2.set_pos(O, r * N.y) P2.set_vel(N, v1 * N.x) assert p.linearmomentum(N) == m2 * v1 * N.x assert p.angularmomentum(O, N) == -m2 * r *v1 * N.z P2.set_vel(N, v2 * N.y) assert p.linearmomentum(N) == m2 * v2 * N.y assert p.angularmomentum(O, N) == 0 P2.set_vel(N, v3 * N.z) assert p.linearmomentum(N) == m2 * v3 * N.z assert p.angularmomentum(O, N) == m2 * r * v3 * N.x P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert p.linearmomentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z) assert p.angularmomentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z)
def vizAsCylinder(self, radius, length, axis='z', color='blue', offset=0, format='pydy'): """ """ if format == 'pydy': # pydy cylinder is along y and centered at the middle of the cylinder from pydy.viz.shapes import Cylinder from pydy.viz.visualization_frame import VisualizationFrame if axis == 'y': e = self.frame a = self.frame.y elif axis == 'z': e = self.frame.orientnew('CF_' + self.name, 'Axis', (np.pi / 2, self.frame.x)) a = self.frame.z elif axis == 'x': e = self.frame.orientnew('CF_' + self.name, 'Axis', (np.pi / 2, self.frame.z)) a = self.frame.x shape = Cylinder(radius=radius, length=length, color=color) center = Point('CC_' + self.name) center.set_pos(self.origin, (length / 2 + offset) * a) return VisualizationFrame(e, center, shape) else: raise NotImplementedError()
def second_order_system(): # from sympy.printing.pycode import NumPyPrinter, pycode coordinates = dynamicsymbols('q:1') # Generalized coordinates speeds = dynamicsymbols('u:1') # Generalized speeds # Force applied to the cart cart_thrust = dynamicsymbols('thrust') m = sp.symbols('m:1') # Mass of each bob g, t = sp.symbols('g t') # Gravity and time ref_frame = ReferenceFrame('I') # Inertial reference frame origin = Point('O') # Origin point origin.set_vel(ref_frame, 0) # Origin's velocity is zero P0 = Point('P0') # Hinge point of top link # Set the position of P0 P0.set_pos(origin, coordinates[0] * ref_frame.x) P0.set_vel(ref_frame, speeds[0] * ref_frame.x) # Set the velocity of P0 Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0 # List to hold the n + 1 frames frames = [ref_frame] points = [P0] # List to hold the n + 1 points # List to hold the n + 1 particles particles = [Pa0] # List to hold the n + 1 applied forces, including the input force, f applied_forces = [(P0, cart_thrust * ref_frame.x - m[0] * g * ref_frame.y)] # List to hold kinematic ODE's kindiffs = [coordinates[0].diff(t) - speeds[0]] # Initialize the object kane = KanesMethod(ref_frame, q_ind=coordinates, u_ind=speeds, kd_eqs=kindiffs) # Generate EoM's fr + frstar = 0 fr, frstar = kane.kanes_equations(particles, applied_forces) state = coordinates + speeds gain = [cart_thrust] kindiff_dict = kane.kindiffdict() M = kane.mass_matrix_full.subs(kindiff_dict) F = kane.forcing_full.subs(kindiff_dict) static_parameters = [g, m[0]] transfer = M.inv() * F return DynamicSystem(state, gain, static_parameters, transfer)
def test_rigidbody2(): M, v, r, omega = dynamicsymbols('M v r omega') N = ReferenceFrame('N') b = ReferenceFrame('b') b.set_ang_vel(N, omega * b.x) P = Point('P') I = outer (b.x, b.x) Inertia_tuple = (I, P) B = RigidBody('B', P, b, M, Inertia_tuple) P.set_vel(N, v * b.x) assert B.angularmomentum(P, N) == omega * b.x O = Point('O') O.set_vel(N, v * b.x) P.set_pos(O, r * b.y) assert B.angularmomentum(O, N) == omega * b.x - M*v*r*b.z
def test_rigidbody2(): M, v, r, omega, g, h = dynamicsymbols('M v r omega g h') N = ReferenceFrame('N') b = ReferenceFrame('b') b.set_ang_vel(N, omega * b.x) P = Point('P') I = outer(b.x, b.x) Inertia_tuple = (I, P) B = RigidBody('B', P, b, M, Inertia_tuple) P.set_vel(N, v * b.x) assert B.angular_momentum(P, N) == omega * b.x O = Point('O') O.set_vel(N, v * b.x) P.set_pos(O, r * b.y) assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z B.potential_energy = M * g * h assert B.potential_energy == M * g * h assert expand(2 * B.kinetic_energy(N)) == omega**2 + M * v**2
def test_rigidbody2(): M, v, r, omega, g, h = dynamicsymbols('M v r omega g h') N = ReferenceFrame('N') b = ReferenceFrame('b') b.set_ang_vel(N, omega * b.x) P = Point('P') I = outer(b.x, b.x) Inertia_tuple = (I, P) B = RigidBody('B', P, b, M, Inertia_tuple) P.set_vel(N, v * b.x) assert B.angular_momentum(P, N) == omega * b.x O = Point('O') O.set_vel(N, v * b.x) P.set_pos(O, r * b.y) assert B.angular_momentum(O, N) == omega * b.x - M*v*r*b.z B.set_potential_energy(M * g * h) assert B.potential_energy == M * g * h assert B.kinetic_energy(N) == (omega**2 + M * v**2) / 2
def test_center_of_mass(): a = ReferenceFrame('a') m = symbols('m', real=True) p1 = Particle('p1', Point('p1_pt'), S(1)) p2 = Particle('p2', Point('p2_pt'), S(2)) p3 = Particle('p3', Point('p3_pt'), S(3)) p4 = Particle('p4', Point('p4_pt'), m) b_f = ReferenceFrame('b_f') b_cm = Point('b_cm') mb = symbols('mb') b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm)) p2.point.set_pos(p1.point, a.x) p3.point.set_pos(p1.point, a.x + a.y) p4.point.set_pos(p1.point, a.y) b.masscenter.set_pos(p1.point, a.y + a.z) point_o=Point('o') point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b)) expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass(): a = ReferenceFrame('a') m = symbols('m', real=True) p1 = Particle('p1', Point('p1_pt'), S.One) p2 = Particle('p2', Point('p2_pt'), S(2)) p3 = Particle('p3', Point('p3_pt'), S(3)) p4 = Particle('p4', Point('p4_pt'), m) b_f = ReferenceFrame('b_f') b_cm = Point('b_cm') mb = symbols('mb') b = RigidBody('b', b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm)) p2.point.set_pos(p1.point, a.x) p3.point.set_pos(p1.point, a.x + a.y) p4.point.set_pos(p1.point, a.y) b.masscenter.set_pos(p1.point, a.y + a.z) point_o=Point('o') point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b)) expr = 5/(m + mb + 6)*a.x + (m + mb + 3)/(m + mb + 6)*a.y + mb/(m + mb + 6)*a.z assert point_o.pos_from(p1.point)-expr == 0
def test_center_of_mass(): a = ReferenceFrame("a") m = symbols("m", real=True) p1 = Particle("p1", Point("p1_pt"), S.One) p2 = Particle("p2", Point("p2_pt"), S(2)) p3 = Particle("p3", Point("p3_pt"), S(3)) p4 = Particle("p4", Point("p4_pt"), m) b_f = ReferenceFrame("b_f") b_cm = Point("b_cm") mb = symbols("mb") b = RigidBody("b", b_cm, b_f, mb, (outer(b_f.x, b_f.x), b_cm)) p2.point.set_pos(p1.point, a.x) p3.point.set_pos(p1.point, a.x + a.y) p4.point.set_pos(p1.point, a.y) b.masscenter.set_pos(p1.point, a.y + a.z) point_o = Point("o") point_o.set_pos(p1.point, center_of_mass(p1.point, p1, p2, p3, p4, b)) expr = (5 / (m + mb + 6) * a.x + (m + mb + 3) / (m + mb + 6) * a.y + mb / (m + mb + 6) * a.z) assert point_o.pos_from(p1.point) - expr == 0
def test_particle(): m, m2, v1, v2, v3, r, g, h = symbols("m m2 v1 v2 v3 r g h") P = Point("P") P2 = Point("P2") p = Particle("pa", P, m) assert p.__str__() == "pa" assert p.mass == m assert p.point == P # Test the mass setter p.mass = m2 assert p.mass == m2 # Test the point setter p.point = P2 assert p.point == P2 # Test the linear momentum function N = ReferenceFrame("N") O = Point("O") P2.set_pos(O, r * N.y) P2.set_vel(N, v1 * N.x) raises(TypeError, lambda: Particle(P, P, m)) raises(TypeError, lambda: Particle("pa", m, m)) assert p.linear_momentum(N) == m2 * v1 * N.x assert p.angular_momentum(O, N) == -m2 * r * v1 * N.z P2.set_vel(N, v2 * N.y) assert p.linear_momentum(N) == m2 * v2 * N.y assert p.angular_momentum(O, N) == 0 P2.set_vel(N, v3 * N.z) assert p.linear_momentum(N) == m2 * v3 * N.z assert p.angular_momentum(O, N) == m2 * r * v3 * N.x P2.set_vel(N, v1 * N.x + v2 * N.y + v3 * N.z) assert p.linear_momentum(N) == m2 * (v1 * N.x + v2 * N.y + v3 * N.z) assert p.angular_momentum(O, N) == m2 * r * (v3 * N.x - v1 * N.z) p.potential_energy = m * g * h assert p.potential_energy == m * g * h # TODO make the result not be system-dependent assert p.kinetic_energy(N) in [ m2 * (v1**2 + v2**2 + v3**2) / 2, m2 * v1**2 / 2 + m2 * v2**2 / 2 + m2 * v3**2 / 2, ]
def vizAsRotor(self, radius=0.1, length=1, nB=3, axis='x', color='white', format='pydy'): # --- Bodies visualization if format == 'pydy': from pydy.viz.shapes import Cylinder from pydy.viz.visualization_frame import VisualizationFrame blade_shape = Cylinder(radius=radius, length=length, color=color) viz = [] if axis == 'x': for iB in np.arange(nB): frame = self.frame.orientnew( 'b', 'Axis', (-np.pi / 2 + (iB - 1) * 2 * np.pi / nB, self.frame.x)) # Y pointing along blade center = Point('RB') center.set_pos(self.origin, length / 2 * frame.y) viz.append(VisualizationFrame(frame, center, blade_shape)) return viz else: raise NotImplementedError()
def frame_viz(Origin, frame, l=1, r=0.08): """ """ from pydy.viz.shapes import Cylinder from pydy.viz.visualization_frame import VisualizationFrame from sympy.physics.mechanics import Point # X_frame = frame.orientnew('ffx', 'Axis', (-np.pi/2, frame.z) ) # Make y be x Z_frame = frame.orientnew('ffz', 'Axis', (+np.pi/2, frame.x) ) # Make y be z X_shape = Cylinder(radius=r, length=l, color='red') # Cylinder are along y Y_shape = Cylinder(radius=r, length=l, color='green') Z_shape = Cylinder(radius=r, length=l, color='blue') X_center=Point('X'); X_center.set_pos(Origin, l/2 * X_frame.y) Y_center=Point('Y'); Y_center.set_pos(Origin, l/2 * frame.y) Z_center=Point('Z'); Z_center.set_pos(Origin, l/2 * Z_frame.y) X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape) Y_viz_frame = VisualizationFrame( frame, Y_center, Y_shape) Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape) return X_viz_frame, Y_viz_frame, Z_viz_frame
def vizFrame(self, radius=0.1, length=1.0, format='pydy'): if format == 'pydy': from pydy.viz.shapes import Cylinder from pydy.viz.visualization_frame import VisualizationFrame from sympy.physics.mechanics import Point X_frame = self.frame.orientnew( 'ffx', 'Axis', (-np.pi / 2, self.frame.z)) # Make y be x Z_frame = self.frame.orientnew( 'ffz', 'Axis', (+np.pi / 2, self.frame.x)) # Make y be z X_shape = Cylinder(radius=radius, length=length, color='red') # Cylinder are along y Y_shape = Cylinder(radius=radius, length=length, color='green') Z_shape = Cylinder(radius=radius, length=length, color='blue') X_center = Point('X') X_center.set_pos(self.origin, length / 2 * X_frame.y) Y_center = Point('Y') Y_center.set_pos(self.origin, length / 2 * self.frame.y) Z_center = Point('Z') Z_center.set_pos(self.origin, length / 2 * Z_frame.y) X_viz_frame = VisualizationFrame(X_frame, X_center, X_shape) Y_viz_frame = VisualizationFrame(self.frame, Y_center, Y_shape) Z_viz_frame = VisualizationFrame(Z_frame, Z_center, Z_shape) return X_viz_frame, Y_viz_frame, Z_viz_frame
#Sets up inertial frame as well as frames for each linkage inertial_frame = ReferenceFrame('I') s_frame = ReferenceFrame('S') #Sets up symbols for joint angles theta_s = dynamicsymbols('theta_s') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 s_frame.orient(inertial_frame, 'Axis', (theta_s, inertial_frame.z)) #Sets up points for the joints and places them relative to each other S = Point('S') s_length = symbols('l_s') T = Point('T') T.set_pos(S, s_length*s_frame.y) #Sets up the angular velocities omega_s = dynamicsymbols('omega_s') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_s - theta_s.diff()] #Sets up the rotational axes of the angular velocities s_frame.set_ang_vel(inertial_frame, omega_s*inertial_frame.z) #Sets up the linear velocities of the points on the linkages S.set_vel(inertial_frame, 0) T.v2pt_theory(S, inertial_frame, s_frame) #Sets up the masses of the linkages s_mass = symbols('m_s')
one_frame = ReferenceFrame('1') two_frame = ReferenceFrame('2') #Sets up symbols for joint angles theta1, theta2 = dynamicsymbols('theta1, theta2') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z)) #Sets up points for the joints and places them relative to each other one = Point('1') one_length = symbols('l_1') two = Point('2') two.set_pos(one, one_length*one_frame.y) three = Point('3') two_length = symbols('l_2') three.set_pos(two, two_length*two_frame.y) #Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z) two_frame.ang_vel_in(inertial_frame)
N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = A.orientnew('B', 'axis', [q2, A.z]) C = N.orientnew('C', 'Axis', [theta, N.z]) # Joints location and speed J0 = Point('J0') J0.set_vel(N, 0) J1 = J0.locatenew('J1', bb * A.y) J2 = J1.locatenew('J2', ob * B.y) J1.v2pt_theory(J0, N, A) J2.v2pt_theory(J1, N, B) #Pedal position and speed O = Point('O') O.set_pos(J0, -d2 * N.x + d1 * N.y) O.set_vel(N, 0 * N.z) S = O.locatenew('S', -ta * C.y) # hoek van pedaal vanaf het hoogste punt van de cirkel S.v2pt_theory(O, N, C) # Centres of mass and speed P = J0.locatenew('P', 0.5 * bb * A.y) R = J1.locatenew('R', 0.5 * ob * B.y) T = O.locatenew('T', 0.5 * ta * C.y) P.v2pt_theory(J0, N, A) R.v2pt_theory(J1, N, B) T.v2pt_theory(O, N, C) # Bodies IP = inertia(A, 1 / 12 * m * (3 * r**2 + bb**2), 0,
body_frame = ReferenceFrame("B") body_frame.orient(l_leg_frame, "Axis", (theta2, l_leg_frame.z)) # Point Locations # =============== # Joints # ------ l_leg_length, hip_width = symbols("l_L, h_W") l_ankle = Point("LA") l_hip = Point("LH") l_hip.set_pos(l_ankle, l_leg_length * l_leg_frame.y) r_hip = Point("RH") r_hip.set_pos(l_hip, -1 * hip_width * body_frame.x) # Center of mass locations # ------------------------ l_leg_com_length, body_com_length, r_leg_com_length, body_com_height = symbols("d_L, d_B, d_R, d_BH") l_leg_mass_center = Point("LL_o") l_leg_mass_center.set_pos(l_ankle, l_leg_com_length * l_leg_frame.y) body_mass_center = Point("B_o") body_mass_center.set_pos(l_hip, body_com_height * body_frame.y)
# =============== # Joints # ------ a_length, b_length, c_length = symbols('l_a, l_b, l_c') a_mass, b_mass, c_mass = symbols('m_a, m_b, m_c') A = Point('A') B = Point('B') C = Point('C') D = Point('D') com_global = Point('G_o') com_bc = Point('BC_o') B.set_pos(A, a_length * a_frame.y) C.set_pos(B, b_length * b_frame.y) D.set_pos(C, c_length * c_frame.y) t_ai = simplify(B.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) t_bi = simplify(C.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) t_ci = simplify(D.pos_from(A).express(inertial_frame).to_matrix(inertial_frame)) t_ba = simplify(C.pos_from(B).express(inertial_frame).to_matrix(inertial_frame)) t_ca = simplify(D.pos_from(B).express(inertial_frame).to_matrix(inertial_frame)) com_global_coords = (t_ai*a_mass + t_bi*b_mass + t_ci*c_mass)/(a_mass+b_mass+c_mass) com_bc_coords = (t_ba*b_mass + t_ba*c_mass)/(b_mass+c_mass) theta_go = atan(com_global_coords[0]/com_global_coords[1]) theta_bco = atan(com_bc_coords[0]/com_bc_coords[1])
vol_C)) print('\nC* = {0}'.format(pCs.pos_from(pC_O))) print('C* = {0}'.format(eval_vec(pCs.pos_from(pC_O)))) ## FIND CENTER OF MASS OF D vol_D = pi*a**3/4 pD_O = Point('D_O') pDs = pD_O.locatenew('D*', (a*N.z + a/2*N.y + (N.x - N.z)*(centroid_sector.subs( theta_pi_4) * sin(pi/4)))) print('\nD* = {0}'.format(pDs.pos_from(pD_O))) print('D* = {0}'.format(eval_vec(pDs.pos_from(pD_O)))) ## FIND CENTER OF MASS OF ASSEMBLY pO = Point('O') pA_O.set_pos(pO, 2*a*N.x - (a+b)*N.z) pB_O.set_pos(pO, 2*a*N.x - a*N.z) pC_O.set_pos(pO, -(a+b)*N.z) pD_O.set_pos(pO, -a*N.z) density_A = 7800 density_B = 17.00 density_C = 2700 density_D = 8400 mass_A = vol_A * density_A mass_B = vol_B * density_B mass_C = vol_C * density_C mass_D = vol_D * density_D pms = pO.locatenew('m*', ((pAs.pos_from(pO)*mass_A + pBs.pos_from(pO)*mass_B + pCs.pos_from(pO)*mass_C + pDs.pos_from(pO)*mass_D) /
j2_frame = ReferenceFrame('J2') #declare dynamic symbols for the three joints theta0, theta1, theta2 = dynamicsymbols('theta0, theta1, theta2') #orient frames j0_frame.orient(inertial_frame, 'Axis', (theta0, inertial_frame.y)) j1_frame.orient(j0_frame, 'Axis', (theta1, j0_frame.z)) j2_frame.orient(j1_frame, 'Axis', (theta2, j1_frame.x)) #TODO figure out better name for joint points #init joints joint0 = Point('j0') j0_length = symbols('l_j0') joint1 = Point('j1') joint1.set_pos(joint0, j0_length * j0_frame.y) j1_length = symbols('l_j1') joint2 = Point('j2') joint2.set_pos(joint1, j1_length * j1_frame.y) #create End Effector EE = Point('E') #COM locations j0_com_length, j1_com_length, j2_com_length = symbols('d_j0, d_j1, d_j2') j0_mass_center = Point('j0_o') j0_mass_center.set_pos(joint0, j0_com_length * j0_frame.y) j1_mass_center = Point('j1_o') j1_mass_center.set_pos(joint1, j1_com_length * j1_frame.y) j2_mass_center = Point('j2_o') j2_mass_center.set_pos(joint2, j2_com_length * j2_frame.y) EE.set_pos(joint2, j2_com_length * 2 * j2_frame.y)
n = 1 q = dynamicsymbols('q:' + str(n + 1)) # Generalized coordinates u = dynamicsymbols('u:' + str(n + 1)) # Generalized speeds f = dynamicsymbols('f') # Force applied to the cart m = symbols('m:' + str(n + 1)) # Mass of each bob length = symbols('l:' + str(n)) # Length of each link g, t = symbols('g t') # Gravity and time ref_frame = ReferenceFrame('I') # Inertial reference frame origin = Point('O') # Origin point origin.set_vel(ref_frame, 0) # Origin's velocity is zero P0 = Point('P0') # Hinge point of top link P0.set_pos(origin, q[0] * ref_frame.x) # Set the position of P0 P0.set_vel(ref_frame, u[0] * ref_frame.x) # Set the velocity of P0 Pa0 = Particle('Pa0', P0, m[0]) # Define a particle at P0 # List to hold the n + 1 frames frames = [ref_frame] points = [P0] # List to hold the n + 1 points particles = [Pa0] # List to hold the n + 1 particles # List to hold the n + 1 applied forces, including the input force, f forces = [ (P0, f * ref_frame.x - m[0] * g * ref_frame.y - 0.01 * q[0] * ref_frame.x) ] kindiffs = [q[0].diff(t) - u[0]] # List to hold kinematic ODE's for i in range(n):
upper_leg_frame.orient(lower_leg_frame, 'Axis', (theta2, lower_leg_frame.z)) # Point Locations # =============== # Joints # ------ lower_leg_length, upper_leg_length = symbols('l_L, l_U') ankle = Point('A') knee = Point('K') knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y) # Center of mass locations # ------------------------ lower_leg_com_length, upper_leg_com_length = symbols('d_L, d_U') lower_leg_mass_center = Point('L_o') lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y) upper_leg_mass_center = Point('U_o') upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y) # Define kinematical differential equations # =========================================
two_frame_dp2.orient(one_frame_dp2, "Axis", (theta2_dp2, one_frame_dp2.z)) # Point Locations # =============== # Joints # ------ one_length_dp1, one_length_dp2, two_length_dp2 = symbols("l_1dp1, l_1dp2, l_2dp2") one_dp1 = Point("1_dp1") one_dp2 = Point("1_dp2") two_dp1 = Point("2_dp1") two_dp2 = Point("2_dp2") two_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y) two_dp2.set_pos(one_dp2, one_length_dp2 * one_frame_dp2.y) three_dp2 = Point("3_dp2") three_dp2.set_pos(two_dp2, two_length_dp2 * two_frame_dp2.y) # Center of mass locations # ------------------------ one_com_xdp1, one_com_ydp1, two_com_xdp1, two_com_ydp1 = dynamicsymbols( "1_comx_dp1, 1_comy_dp1, 2_comx_dp1, 2_comy_dp1" ) one_mass_center_dp1 = Point("1_odp1") one_mass_center_dp1.set_pos(one_dp1, one_length_dp1 * one_frame_dp1.y)
body_frame = ReferenceFrame("B") body_frame.orient(crotch_frame, "Axis", (theta4, crotch_frame.z)) # Point Locations # =============== # Joints # ------ l_leg_length, hip_width, body_height = symbols("l_L, h_W, b_H") l_ankle = Point("LA") l_hip = Point("LH") l_hip.set_pos(l_ankle, l_leg_length * l_leg_frame.y) r_hip = Point("RH") r_hip.set_pos(l_hip, hip_width * crotch_frame.x) body_middle = Point("B_m") body_middle.set_pos(l_hip, (hip_width / 2) * crotch_frame.x) waist = Point("W") waist.set_pos(body_middle, body_height * crotch_frame.y) # Center of mass locations # ------------------------ l_leg_com_length, r_leg_com_length, crotch_com_height, body_com_height = symbols("d_L, d_R, d_C, d_B")
torso_frame = ReferenceFrame('T') torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z)) # Point Locations # =============== # Joints # ------ lower_leg_length, upper_leg_length = symbols('l_L, l_U') ankle = Point('A') knee = Point('K') knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y) hip = Point('H') hip.set_pos(knee, upper_leg_length * upper_leg_frame.y) # Center of mass locations # ------------------------ lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols('d_L, d_U, d_T') lower_leg_mass_center = Point('L_o') lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y) upper_leg_mass_center = Point('U_o') upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
def get_model(model_name, **opts): """ model: string defining model opts: dictionary of options with keys: see _defaultOpts """ for k, v in _defaultOpts.items(): if k not in opts.keys(): opts[k] = v for k, v in opts.items(): if k not in _defaultOpts.keys(): raise Exception( 'Key {} not supported for model options.'.format(k)) #print(opts) # Extract info from model name sFnd = model_name.split('T')[0][1:] if len(sFnd) == 1: nDOF_fnd = int(sFnd[0]) bFndDOFs = [False] * 6 bFndDOFs[0] = nDOF_fnd >= 1 # x bFndDOFs[4] = nDOF_fnd >= 2 # phiy bFndDOFs[2] = nDOF_fnd == 3 or nDOF_fnd == 6 # z bFndDOFs[1] = nDOF_fnd >= 5 # y bFndDOFs[3] = nDOF_fnd >= 5 # phi_x bFndDOFs[5] = nDOF_fnd >= 5 # phi_z else: bFndDOFs = [s == '1' for s in sFnd] nDOF_fnd = sum(bFndDOFs) nDOF_twr = int(model_name.split('T')[1][0]) bFullRNA = model_name.find('RNA') == -1 bBlades = False # Rotor nDOF_nac = 0 nDOF_sft = 0 nDOF_bld = 0 if bFullRNA: nDOF_nac = int(model_name.split('N')[1][0]) nDOF_sft = int(model_name.split('S')[1][0]) bBlades = model_name.find('B') > 0 if bBlades: nDOF_bld = model_name.split('B')[1][0] else: nDOF_bld = 0 #print('fnd',','.join(['1' if b else '0' for b in bFndDOFs]), 'twr',nDOF_twr, 'nac',nDOF_nac, 'sft',nDOF_sft, 'bld',nDOF_bld) # --------------------------------------------------------------------------------} # --- Isolated bodies # --------------------------------------------------------------------------------{ # Reference frame ref = YAMSInertialBody('E') # Foundation, floater, always rigid for now if (not opts['floating']) or opts['mergeFndTwr']: fnd = None # the floater is merged with the twr, or we are not floating else: fnd = YAMSRigidBody('F', rho_G=[0, 0, z_FG], J_diag=True) # Tower if nDOF_twr == 0: # Ridid tower twr = YAMSRigidBody('T', rho_G=[0, 0, z_TG], J_diag=True) twrDOFs = [] twrSpeeds = [] elif nDOF_twr <= 4: # Flexible tower twr = YAMSFlexibleBody('T', nDOF_twr, directions=opts['twrDOFDir'], orderMM=opts['orderMM'], orderH=opts['orderH'], predefined_kind='twr-z') twrDOFs = twr.q twrSpeeds = twr.qd # Nacelle rotor assembly if bFullRNA: # Nacelle nac = YAMSRigidBody('N', rho_G=[x_NG, 0, z_NG], J_cross=True) # Shaft #... # Individual blades or rotor if bBlades: # Individual blades raise NotImplementedError() else: # Rotor rot = YAMSRigidBody('R', rho_G=[0, 0, 0], J_diag=True) rot.inertia = (inertia(rot.frame, Jxx_R, JO_R, JO_R), rot.origin ) # defining inertia at orign else: # Nacelle #nac = YAMSRigidBody('RNA', rho_G = [x_RNAG ,0, z_RNAG], J_diag=True) nac = YAMSRigidBody('RNA', rho_G=[x_RNAG, 0, z_RNAG], J_cross=True) rot = None # --------------------------------------------------------------------------------} # --- Body DOFs # --------------------------------------------------------------------------------{ # Fnd if (not opts['floating']): fndDOFs = [] fndSpeeds = [] else: fndDOFsAll = [x, y, z, phi_x, phi_y, phi_z] fndSpeedsAll = [xd, yd, zd, omega_x_T, omega_y_T, omega_z_T] fndDOFs = [dof for active, dof in zip(bFndDOFs, fndDOFsAll) if active] fndSpeeds = [ dof for active, dof in zip(bFndDOFs, fndSpeedsAll) if active ] # Twr # Nac if nDOF_nac == 2: opts['yaw'] = 'dynamic' opts['tilt'] = 'dynamic' if opts['tiltShaft'] and opts['tilt'] == 'dynamic': raise Exception('Cannot do tiltshaft with tilt dynamic') yawDOF = {'zero': 0, 'fixed': theta_yaw, 'dynamic': q_yaw}[opts['yaw']] tiltDOF = {'zero': 0, 'fixed': theta_tilt, 'dynamic': q_tilt}[opts['tilt']] nacDOFs = [] nacSpeeds = [] nacKDEqSubs = [] if opts['yaw'] == 'dynamic': nacDOFs += [q_yaw] nacSpeeds += [qd_yaw] nacKDEqSubs += [(qd_yaw, diff(q_yaw, time))] if opts['tilt'] == 'dynamic': nacDOFs += [q_tilt] nacSpeeds += [qd_tilt] nacKDEqSubs += [(qd_tilt, diff(q_tilt, time))] nacDOFsAct = (opts['yaw'] == 'dynamic', opts['tilt'] == 'dynamic') if nDOF_nac == 0: if not (nacDOFsAct == (False, False)): raise Exception( 'If nDOF_nac is 0, yaw and tilt needs to be "fixed" or "zero"') elif nDOF_nac == 1: if not (nacDOFsAct == (True, False) or nacDOFsAct == (False, True)): raise Exception( 'If nDOF_nac is 1, yaw or tilt needs to be "dynamic"') else: if not (nacDOFsAct == (True, True)): raise Exception( 'If nDOF_nac is 2, yaw and tilt needs to be "dynamic"') # Shaft sftDOFs = [] sftSpeeds = [] if bFullRNA: if nDOF_sft == 1: sftDOFs = [psi] sftSpeeds = [omega_x_R] elif nDOF_sft == 0: pass else: raise Exception('nDOF shaft should be 0 or 1') # Blade Rotor if bBlades: pass else: pass coordinates = fndDOFs + twrDOFs + nacDOFs + sftDOFs speeds = fndSpeeds + twrSpeeds + nacSpeeds + sftSpeeds # Order determine eq order # --------------------------------------------------------------------------------} # --- Connections between bodies # --------------------------------------------------------------------------------{ z_OT = Symbol('z_OT') if opts['floating']: rel_pos = [0, 0, 0] rel_pos[0] = x if bFndDOFs[0] else 0 rel_pos[1] = y if bFndDOFs[1] else 0 rel_pos[2] = z + z_OT if bFndDOFs[2] else z_OT rots = [0, 0, 0] rots[0] = phi_x if bFndDOFs[3] else 0 rots[1] = phi_y if bFndDOFs[4] else 0 rots[2] = phi_z if bFndDOFs[5] else 0 if nDOF_fnd == 0: #print('Rigid connection ref twr', rel_pos) ref.connectTo(twr, type='Rigid', rel_pos=rel_pos) elif nDOF_fnd == 1: #print('Constraint connection ref twr') ref.connectTo(twr, type='Free', rel_pos=(x, 0, z_OT), rot_amounts=(0, x * symbols('nu'), 0), rot_order='XYZ') else: #print('Free connection ref twr', rel_pos, rots) ref.connectTo( twr, type='Free', rel_pos=rel_pos, rot_amounts=rots, rot_order='XYZ' ) #NOTE: rot order is not "optimal".. phi_x should be last #ref.connectTo(twr, type='Free' , rel_pos=rel_pos, rot_amounts=(rots[2],rots[1],rots[0]), rot_order='ZYX') #NOTE: rot order is not "optimal".. phi_x should be last else: #print('Rigid connection ref twr') ref.connectTo(twr, type='Rigid', rel_pos=(0, 0, 0)) # Rigid connection between twr and fnd if fnd exists if fnd is not None: #print('Rigid connection twr fnd') if nDOF_twr == 0: twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F else: twr.connectTo(fnd, type='Rigid', rel_pos=(0, 0, 0)) # -L_F if nDOF_twr == 0: # Tower rigid -> Rigid connection to nacelle # TODO TODO L_T or twr.L #if nDOF_nac==0: #print('Rigid connection twr nac') #else: #print('Dynamic connection twr nac') if opts['tiltShaft']: # Shaft will be tilted, not nacelle twr.connectTo(nac, type='Rigid', rel_pos=(0, 0, L_T), rot_amounts=(yawDOF, 0, 0), rot_order='ZYX') else: # Nacelle is tilted twr.connectTo(nac, type='Rigid', rel_pos=(0, 0, L_T), rot_amounts=(yawDOF, tiltDOF, 0), rot_order='ZYX') else: # Flexible tower -> Flexible connection to nacelle #print('Flexible connection twr nac') if opts['tiltShaft']: twr.connectToTip(nac, type='Joint', rel_pos=(0, 0, twr.L), rot_amounts=(yawDOF, 0, 0), rot_order='ZYX', rot_type_elastic=opts['rot_elastic_type'], doSubs=opts['rot_elastic_subs']) else: twr.connectToTip(nac, type='Joint', rel_pos=(0, 0, twr.L), rot_amounts=(yawDOF, tiltDOF, 0), rot_order='ZYX', rot_type_elastic=opts['rot_elastic_type'], doSubs=opts['rot_elastic_subs']) if bFullRNA: if bBlades: raise NotImplementedError() else: if opts['tiltShaft']: if nDOF_sft == 0: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, tiltDOF, 0), rot_order='ZYX') else: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, tiltDOF, psi), rot_order='ZYX') else: if nDOF_sft == 0: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, 0, 0), rot_order='ZYX') else: nac.connectTo(rot, type='Joint', rel_pos=(x_NR, 0, z_NR), rot_amounts=(0, 0, psi), rot_order='ZYX') # --- Defining Body rotational velocities omega_TE = twr.ang_vel_in( ref) # Angular velocity of nacelle in inertial frame omega_NT = nac.ang_vel_in( twr.frame) # Angular velocity of nacelle in inertial frame if rot is not None: omega_RN = rot.ang_vel_in( nac.frame ) # Angular velocity of rotor wrt Nacelle (omega_R-omega_N) # --- Kinetics body_loads = [] bodies = [] g = symbols('g') g_vect = -g * ref.frame.z # Foundation/floater loads if fnd is not None: bodies += [fnd] grav_F = (fnd.masscenter, -fnd.mass * g * ref.frame.z) # Point of application for Buoyancy and mooring P_B = twr.origin.locatenew('P_B', z_TB * fnd.frame.z) # <<<< Measured from T P_M = twr.origin.locatenew('P_M', z_TM * fnd.frame.z) # <<<< Measured from T P_B.v2pt_theory(twr.origin, ref.frame, twr.frame) # PB & T are fixed in e_T P_M.v2pt_theory(twr.origin, ref.frame, twr.frame) # PM & T are fixed in e_T if opts['fnd_loads']: K_Mx, K_My, K_Mz = symbols( 'K_x_M, K_y_M, K_z_M') # Mooring restoring K_Mphix, K_Mphiy, K_Mphiz = symbols( 'K_phi_x_M, K_phi_y_M, K_phi_z_M') # Mooring restoring F_B = dynamicsymbols('F_B') # Buoyancy force #print('>>> Adding fnd loads. NOTE: reference might need to be adapted') # Buoyancy body_loads += [(fnd, (P_B, F_B * ref.frame.z))] ## Restoring mooring and torques fr = 0 fr += -K_Mx * x * ref.frame.x if bFndDOFs[0] else 0 fr += -K_My * y * ref.frame.y if bFndDOFs[1] else 0 fr += -K_Mz * z * ref.frame.z if bFndDOFs[2] else 0 Mr += -K_MPhix * phi_x * ref.frame.x if bFndDOFs[3] else 0 Mr += -K_MPhiy * phi_y * ref.frame.y if bFndDOFs[4] else 0 Mr += -K_MPhiz * phi_z * ref.frame.z if bFndDOFs[5] else 0 body_loads += [(fnd, (P_M, fr))] body_loads += [(fnd, (fnd.frame, Mr))] body_loads += [(fnd, grav_F)] # Tower loads grav_T = (twr.masscenter, -twr.mass * g * ref.frame.z) bodies += [twr] body_loads += [(twr, grav_T)] # Nacelle loads grav_N = (nac.masscenter, -nac.mass * g * ref.frame.z) bodies += [nac] body_loads += [(nac, grav_N)] T_a = dynamicsymbols('T_a') # NOTE NOTE #T_a = Function('T_a')(dynamicsymbols._t, *coordinates, *speeds) # NOTE: to introduce it in the linearization, add coordinates M_ax, M_ay, M_az = dynamicsymbols('M_x_a, M_y_a, M_z_a') # Aero torques if bFullRNA: if bBlades: raise NotImplementedError() else: # Rotor loads grav_R = (rot.masscenter, -M_R * g * ref.frame.z) bodies += [rot] body_loads += [(rot, grav_R)] # NOTE: loads on rot, but expressed in N frame if opts['tiltShaft']: # TODO actually tilt shaft, introduce non rotating shaft body if opts['aero_forces']: thrustR = (rot.origin, T_a * cos(tiltDOF) * nac.frame.x - T_a * sin(tiltDOF) * nac.frame.z) if opts['aero_torques']: M_a_R = (nac.frame, M_ax * nac.frame.x * 0) # TODO TODO #print('>>> WARNING tilt shaft aero moments not implemented') # TODO body_loads += [(nac, M_a_R)] else: thrustR = (rot.origin, T_a * nac.frame.x) #thrustR = (rot.origin, T_a * rot.frame.x ) #M_a_R = (rot.frame, M_ax*rot.frame.x + M_ay*rot.frame.y + M_az*rot.frame.z) # TODO TODO TODO introduce a non rotating shaft if opts['aero_torques']: M_a_R = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y + M_az * nac.frame.z) body_loads += [(nac, M_a_R)] body_loads += [(rot, thrustR)] else: # RNA loads, point load at R R = Point('R') R.set_pos(nac.origin, x_NR * nac.frame.x + z_NR * nac.frame.z) R.set_vel(nac.frame, 0 * nac.frame.x) R.v2pt_theory(nac.origin, ref.frame, nac.frame) #thrustN = (nac.masscenter, T * nac.frame.x) if opts['tiltShaft']: thrustN = (R, T_a * cos(tiltDOF) * nac.frame.x - T_a * sin(tiltDOF) * nac.frame.z) else: thrustN = (R, T_a * nac.frame.x) if opts['aero_forces']: body_loads += [(nac, thrustN)] if opts['aero_torques']: #print('>>> Adding aero torques') if opts['tiltShaft']: # NOTE: for a rigid RNA we keep only M_y and M_z, no shaft torque x_tilted = cos(tiltDOF) * nac.frame.x - sin( tiltDOF) * nac.frame.z z_tilted = cos(tiltDOF) * nac.frame.y + sin( tiltDOF) * nac.frame.x M_a_N = (nac.frame, M_ay * nac.frame.y + M_az * z_tilted) else: M_a_N = (nac.frame, M_ax * nac.frame.x + M_ay * nac.frame.y + M_az * nac.frame.z) body_loads += [(nac, M_a_N)] # --------------------------------------------------------------------------------} # --- Kinematic equations # --------------------------------------------------------------------------------{ kdeqsSubs = [] # --- Fnd if not opts['floating']: pass else: # Kdeqs for fnd: # : (xd, diff(x,time)) and (omega_y_T, diff(phi_y,time)) fndVelAll = [diff(x, time), diff(y, time), diff(z, time)] if opts['linRot']: fndVelAll += [ diff(phi_x, time), diff(phi_y, time), diff(phi_z, time) ] else: #print('>>>>>>>> TODO sort out which frame') #fndVelAll +=[ omega_TE.dot(ref.frame.x).simplify(), omega_TE.dot(ref.frame.y).simplify(), omega_TE.dot(ref.frame.z).simplify()] fndVelAll += [ omega_TE.dot(twr.frame.x).simplify(), omega_TE.dot(twr.frame.y).simplify(), omega_TE.dot(twr.frame.z).simplify() ] kdeqsSubs += [(fndSpeedsAll[i], fndVelAll[i]) for i, dof in enumerate(bFndDOFs) if dof] # --- Twr if nDOF_twr == 0: pass else: kdeqsSubs += [(twr.qd[i], twr.qdot[i]) for i, _ in enumerate(twr.q)] # --- Nac kdeqsSubs += nacKDEqSubs # --- Shaft if bFullRNA: if bBlades: raise NotImplementedError() else: if nDOF_sft == 1: #print('>>>>>>>> TODO sort out which frame') # I believe we should use omega_RE kdeqsSubs += [(omega_x_R, omega_RN.dot(rot.frame.x).simplify()) ] # --- Create a YAMS wrapper model model = YAMSModel(name=model_name) model.opts = opts model.ref = ref model.bodies = bodies model.body_loads = body_loads model.coordinates = coordinates model.speeds = speeds model.kdeqsSubs = kdeqsSubs #print(model) model.fnd = fnd model.twr = twr model.nac = nac model.rot = rot #model.sft=sft #model.bld=bld model.g_vect = g_vect # Small angles model.smallAnglesFnd = [phi_x, phi_y, phi_z] if nDOF_twr > 0: if opts['rot_elastic_smallAngle']: model.smallAnglesTwr = twr.vcList else: model.smallAnglesTwr = [] else: model.smallAnglesTwr = [] model.smallAnglesNac = [] if opts['yaw'] == 'dynamic': model.smallAnglesNac += [q_yaw] if opts['tilt'] == 'dynamic': model.smallAnglesNac += [q_tilt] model.smallAngles = model.smallAnglesFnd + model.smallAnglesTwr + model.smallAnglesNac # Shape normalization if nDOF_twr > 0: model.shapeNormSubs = [(v, 1) for v in twr.ucList] else: model.shapeNormSubs = [] return model
c_frame = ReferenceFrame('C') #Sets up symbols for joint angles theta_a, theta_b, theta_c = dynamicsymbols('theta_a, theta_b, theta_c') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z)) b_frame.orient(a_frame, 'Axis', (theta_b, a_frame.z)) c_frame.orient(b_frame, 'Axis', (theta_c, b_frame.z)) #Sets up points for the joints and places them relative to each other A = Point('A') a_length = symbols('l_a') B = Point('B') B.set_pos(A, a_length*a_frame.y) C = Point('C') b_length = symbols('l_b') C.set_pos(B, b_length*b_frame.y) D = Point('D') c_length = symbols('l_c') D.set_pos(C, c_length*c_frame.y) #Sets up the angular velocities omega_a, omega_b, omega_c = dynamicsymbols('omega_a, omega_b, omega_c') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_a - theta_a.diff(), omega_b - theta_b.diff(), omega_c - theta_c.diff()] #Sets up the rotational axes of the angular velocities
two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z)) # Point Locations # =============== # Joints # ------ one_length = symbols('l_1') one = Point('1') two = Point('2') two.set_pos(one, one_length * one_frame.y) # Center of mass locations # ------------------------ one_com_x, one_com_y, two_com_x, two_com_y = dynamicsymbols('1_comx, 1_comy, 2_comx, 2_comy') one_mass_center = Point('1_o') one_mass_center.set_pos(one, one_com_x*inertial_frame.x + one_com_y*inertial_frame.y) two_mass_center = Point('2_o') two_mass_center.set_pos(two, two_com_x*inertial_frame.x + two_com_y*inertial_frame.y) # Define kinematical differential equations # =========================================
leg_frame = ReferenceFrame('L') body_frame = ReferenceFrame('B') #Sets up symbols for joint angles theta1, theta2 = dynamicsymbols('theta1, theta2') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) body_frame.orient(leg_frame, 'Axis', (theta2, leg_frame.z)) #Sets up points for the joints and places them relative to each other ankle = Point('A') leg_length = symbols('l_L') waist = Point('W') waist.set_pos(ankle, leg_length*leg_frame.y) body = Point('B') body_length = symbols('l_B') body.set_pos(waist, body_length*body_frame.y) #Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) leg_frame.ang_vel_in(inertial_frame) body_frame.set_ang_vel(leg_frame, omega2*inertial_frame.z) body_frame.ang_vel_in(inertial_frame)
# Referenciais # Referencial Inercial B0 = ReferenceFrame('B0') # Referencial móvel: theta_1 em relação a B0.z B1 = ReferenceFrame('B1') B1.orient(B0, 'Axis', [THETA_1, B0.z]) # Referencial móvel: theta_2 em relação a B1.z B2 = ReferenceFrame('B2') B2.orient(B1, 'Axis', [THETA_2, 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(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))
RRTethCy = symbols('RRTethCy') RRTethCz = symbols('RRTethCz') LFTethLx = symbols('LFTethLx') # Tether lower positions LFTethLy = symbols('LFTethLy') LFTethLz = symbols('LFTethLz') RFTethLx = symbols('RFTethLx') RFTethLy = symbols('RFTethLy') RFTethLz = symbols('RFTethLz') LRTethHy = symbols('LRTethHy') # Tether housing positions RRTethHy = symbols('RRTethHy') #___________________________________________________________________________________________________________________________________________________ # Set Chassis Positions ChassisC.set_pos(GlobalCoord0,(ChassisDeltax * inertial_frame.x + ChassisDeltay * inertial_frame.y + ChassisDeltaz * inertial_frame.z)) # Chassis CGpos.set_pos(ChassisC,(CGposx * Chassis_frame.x + CGposy * Chassis_frame.y + CGposz * Chassis_frame.z)) # CG Position LUCAMP.set_pos(ChassisC,(LUCAMPx * Chassis_frame.x + LUCAMPy * Chassis_frame.y + LUCAMPz * Chassis_frame.z)) # Control Arm Mount Midpoints RUCAMP.set_pos(ChassisC,(RUCAMPx * Chassis_frame.x + RUCAMPy * Chassis_frame.y + RUCAMPz * Chassis_frame.z)) LLCAMP.set_pos(ChassisC,(LLCAMPx * Chassis_frame.x + LLCAMPy * Chassis_frame.y + LLCAMPz * Chassis_frame.z)) RLCAMP.set_pos(ChassisC,(RLCAMPx * Chassis_frame.x + RLCAMPy * Chassis_frame.y + RLCAMPz * Chassis_frame.z)) LUBC.set_pos(ChassisC,(LUBCx * Chassis_frame.x + LUBCy * Chassis_frame.y + LUBCz * Chassis_frame.z)) # Four bar chassis positions RUBC.set_pos(ChassisC,(RUBCx * Chassis_frame.x + RUBCy * Chassis_frame.y + RUBCz * Chassis_frame.z)) LLBC.set_pos(ChassisC,(LLBCx * Chassis_frame.x + LLBCy * Chassis_frame.y + LLBCz * Chassis_frame.z)) RLBC.set_pos(ChassisC,(RLBCx * Chassis_frame.x + RLBCy * Chassis_frame.y + RLBCz * Chassis_frame.z)) FifthShkC.set_pos(ChassisC,(FifthShkCx * Chassis_frame.x + FifthShkCy * Chassis_frame.y + FifthShkCz * Chassis_frame.z)) # Fifth coil chassis position JbarC.set_pos(ChassisC,(JbarCx * Chassis_frame.x + JbarCy * Chassis_frame.y + JbarCz * Chassis_frame.z)) # Jbar chassis position LFShkC.set_pos(ChassisC,(LFShkCx * Chassis_frame.x + LFShkCy * Chassis_frame.y + LFShkCz * Chassis_frame.z)) # Shock chassis positions RFShkC.set_pos(ChassisC,(RFShkCx * Chassis_frame.x + RFShkCy * Chassis_frame.y + RFShkCz * Chassis_frame.z)) LRShkC.set_pos(ChassisC,(LRShkCx * Chassis_frame.x + LRShkCy * Chassis_frame.y + LRShkCz * Chassis_frame.z)) RRShkC.set_pos(ChassisC,(RRShkCx * Chassis_frame.x + RRShkCy * Chassis_frame.y + RRShkCz * Chassis_frame.z))
#Sets up symbols for joint angles theta1, theta2 = dynamicsymbols('theta1, theta2') #Sets up the masses of the linkages one_mass, two_mass = symbols('m_1, m_2') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z)) #Sets up points for the joints and places them relative to each other one = Point('1') one_length = symbols('l_1') two = Point('2') two.set_pos(one, one_length*one_frame.y) three = Point('3') two_length = symbols('l_2') three.set_pos(two, two_length*two_frame.y) com = Point('c') com.set_pos(two, (two_length)*(two_mass/(one_mass + two_mass))*two_frame.y) #Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) one_frame.ang_vel_in(inertial_frame)
torso_frame = ReferenceFrame('T') torso_frame.orient(upper_leg_frame, 'Axis', (theta3, upper_leg_frame.z)) # Point Locations # =============== # Joints # ------ lower_leg_length, upper_leg_length = symbols('l_L, l_U') ankle = Point('A') knee = Point('K') knee.set_pos(ankle, lower_leg_length * lower_leg_frame.y) hip = Point('H') hip.set_pos(knee, upper_leg_length * upper_leg_frame.y) # Center of mass locations # ------------------------ lower_leg_com_length, upper_leg_com_length, torso_com_length = symbols( 'd_L, d_U, d_T') lower_leg_mass_center = Point('L_o') lower_leg_mass_center.set_pos(ankle, lower_leg_com_length * lower_leg_frame.y) upper_leg_mass_center = Point('U_o') upper_leg_mass_center.set_pos(knee, upper_leg_com_length * upper_leg_frame.y)
pCs_3.pos_from(pC_O) * vol_C_3 + pCs_4.pos_from(pC_O) * vol_C_4) / vol_C)) print('\nC* = {0}'.format(pCs.pos_from(pC_O))) print('C* = {0}'.format(eval_vec(pCs.pos_from(pC_O)))) ## FIND CENTER OF MASS OF D vol_D = pi * a**3 / 4 pD_O = Point('D_O') pDs = pD_O.locatenew('D*', (a * N.z + a / 2 * N.y + (N.x - N.z) * (centroid_sector.subs(theta_pi_4) * sin(pi / 4)))) print('\nD* = {0}'.format(pDs.pos_from(pD_O))) print('D* = {0}'.format(eval_vec(pDs.pos_from(pD_O)))) ## FIND CENTER OF MASS OF ASSEMBLY pO = Point('O') pA_O.set_pos(pO, 2 * a * N.x - (a + b) * N.z) pB_O.set_pos(pO, 2 * a * N.x - a * N.z) pC_O.set_pos(pO, -(a + b) * N.z) pD_O.set_pos(pO, -a * N.z) density_A = 7800 density_B = 17.00 density_C = 2700 density_D = 8400 mass_A = vol_A * density_A mass_B = vol_B * density_B mass_C = vol_C * density_C mass_D = vol_D * density_D pms = pO.locatenew('m*', ((pAs.pos_from(pO) * mass_A + pBs.pos_from(pO) * mass_B +
# these can be arbitray points on each of you robots links # later these points are visualized, so you can verify the correct build of your robot origin = Point('Origin') ankle_left = Point('AnkleLeft') knee_left = Point('KneeLeft') hip_left = Point('HipLeft') hip_center = Point('HipCenter') hip_right = Point('HipRight') knee_right = Point('KneeRight') ankle_right = Point('AnkleRight') # here go the lengths of your robots links lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3') ankle_left.set_pos(origin, (0 * inertial_frame.y) + (0 * inertial_frame.x)) #ankle_left.pos_from(origin).express(inertial_frame).simplify() knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y) #knee_left.pos_from(origin).express(inertial_frame).simplify() hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y) #hip_left.pos_from(origin).express(inertial_frame).simplify() hip_center.set_pos(hip_left, hip_length / 2 * hip_frame.x) #hip_center.pos_from(origin).express(inertial_frame).simplify() hip_right.set_pos(hip_center, hip_length / 2 * hip_frame.x) #hip_right.pos_from(origin).express(inertial_frame).simplify() knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)
# these can be arbitray points on each of you robots links # later these points are visualized, so you can verify the correct build of your robot origin = Point('Origin') ankle_left = Point('AnkleLeft') knee_left = Point('KneeLeft') hip_left = Point('HipLeft') hip_center = Point('HipCenter') hip_right = Point('HipRight') knee_right = Point('KneeRight') ankle_right = Point('AnkleRight') # here go the lengths of your robots links lower_leg_length, upper_leg_length, hip_length = symbols('l1, l2, l3') ankle_left.set_pos(origin, (0 * inertial_frame.y)+(0 * inertial_frame.x)) #ankle_left.pos_from(origin).express(inertial_frame).simplify() knee_left.set_pos(ankle_left, lower_leg_length * lower_leg_left_frame.y) #knee_left.pos_from(origin).express(inertial_frame).simplify() hip_left.set_pos(knee_left, upper_leg_length * upper_leg_left_frame.y) #hip_left.pos_from(origin).express(inertial_frame).simplify() hip_center.set_pos(hip_left, hip_length/2 * hip_frame.x) #hip_center.pos_from(origin).express(inertial_frame).simplify() hip_right.set_pos(hip_center, hip_length/2 * hip_frame.x) #hip_right.pos_from(origin).express(inertial_frame).simplify() knee_right.set_pos(hip_right, upper_leg_length * -upper_leg_right_frame.y)
a_frame = ReferenceFrame('A') base_frame = ReferenceFrame('Base') #Sets up symbols for joint angles theta_a = dynamicsymbols('theta_a') theta_base = dynamicsymbols('theta_base') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 a_frame.orient(inertial_frame, 'Axis', (theta_a, inertial_frame.z)) base_frame.orient(inertial_frame, 'Axis', (theta_base, inertial_frame.z)) #Sets up points for the joints and places them relative to each other A = Point('A') a_length = symbols('l_a') B = Point('B') B.set_pos(A, a_length*a_frame.y) #Sets up the angular velocities omega_base, omega_a = dynamicsymbols('omega_b, omega_a') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega_a - theta_a.diff()] #Sets up the rotational axes of the angular velocities a_frame.set_ang_vel(inertial_frame, omega_base*inertial_frame.z + omega_a*inertial_frame.z) #Sets up the linear velocities of the points on the linkages base_vel_x, base_vel_y = dynamicsymbols('v_bx, v_by') A.set_vel(inertial_frame, base_vel_x*base_frame.x + base_vel_y*base_frame.y) B.v2pt_theory(A, inertial_frame, a_frame) #Sets up the masses of the linkages
one_frame = ReferenceFrame('1') two_frame = ReferenceFrame('2') #Sets up symbols for joint angles theta1, theta2 = dynamicsymbols('theta1, theta2') #Orients the leg frame to the inertial frame by angle theta1 #and the body frame to to the leg frame by angle theta2 one_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) two_frame.orient(one_frame, 'Axis', (theta2, one_frame.z)) #Sets up points for the joints and places them relative to each other one = Point('1') one_length = symbols('l_1') two = Point('2') two.set_pos(one, one_length*one_frame.y) three = Point('3') two_length_x = symbols('l_2x') two_length_y = symbols('l_2y') three.set_pos(two, two_length_x*inertial_frame.x + two_length_y*inertial_frame.y) #Sets up the angular velocities omega1, omega2 = dynamicsymbols('omega1, omega2') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff()] #Sets up the rotational axes of the angular velocities one_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) one_frame.ang_vel_in(inertial_frame) two_frame.set_ang_vel(one_frame, omega2*inertial_frame.z)