% MotionGenesis file: MGAircraftTrifilarPendulumDynamicsKaneEmbedded.txt % Copyright (c) 2016-18 Motion Genesis LLC. %-------------------------------------------------------------------- SetAutoZ( OFF ) NewtonianFrame N % Earth / aircraft hanger. RigidBody C % Aircraft. Point N1(N), N2(N), N3(N) % End-points of cable on N. Point C1(C), C2(C), C3(C) % End-points of cable on C. %-------------------------------------------------------------------- Constant L1 = 30 m % Length of cable between N1 and C1. Constant L2 = 30 m % Length of cable between N2 and C2. Constant L3 = 30 m % Length of cable between N3 and C3. Constant dN = 30 m % Distance between No and N1. Constant dC = 30 m % Distance between Co and C1. Constant wN = 20 m % Distance between No and N2. Constant wC = 20 m % Distance between Co and C2. Constant dcm = 8 m % Distance between Co and Ccm. Constant g = 9.8 m/s^2 % Earth's gravitational acceleration. C.SetMassInertia( m = 9000 kg, Ixx = 4.0E5 kg*m^2, Iyy = 3.0E5 kg*m^2, Izz = Ixx + Iyy ) %-------------------------------------------------------------------- Constant bT = 8.0E3 noUnits % Torque damping constant. Constant bF = 6.0E3 noUnits % Force damping constant. Constant epsilonV = 1.0E-5 m/s % Small number to avoid divide by zero errors. Constant epsilonW = 1.0E-5 rad/s % Small number to avoid divide by zero errors. Constant expDamping = 0.25 noUnits % Exponent used with damping force/torques. %-------------------------------------------------------------------- Variable x'', y'', z'' % Locates Ccm from No. Variable q1', q2', q3' % BodyZYX Euler angles. Variable wx', wy', wz' % Cxyz> measures of C's angular velocity in N. SetGeneralizedSpeeds( x', y', wz ) %-------------------------------------------------------------------- % Rotational kinematics. C.SetAngularVelocityAcceleration( N, wx*Cx> + wy*Cy> + wz*Cz> ) C.SetRotationMatrixODE( N, BodyZYX, q1, q2, q3 ) %-------------------------------------------------------------------- % Translational kinematics. CCm.Translate( No, x*Nx> + y*Ny> + z*Nz> ) Co.SetPosition( CCm, -dcm*Cx> ) C1.SetPosition( Co, dC*Cx> ) C2.SetPosition( Co, -wC*Cy> ) C3.SetPosition( Co, wC*Cy> ) N1.SetPosition( No, dN*Nx> ) N2.SetPosition( No, -wN*Ny> ) N3.SetPosition( No, wN*Ny> ) %-------------------------------------------------------------------- % Configuration constraints: Length of cables. CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2 CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2 CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2 %-------------------------------------------------------------------- % Embed constraints so tensions do not appear in statics equations. % Need to identify useful subset of generalized speed. What worked % (from looking at system and using physical insight) was: x', y', wz % Hence, need to solve z', wx, wy in terms of x', y', wz. SolveDt( Dt(CableConstraint) = 0, z', wx, wy ) %-------------------------------------------------------------------- % Add forces from gravity. It is unnecessary to add tension because tension does % not contribute to generalized forces (force is perpendicular to velocity). Ccm.AddForce( m*g*Nz> ) %-------------------------------------------------------------------- % Add force to damp translational motion. vMag = Ccm.GetSpeed( N ) unitVectorVelocity> = CCm.GetVelocity( N ) / ( vMag + epsilonV ) dampingForce> = -bF * vMag^expDamping * unitVectorVelocity> CCm.AddForce( dampingForce> ) %-------------------------------------------------------------------- % Add torque to damp rotational motion. wMag = C.GetAngularSpeed( N ) unitVectorAngularVelocity> = C.GetAngularVelocity( N ) / ( wMag + epsilonW ) dampingTorque> = -bT * wMag^expDamping * unitVectorAngularVelocity> C.AddTorque( dampingTorque> ) %-------------------------------------------------------------------- % Form dynamic equations with Kane's method. Dynamics = System.GetDynamicsKane() %-------------------------------------------------------------------- % Integration parameters and initial values for variables. Input tFinal = 120 sec, tStep = 0.05 sec, absError = 1.0E-07 Input x' = 0 m/s, y' = 0 m/s, wz = 0 rad/sec %-------------------------------------------------------------------- % Calculate yaw, pitch and roll angles in terms of q1, q2, q3. Yaw = GetAngleBetweenUnitVectors( Nx>, Cy> ) - pi/2 Pitch = GetAngleBetweenUnitVectors( Nz>, Cx> ) - pi/2 Roll = pi/2 - GetAngleBetweenUnitVectors( Nz>, Cy> ) %-------------------------------------------------------------------- % Solve for initial values of q1, q2, q3 via initial yaw, pitch, roll angles. Constant Yaw0 = 60 deg, Pitch0 = 0 deg, Roll0 = 0 deg SolveSetInput( [Yaw; Pitch; Roll] = [Yaw0; Pitch0; Roll0], q1 = 60 deg, q2 = 0 deg, q3 = 0 deg ) %-------------------------------------------------------------------- % Solve for initial values of x, y, z using the CableConstraint (and initial values of q1, q2, q3). SolveSetInput( CableConstraint = 0, x = Input(dN)/2 m, y=0 m, z = 0.8*Input(L1) m ) %-------------------------------------------------------------------- % List output quantities and solve ODEs. Distance = Co.GetDistance( No ) Output t sec, Distance m, Yaw deg, Pitch deg, Roll deg, vMag m/s, wMag rad/sec, CableConstraint[1] m, CableConstraint[2] m, CableConstraint[3] m Output t sec, x m, y m, z m, x' m/s, y' m/s, z' m/s, q1 deg, q2 deg, q3 deg, wx rad/sec, wy rad/sec, wz rad/sec ODE( Dynamics = 0, x'', y'', wz' ) MGAircraftTrifilarPendulumDynamicsKaneEmbedded.m %-------------------------------------------------------------------- Save MGAircraftTrifilarPendulumDynamicsKaneEmbedded.html Quit