def main(): print = lambda x: sympy.pprint(x, use_unicode=False, wrap_line=False) # init_session() # init_printing() theta, phi, psi = dynamicsymbols('theta, phi, psi') inertial_frame = ReferenceFrame('O') gyro_frame = ReferenceFrame('e') gyro_frame.orient(inertial_frame, 'Body', (phi, theta, psi), 'ZXZ') print('>>>>') print('>>>>') omega = gyro_frame.ang_vel_in( inertial_frame) # Angular velocity of a frame in another print(omega.to_matrix(gyro_frame))
def test_ang_vel(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1) N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) C = B.orientnew('C', 'Axis', [q3, B.y]) D = N.orientnew('D', 'Axis', [q4, N.y]) u1, u2, u3 = dynamicsymbols('u1 u2 u3') assert A.ang_vel_in(N) == (q1d)*A.z assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z A2 = N.orientnew('A2', 'Axis', [q4, N.y]) assert N.ang_vel_in(N) == 0 assert N.ang_vel_in(A) == -q1d*N.z assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y assert N.ang_vel_in(A2) == -q4d*N.y assert A.ang_vel_in(N) == q1d*N.z assert A.ang_vel_in(A) == 0 assert A.ang_vel_in(B) == - q2d*B.x assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x assert B.ang_vel_in(A) == q2d*A.x assert B.ang_vel_in(B) == 0 assert B.ang_vel_in(C) == -q3d*B.y assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y assert C.ang_vel_in(B) == q3d*B.y assert C.ang_vel_in(C) == 0 assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y assert A2.ang_vel_in(N) == q4d*A2.y assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y assert A2.ang_vel_in(A2) == 0 C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z) assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y q0 = dynamicsymbols('q0') q0d = dynamicsymbols('q0', 1) E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3)) assert E.ang_vel_in(N) == ( 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x + 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y + 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z) F = N.orientnew('F', 'Body', (q1, q2, q3), '313') assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x + (sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z) G = N.orientnew('G', 'Axis', (q1, N.x + N.y)) assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize() assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
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_length*body_frame.y) r_leg_mass_center = Point('RL_o') r_leg_mass_center.set_pos(r_hip, l_leg_com_length*r_leg_frame.y) #Sets up the angular velocities omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3') #Relates angular velocity values to the angular positions theta1 and theta2 kinematic_differential_equations = [omega1 - theta1.diff(), omega2 - theta2.diff(), omega3 - theta3.diff()] #Sets up the rotational axes of the angular velocities l_leg_frame.set_ang_vel(inertial_frame, omega1*inertial_frame.z) l_leg_frame.ang_vel_in(inertial_frame) body_frame.set_ang_vel(l_leg_frame, omega2*inertial_frame.z) body_frame.ang_vel_in(inertial_frame) r_leg_frame.set_ang_vel(body_frame, omega3*inertial_frame.z) r_leg_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages l_ankle.set_vel(inertial_frame, 0) l_leg_mass_center.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_leg_mass_center.vel(inertial_frame) l_hip.v2pt_theory(l_ankle, inertial_frame, l_leg_frame) l_hip.vel(inertial_frame) body_mass_center.v2pt_theory(l_hip, inertial_frame, body_frame) body_mass_center.vel(inertial_frame) r_hip.v2pt_theory(l_hip, inertial_frame, body_frame) r_hip.vel(inertial_frame)
def test_ang_vel(): q1, q2, q3, q4 = dynamicsymbols("q1 q2 q3 q4") q1d, q2d, q3d, q4d = dynamicsymbols("q1 q2 q3 q4", 1) N = ReferenceFrame("N") A = N.orientnew("A", "Axis", [q1, N.z]) B = A.orientnew("B", "Axis", [q2, A.x]) C = B.orientnew("C", "Axis", [q3, B.y]) D = N.orientnew("D", "Axis", [q4, N.y]) u1, u2, u3 = dynamicsymbols("u1 u2 u3") assert A.ang_vel_in(N) == (q1d) * A.z assert B.ang_vel_in(N) == (q2d) * B.x + (q1d) * A.z assert C.ang_vel_in(N) == (q3d) * C.y + (q2d) * B.x + (q1d) * A.z A2 = N.orientnew("A2", "Axis", [q4, N.y]) assert N.ang_vel_in(N) == 0 assert N.ang_vel_in(A) == -q1d * N.z assert N.ang_vel_in(B) == -q1d * A.z - q2d * B.x assert N.ang_vel_in(C) == -q1d * A.z - q2d * B.x - q3d * B.y assert N.ang_vel_in(A2) == -q4d * N.y assert A.ang_vel_in(N) == q1d * N.z assert A.ang_vel_in(A) == 0 assert A.ang_vel_in(B) == -q2d * B.x assert A.ang_vel_in(C) == -q2d * B.x - q3d * B.y assert A.ang_vel_in(A2) == q1d * N.z - q4d * N.y assert B.ang_vel_in(N) == q1d * A.z + q2d * A.x assert B.ang_vel_in(A) == q2d * A.x assert B.ang_vel_in(B) == 0 assert B.ang_vel_in(C) == -q3d * B.y assert B.ang_vel_in(A2) == q1d * A.z + q2d * A.x - q4d * N.y assert C.ang_vel_in(N) == q1d * A.z + q2d * A.x + q3d * B.y assert C.ang_vel_in(A) == q2d * A.x + q3d * C.y assert C.ang_vel_in(B) == q3d * B.y assert C.ang_vel_in(C) == 0 assert C.ang_vel_in(A2) == q1d * A.z + q2d * A.x + q3d * B.y - q4d * N.y assert A2.ang_vel_in(N) == q4d * A2.y assert A2.ang_vel_in(A) == q4d * A2.y - q1d * N.z assert A2.ang_vel_in(B) == q4d * N.y - q1d * A.z - q2d * A.x assert A2.ang_vel_in(C) == q4d * N.y - q1d * A.z - q2d * A.x - q3d * B.y assert A2.ang_vel_in(A2) == 0 C.set_ang_vel(N, u1 * C.x + u2 * C.y + u3 * C.z) assert C.ang_vel_in(N) == (u1) * C.x + (u2) * C.y + (u3) * C.z assert N.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z assert C.ang_vel_in(D) == (u1) * C.x + (u2) * C.y + (u3) * C.z + (-q4d) * D.y assert D.ang_vel_in(C) == (-u1) * C.x + (-u2) * C.y + (-u3) * C.z + (q4d) * D.y q0 = dynamicsymbols("q0") q0d = dynamicsymbols("q0", 1) E = N.orientnew("E", "Quaternion", (q0, q1, q2, q3)) assert E.ang_vel_in(N) == ( 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x + 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y + 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z ) F = N.orientnew("F", "Body", (q1, q2, q3), "313") assert F.ang_vel_in(N) == ( (sin(q2) * sin(q3) * q1d + cos(q3) * q2d) * F.x + (sin(q2) * cos(q3) * q1d - sin(q3) * q2d) * F.y + (cos(q2) * q1d + q3d) * F.z ) G = N.orientnew("G", "Axis", (q1, N.x + N.y)) assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize() assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
def test_ang_vel(): q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4') q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1) N = ReferenceFrame('N') A = N.orientnew('A', 'Axis', [q1, N.z]) B = A.orientnew('B', 'Axis', [q2, A.x]) C = B.orientnew('C', 'Axis', [q3, B.y]) D = N.orientnew('D', 'Axis', [q4, N.y]) u1, u2, u3 = dynamicsymbols('u1 u2 u3') assert A.ang_vel_in(N) == (q1d)*A.z assert B.ang_vel_in(N) == (q2d)*B.x + (q1d)*A.z assert C.ang_vel_in(N) == (q3d)*C.y + (q2d)*B.x + (q1d)*A.z A2 = N.orientnew('A2', 'Axis', [q4, N.y]) assert N.ang_vel_in(N) == 0 assert N.ang_vel_in(A) == -q1d*N.z assert N.ang_vel_in(B) == -q1d*A.z - q2d*B.x assert N.ang_vel_in(C) == -q1d*A.z - q2d*B.x - q3d*B.y assert N.ang_vel_in(A2) == -q4d*N.y assert A.ang_vel_in(N) == q1d*N.z assert A.ang_vel_in(A) == 0 assert A.ang_vel_in(B) == - q2d*B.x assert A.ang_vel_in(C) == - q2d*B.x - q3d*B.y assert A.ang_vel_in(A2) == q1d*N.z - q4d*N.y assert B.ang_vel_in(N) == q1d*A.z + q2d*A.x assert B.ang_vel_in(A) == q2d*A.x assert B.ang_vel_in(B) == 0 assert B.ang_vel_in(C) == -q3d*B.y assert B.ang_vel_in(A2) == q1d*A.z + q2d*A.x - q4d*N.y assert C.ang_vel_in(N) == q1d*A.z + q2d*A.x + q3d*B.y assert C.ang_vel_in(A) == q2d*A.x + q3d*C.y assert C.ang_vel_in(B) == q3d*B.y assert C.ang_vel_in(C) == 0 assert C.ang_vel_in(A2) == q1d*A.z + q2d*A.x + q3d*B.y - q4d*N.y assert A2.ang_vel_in(N) == q4d*A2.y assert A2.ang_vel_in(A) == q4d*A2.y - q1d*N.z assert A2.ang_vel_in(B) == q4d*N.y - q1d*A.z - q2d*A.x assert A2.ang_vel_in(C) == q4d*N.y - q1d*A.z - q2d*A.x - q3d*B.y assert A2.ang_vel_in(A2) == 0 C.set_ang_vel(N, u1*C.x + u2*C.y + u3*C.z) assert C.ang_vel_in(N) == (u1)*C.x + (u2)*C.y + (u3)*C.z assert N.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z assert C.ang_vel_in(D) == (u1)*C.x + (u2)*C.y + (u3)*C.z + (-q4d)*D.y assert D.ang_vel_in(C) == (-u1)*C.x + (-u2)*C.y + (-u3)*C.z + (q4d)*D.y q0 = dynamicsymbols('q0') q0d = dynamicsymbols('q0', 1) E = N.orientnew('E', 'Quaternion', (q0, q1, q2, q3)) assert E.ang_vel_in(N) == ( 2 * (q1d * q0 + q2d * q3 - q3d * q2 - q0d * q1) * E.x + 2 * (q2d * q0 + q3d * q1 - q1d * q3 - q0d * q2) * E.y + 2 * (q3d * q0 + q1d * q2 - q2d * q1 - q0d * q3) * E.z) F = N.orientnew('F', 'Body', (q1, q2, q3), '313') assert F.ang_vel_in(N) == ((sin(q2)*sin(q3)*q1d + cos(q3)*q2d)*F.x + (sin(q2)*cos(q3)*q1d - sin(q3)*q2d)*F.y + (cos(q2)*q1d + q3d)*F.z) G = N.orientnew('G', 'Axis', (q1, N.x + N.y)) assert G.ang_vel_in(N) == q1d * (N.x + N.y).normalize() assert N.ang_vel_in(G) == -q1d * (N.x + N.y).normalize()
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] # Since S is rolling against C, v_S^_C = 0. pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) vc += [dot(pS_hat.vel(C), basis) for basis in A] # Cone has only angular velocity ω in R.z direction. vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc += [omega - dot(C.ang_vel_in(R), R.z)] vc_map = solve(vc, u) # cone rigidbody I_C = inertia(A, I11, I22, J) rbC = RigidBody('rbC', pO, C, M, (I_C, pO)) # sphere rigidbody I_S = inertia(A, 2 * m * r**2 / 5, 2 * m * r**2 / 5, 2 * m * r**2 / 5) rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star)) # kinetic energy K = radsimp( expand((rbC.kinetic_energy(R) + 4 * rbS.kinetic_energy(R)).subs(vc_map))) print('K = {0}'.format(msprint(collect(K, omega**2 / 2))))
from sympy import sin, cos, pi, integrate, Matrix from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols from util import msprint, partial_velocities, generalized_active_forces ## --- Declare symbols --- u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10') c, R = symbols('c R') x, y, z, r, phi, theta = symbols('x y z r phi theta') # --- Reference Frames --- A = ReferenceFrame('A') B = ReferenceFrame('B') C = ReferenceFrame('C') B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z) C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z) C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A)) pC_star = Point('C*') pC_star.set_vel(C, 0) # since radius of cavity is very small, assume C* has zero velocity in B pC_star.set_vel(B, 0) pC_star.set_vel(A, u7 * B.x + u8 * B.y + u9 * B.z) ## --- define points P, P' --- # point on C pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z) pP.set_vel(C, 0) pP.v2pt_theory(pC_star, B, C) pP.v2pt_theory(pC_star, A, C) # point on B pP_prime = pP.locatenew("P'", 0)
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 a_frame.set_ang_vel(inertial_frame, omega_a*inertial_frame.z) a_frame.ang_vel_in(inertial_frame) b_frame.set_ang_vel(a_frame, omega_b*inertial_frame.z) b_frame.ang_vel_in(inertial_frame) c_frame.set_ang_vel(b_frame, omega_c*inertial_frame.z) c_frame.ang_vel_in(inertial_frame) #Sets up the linear velocities of the points on the linkages A.set_vel(inertial_frame, 0) B.v2pt_theory(A, inertial_frame, a_frame) B.vel(inertial_frame) C.v2pt_theory(B, inertial_frame, b_frame) C.vel(inertial_frame) D.v2pt_theory(C, inertial_frame, c_frame) D.vel(inertial_frame) #Sets up the masses of the linkages
from sympy.physics.mechanics import ReferenceFrame, Point, dynamicsymbols from util import msprint, partial_velocities, generalized_active_forces ## --- Declare symbols --- u1, u2, u3, u4, u5, u6, u7, u8, u9 = dynamicsymbols('u1:10') c, R = symbols('c R') x, y, z, r, phi, theta = symbols('x y z r phi theta') # --- Reference Frames --- A = ReferenceFrame('A') B = ReferenceFrame('B') C = ReferenceFrame('C') B.set_ang_vel(A, u1 * B.x + u2 * B.y + u3 * B.z) C.set_ang_vel(A, u4 * B.x + u5 * B.y + u6 * B.z) C.set_ang_vel(B, C.ang_vel_in(A) - B.ang_vel_in(A)) pC_star = Point('C*') pC_star.set_vel(C, 0) # since radius of cavity is very small, assume C* has zero velocity in B pC_star.set_vel(B, 0) pC_star.set_vel(A, u7 * B.x + u8*B.y + u9*B.z) ## --- define points P, P' --- # point on C pP = pC_star.locatenew('P', x * B.x + y * B.y + z * B.z) pP.set_vel(C, 0) pP.v2pt_theory(pC_star, B, C) pP.v2pt_theory(pC_star, A, C) # point on B pP_prime = pP.locatenew("P'", 0)
class YAMSBody(object): def __init__(self, name): """ Origin point have no velocities in the body frame! """ self.frame = ReferenceFrame('e_' + name) self.origin = Point('O_' + name) self.masscenter = Point('G_' + name) self.name = name self.origin.set_vel(self.frame, 0 * self.frame.x) self.parent = None # Parent body, assuming a tree structure self.children = [] # children bodies self.inertial_frame = None # storing the typical inertial frame use for computation self.inertial_origin = None # storing the typical inertial frame use for computation def __repr__(self): s = '<{} object "{}" with attributes:>\n'.format( type(self).__name__, self.name) s += ' - origin: {}\n'.format(self.origin) s += ' - frame: {}\n'.format(self.frame) return s def ang_vel_in(self, frame_or_body): """ Angular velocity of body wrt to another frame or body This is just a wrapper for the ReferenceFrame ang_vel_in function """ if isinstance(frame_or_body, ReferenceFrame): return self.frame.ang_vel_in(frame_or_body) else: if issubclass(type(frame_or_body), YAMSBody): return self.frame.ang_vel_in(frame_or_body.frame) else: raise Exception( 'Unknown class type, use ReferenceFrame of YAMSBody as argument' ) def connectTo(parent, child, type='Rigid', rel_pos=None, rot_type='Body', rot_amounts=None, rot_order=None, dynamicAllowed=False): # register parent/child relationship child.parent = parent parent.children.append(child) if isinstance(parent, YAMSInertialBody): parent.inertial_frame = parent.frame child.inertial_frame = parent.frame parent.inertial_origin = parent.origin child.inertial_origin = parent.origin else: if parent.inertial_frame is None: raise Exception( 'Parent body was not connected to an inertial frame. Bodies needs to be connected in order, starting from inertial frame.' ) else: child.inertial_frame = parent.inertial_frame # the same frame is used for all connected bodies child.inertial_origin = parent.origin if rel_pos is None or len(rel_pos) != 3: raise Exception('rel_pos needs to be an array of size 3') pos = 0 * parent.frame.x vel = 0 * parent.frame.x t = dynamicsymbols._t if type == 'Free': # --- "Free", "floating" connection if not isinstance(parent, YAMSInertialBody): raise Exception( 'Parent needs to be inertial body for a free connection') # Defining relative position and velocity of child wrt parent for d, e in zip(rel_pos[0:3], (parent.frame.x, parent.frame.y, parent.frame.z)): if d is not None: pos += d * e vel += diff(d, t) * e elif type == 'Rigid': # Defining relative position and velocity of child wrt parent for d, e in zip(rel_pos[0:3], (parent.frame.x, parent.frame.y, parent.frame.z)): if d is not None: pos += d * e if exprHasFunction(d) and not dynamicAllowed: raise Exception( 'Position variable cannot be a dynamic variable for a rigid connection: variable {}' .format(d)) if dynamicAllowed: vel += diff(d, t) * e elif type == 'Joint': # Defining relative position and velocity of child wrt parent for d, e in zip(rel_pos[0:3], (parent.frame.x, parent.frame.y, parent.frame.z)): if d is not None: pos += d * e if exprHasFunction(d) and not dynamicAllowed: raise Exception( 'Position variable cannot be a dynamic variable for a joint connection, variable: {}' .format(d)) if dynamicAllowed: vel += diff(d, t) * e # Orientation if rot_amounts is None: raise Exception( 'rot_amounts needs to be provided with Joint connection') for d in rot_amounts: if d != 0 and not exprHasFunction(d): raise Exception( 'Rotation amount variable should be a dynamic variable for a joint connection, variable: {}' .format(d)) else: raise Exception('Unsupported joint type: {}'.format(type)) # Orientation (creating a path connecting frames together) if rot_amounts is None: child.frame.orient(parent.frame, 'Axis', (0, parent.frame.x)) else: if rot_type in ['Body', 'Space']: child.frame.orient(parent.frame, rot_type, rot_amounts, rot_order) # <<< else: child.frame.orient(parent.frame, rot_type, rot_amounts) # <<< # Position of child origin wrt parent origin child.origin.set_pos(parent.origin, pos) # Velocity of child origin frame wrt parent frame (0 for rigid or joint) child.origin.set_vel(parent.frame, vel) # Velocity of child masscenter wrt parent frame, based on origin vel (NOTE: for rigid body only, should be overriden for flexible body) child.masscenter.v2pt_theory(child.origin, parent.frame, child.frame) # Velocity of child origin wrt inertial frame, using parent origin/frame as intermediate child.origin.v1pt_theory(parent.origin, child.inertial_frame, parent.frame) # Velocity of child masscenter wrt inertial frame, using parent origin/frame as intermediate child.masscenter.v1pt_theory(parent.origin, child.inertial_frame, parent.frame) #r_OB = child.origin.pos_from(child.inertial_origin) #vel_OB = r_OB.diff(t, child.inertial_frame) # --------------------------------------------------------------------------------} # --- Visualization # --------------------------------------------------------------------------------{ def vizOrigin(self, radius=1.0, color='black', format='pydy'): if format == 'pydy': from pydy.viz.shapes import Sphere from pydy.viz.visualization_frame import VisualizationFrame return VisualizationFrame(self.frame, self.origin, Sphere(color=color, radius=radius)) def vizCOG(self, radius=1.0, color='red', format='pydy'): if format == 'pydy': from pydy.viz.shapes import Sphere from pydy.viz.visualization_frame import VisualizationFrame return VisualizationFrame(self.frame, self.masscenter, Sphere(color=color, radius=radius)) 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 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 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()
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) #Sets up the linear velocities of the points on the linkages #one.set_vel(inertial_frame, 0) two.v2pt_theory(one, inertial_frame, one_frame) two.vel(inertial_frame) three.v2pt_theory(two, inertial_frame, two_frame) three.vel(inertial_frame) #Sets up the masses of the linkages one_mass, two_mass = symbols('m_1, m_2') #Defines the linkages as particles twoP = Particle('twoP', two, one_mass)
pO = Point('O') pS_star = pO.locatenew('S*', b*A.y) pS_hat = pS_star.locatenew('S^', -r*B.y) # S^ touches the cone pS1 = pS_star.locatenew('S1', -r*A.z) # S1 touches horizontal wall of the race pS2 = pS_star.locatenew('S2', r*A.y) # S2 touches vertical wall of the race pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) # Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) # Since S is rolling against C, v_S^_C = 0. # Cone has only angular velocity in R.z direction. vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A] vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc_map = solve(vc, u) # Pure rolling between S and C, dot(ω_C_S, B.y) = 0. b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0] print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r)))) b_expected = r*(1 + sin(theta))/(cos(theta) - sin(theta)) assert trigsimp(b_val - b_expected) == 0
# Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] # Since S is rolling against C, v_S^_C = 0. pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) vc += [dot(pS_hat.vel(C), basis) for basis in A] # Cone has only angular velocity ω in R.z direction. vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc += [omega - dot(C.ang_vel_in(R), R.z)] vc_map = solve(vc, u) # cone rigidbody I_C = inertia(A, I11, I22, J) rbC = RigidBody('rbC', pO, C, M, (I_C, pO)) # sphere rigidbody I_S = inertia(A, 2*m*r**2/5, 2*m*r**2/5, 2*m*r**2/5) rbS = RigidBody('rbS', pS_star, S, m, (I_S, pS_star)) # kinetic energy K = radsimp(expand((rbC.kinetic_energy(R) + 4*rbS.kinetic_energy(R)).subs(vc_map))) print('K = {0}'.format(msprint(collect(K, omega**2/2))))
from __future__ import print_function, division from sympy import symbols, simplify from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point from sympy.physics.vector import init_vprinting init_vprinting(use_latex='mathjax', pretty_print=False) inertial_frame = ReferenceFrame('I') lower_leg_frame = ReferenceFrame('L') theta1, theta2, theta3 = dynamicsymbols('theta1, theta2, theta3') lower_leg_frame.orient(inertial_frame, 'Axis', (theta1, inertial_frame.z)) lower_leg_frame.dcm(inertial_frame) omega1, omega2, omega3 = dynamicsymbols('omega1, omega2, omega3') lower_leg_frame.set_ang_vel(inertial_frame, omega1 * inertial_frame.z) print(lower_leg_frame.ang_vel_in(inertial_frame))
# Define kinematical differential equations # ========================================= omega1_dp1, omega2_dp1, omega1_dp2, omega2_dp2 = dynamicsymbols("omega_1dp1, omega_2dp1, omega_1dp2, omega_2dp2") time = symbols("t") kinematical_differential_equations_dp1 = [omega1_dp1 - theta1_dp1.diff(time), omega2_dp1 - theta2_dp2.diff(time)] kinematical_differential_equations_dp2 = [omega1_dp2 - theta1_dp2.diff(time), omega2_dp2 - theta2_dp2.diff(time)] # Angular Velocities # ================== one_frame_dp1.set_ang_vel(inertial_frame_dp1, omega1_dp1 * inertial_frame_dp1.z) one_frame_dp1.ang_vel_in(inertial_frame_dp1) one_frame_dp2.set_ang_vel(inertial_frame_dp2, omega1_dp2 * inertial_frame_dp2.z) one_frame_dp2.ang_vel_in(inertial_frame_dp2) two_frame_dp1.set_ang_vel(one_frame_dp1, omega2_dp1 * one_frame_dp1.z - omega1_dp2 * one_frame_dp1.z) two_frame_dp1.ang_vel_in(inertial_frame_dp1) two_frame_dp2.set_ang_vel(one_frame_dp2, omega2_dp2 * one_frame_dp2.z) two_frame_dp2.ang_vel_in(inertial_frame_dp2) # Linear Velocities # ================= one_dp1.set_vel(inertial_frame_dp1, 0) one_dp2.set_vel(inertial_frame_dp2, two_dp1.vel)
pS_star = pO.locatenew('S*', b * A.y) pS_hat = pS_star.locatenew('S^', -r * B.y) # S^ touches the cone pS1 = pS_star.locatenew('S1', -r * A.z) # S1 touches horizontal wall of the race pS2 = pS_star.locatenew('S2', r * A.y) # S2 touches vertical wall of the race pO.set_vel(R, 0) pS_star.v2pt_theory(pO, R, A) pS1.v2pt_theory(pS_star, R, S) pS2.v2pt_theory(pS_star, R, S) # Since S is rolling against R, v_S1_R = 0, v_S2_R = 0. vc = [dot(p.vel(R), basis) for p in [pS1, pS2] for basis in R] pO.set_vel(C, 0) pS_star.v2pt_theory(pO, C, A) pS_hat.v2pt_theory(pS_star, C, S) # Since S is rolling against C, v_S^_C = 0. # Cone has only angular velocity in R.z direction. vc += [dot(pS_hat.vel(C), basis).subs(vc_map) for basis in A] vc += [dot(C.ang_vel_in(R), basis) for basis in [R.x, R.y]] vc_map = solve(vc, u) # Pure rolling between S and C, dot(ω_C_S, B.y) = 0. b_val = solve([dot(C.ang_vel_in(S), B.y).subs(vc_map).simplify()], b)[0][0] print('b = {0}'.format(msprint(collect(cancel(expand_trig(b_val)), r)))) b_expected = r * (1 + sin(theta)) / (cos(theta) - sin(theta)) assert trigsimp(b_val - b_expected) == 0