% 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