% File: BabybootStatic.al %-------------------------------------------------------------------- % Set up problem NewtonianFrame N % Fixed reference frame RigidBody A % Upper rod RigidBody B % Lower plate Point P % Attachment point of plate to rod Variable qA'' % Pendulum angle and its time-derivatives Variable qB'' % Plate angle and its time-derivative Variable FxA, FyA, FzA, TyA, TzA % Reactions on A at No Variable FxB, FyB, FzB, TxB, TyB % Reactions on B at P Constant mA, mB % Mass of A and B Constant IxxA, IyyA, IzzA Constant IxxB, IyyB, IzzB Constant L % Length of upper rod Constant H,W,D % Height, width, and depth of lower plate Constant g % Gravity Constant k,qBo % Spring stiffness and resting angle AutoOverwrite On %-------------------------------------------------------------------- % Rotation matrices A.RotateX( N, qA ) % A rotates "about +x" in N by qA B.RotateZ( A, qB ) % B rotates "about +z" in A by qB %-------------------------------------------------------------------- % Angular velocities - start from N and work outward w_A_N> = qA'*Ax> w_B_A> = qB'*Az> w_B_N> = w_A_N> + w_B_A> Express( w_B_N>, B ) %-------------------------------------------------------------------- % Position vectors p_No_Acm> = -0.5*L*Az> p_No_Bcm> = -(L+0.5*H)*Az> %-------------------------------------------------------------------- % Velocities - start from N and work outward v_No_N> = 0> v_Acm_N> = v_No_N> + Cross( w_A_N>, p_No_Acm> ) % Two points fixed v_Bcm_N> = Dt( p_No_Bcm>, A ) + Cross( w_A_N>, p_No_Bcm> ) % Golden rule Express( v_Bcm_N>, B ) %-------------------------------------------------------------------- % Angular momenta I_A_Acm>> = IxxA*Ax>*Ax> + IyyA*Ay>*Ay> + IzzA*Az>*Az> IxxB = (1/12)*mB*(W^2+H^2) IyyB = (1/12)*mB*(H^2+D^2) IzzB = (1/12)*mB*(W^2+D^2) I_B_Bcm>> = IxxB*Bx>*Bx> + IyyB*By>*By> + IzzB*Bz>*Bz> H_A_Acm_N> = Dot( I_A_Acm>>, w_A_N> ) H_B_Bcm_N> = Dot( I_B_Bcm>>, w_B_N> ) %-------------------------------------------------------------------- % Linear momenta L_A_N> = mA*v_Acm_N> L_B_N> = mB*v_Bcm_N> %-------------------------------------------------------------------- % Moments - start from B and work inward F_B_Bcm> = -mB*g*Nz> F_A_Acm> = -mA*g*Nz> T_B> = TxB*Bx> + TyB*By> - k*(qB-qBo)*Bz> T_A> = TyA*Ny> + TzA*Nz> - T_B> p_Bcm_P> = 0.5*H*Bz> M_B_P> = Cross( p_P_Bcm>, F_B_Bcm> ) + T_B> M_AB_No> = Cross( p_No_Acm>, F_A_Acm> ) + Cross( p_No_Bcm>, F_B_Bcm> ) + T_A> + T_B> %-------------------------------------------------------------------- % Angular momentum principle - start from B and work inward % Zero_Ang_B> = M_B_P> - Dt( H_B_Bcm_N>, N ) - Cross( p_P_Bcm>, Dt( L_B_N>, N ) ) D_H_B_Bcm_N> = Dt( H_B_Bcm_N>, B ) + Cross( w_B_N>, H_B_Bcm_N> ) % Golden rule D_L_B_N> = Dt( L_B_N>, B ) + Cross( w_B_N>, L_B_N> ) % Golden rule Zero_Ang_B> = M_B_P> - D_H_B_Bcm_N> - Cross( p_P_Bcm>, D_L_B_N> ) % Zero_Ang_AB> = M_AB_No> - Dt( H_A_Acm_N>, N ) - Cross( p_No_Acm>, Dt( L_A_N>, N ) ) - & % Dt( H_B_Bcm_N>, N ) - Cross( p_No_Bcm>, Dt( L_B_N>, N ) ) D_H_A_Acm_N> = Dt( H_A_Acm_N>, A ) + Cross( w_A_N>, H_A_Acm_N> ) % Golden rule D_L_A_N> = Dt( L_A_N>, A ) + Cross( w_A_N>, L_A_N> ) % Golden rule Zero_Ang_AB> = M_AB_No> - D_H_A_Acm_N> - Cross( p_No_Acm>, D_L_A_N> ) - & D_H_B_Bcm_N> - Cross( p_No_Bcm>, D_L_B_N> ) %-------------------------------------------------------------------- % Scalar dynamics equations - start from B and work inward Zero[1] = Dot( Zero_Ang_B>, Bz> ) Zero[2] = Dot( Zero_Ang_AB>, Ax> ) %-------------------------------------------------------------------- % Scalar nonlinear statics equations - set q' and q'' terms to zero ZeroStatic = Evaluate( Zero, qA' = 0, qB' = 0, qA'' = 0, qB'' = 0 ) %-------------------------------------------------------------------- % Solution of nonlinear algebraic equations for qA and qB % Use mNs units UnitSystem kg, m, sec Input L = 0.3, H = 0.1, W = 0.2, D = 0.01 Input g = 9.81 Input mA = 0.1, mB = 0.5 Input IxxA = 0, IyyA = 0, IzzA = 0 Input k = 100.0, qBo = 5.0 deg Input absErr = 1.0e-9 SolveNonlinear( ZeroStatic, qA = 60.0*pi/180.0, qB = 60.0*pi/180.0 ) qBdeg = Explicit(qB*180/pi)