MGAircraftTrifilarPendulumDynamicsKaneEmbedded.html  (MotionGenesis input/output).
```   (1) % MotionGenesis file:  MGAircraftTrifilarPendulumDynamicsKaneEmbedded.txt
(2) % Copyright (c) 2016-18 Motion Genesis LLC.
(3) %--------------------------------------------------------------------
(4) SetAutoZ( OFF )
(5) NewtonianFrame  N                    % Earth / aircraft hanger.
(6) RigidBody       C                    % Aircraft.
(7) Point           N1(N), N2(N), N3(N)  % End-points of cable on N.
(8) Point           C1(C), C2(C), C3(C)  % End-points of cable on C.
(9) %--------------------------------------------------------------------
(10) Constant  L1 = 30 m                  % Length of cable between N1 and C1.
(11) Constant  L2 = 30 m                  % Length of cable between N2 and C2.
(12) Constant  L3 = 30 m                  % Length of cable between N3 and C3.
(13) Constant  dN = 30 m                  % Distance between No and N1.
(14) Constant  dC = 30 m                  % Distance between Co and C1.
(15) Constant  wN = 20 m                  % Distance between No and N2.
(16) Constant  wC = 20 m                  % Distance between Co and C2.
(17) Constant dcm =  8 m                  % Distance between Co and Ccm.
(18) Constant  g = 9.8 m/s^2              % Earth's gravitational acceleration.
(19) C.SetMassInertia( m = 9000 kg, Ixx = 4.0E5 kg*m^2,  Iyy = 3.0E5 kg*m^2,  Izz = Ixx + Iyy )
-> (20) Izz = Ixx + Iyy

(21) %--------------------------------------------------------------------
(22) Constant  bT = 8.0E3 noUnits         % Torque damping constant.
(23) Constant  bF = 6.0E3 noUnits         % Force  damping constant.
(24) Constant  epsilonV   = 1.0E-5 m/s    % Small number to avoid divide by zero errors.
(25) Constant  epsilonW   = 1.0E-5 rad/s  % Small number to avoid divide by zero errors.
(26) Constant  expDamping = 0.25 noUnits  % Exponent used with damping force/torques.
(27) %--------------------------------------------------------------------
(28) Variable  x'',  y'',  z''            % Locates Ccm from No.
(29) Variable  q1',  q2',  q3'            % BodyZYX Euler angles.
(30) Variable  wx',  wy',  wz'            % Cxyz> measures of C's angular velocity in N.
(31) SetGeneralizedSpeeds(  x',  y',  wz  )
(32) %--------------------------------------------------------------------
(33) %   Rotational kinematics.
(34) C.SetAngularVelocityAcceleration(  N,  wx*Cx> + wy*Cy> + wz*Cz>  )
-> (35) w_C_N> = wx*Cx> + wy*Cy> + wz*Cz>
-> (36) alf_C_N> = wx'*Cx> + wy'*Cy> + wz'*Cz>

(37) C.SetRotationMatrixODE( N, BodyZYX, q1, q2, q3 )
-> (38) C_N[1,1] = cos(q1)*cos(q2)
-> (39) C_N[1,2] = sin(q1)*cos(q2)
-> (40) C_N[1,3] = -sin(q2)
-> (41) C_N[2,1] = sin(q2)*sin(q3)*cos(q1) - sin(q1)*cos(q3)
-> (42) C_N[2,2] = cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)
-> (43) C_N[2,3] = sin(q3)*cos(q2)
-> (44) C_N[3,1] = sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3)
-> (45) C_N[3,2] = sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1)
-> (46) C_N[3,3] = cos(q2)*cos(q3)
-> (47) q1' = (wy*sin(q3)+cos(q3)*wz)/cos(q2)
-> (48) q2' = wy*cos(q3) - sin(q3)*wz
-> (49) q3' = wx + tan(q2)*(wy*sin(q3)+cos(q3)*wz)

(50) %--------------------------------------------------------------------
(51) %   Translational kinematics.
(52) CCm.Translate( No, x*Nx> + y*Ny> + z*Nz> )
-> (53) p_No_Ccm> = x*Nx> + y*Ny> + z*Nz>
-> (54) v_Ccm_N> = x'*Nx> + y'*Ny> + z'*Nz>
-> (55) a_Ccm_N> = x''*Nx> + y''*Ny> + z''*Nz>

(56) Co.SetPosition( CCm, -dcm*Cx> )
-> (57) p_Ccm_Co> = -dcm*Cx>

(58) C1.SetPosition( Co,    dC*Cx> )
-> (59) p_Co_C1> = dC*Cx>

(60) C2.SetPosition( Co,   -wC*Cy> )
-> (61) p_Co_C2> = -wC*Cy>

(62) C3.SetPosition( Co,    wC*Cy> )
-> (63) p_Co_C3> = wC*Cy>

(64) N1.SetPosition( No,    dN*Nx> )
-> (65) p_No_N1> = dN*Nx>

(66) N2.SetPosition( No,   -wN*Ny> )
-> (67) p_No_N2> = -wN*Ny>

(68) N3.SetPosition( No,    wN*Ny> )
-> (69) p_No_N3> = wN*Ny>

(70) %--------------------------------------------------------------------
(71) %   Configuration constraints: Length of cables.
(72) CableConstraint[1] = C1.GetDistanceSquared( N1 ) - L1^2
-> (73) CableConstraint[1] = (dC-dcm)^2 + y^2 + z^2 + (dN-x)^2 + 2*(dC-dcm)*y*
sin(q1)*cos(q2) - L1^2 - 2*(dC-dcm)*z*sin(q2) - 2*(dC-dcm)*cos(q1)*cos(
q2)*(dN-x)

(74) CableConstraint[2] = C2.GetDistanceSquared( N2 ) - L2^2
-> (75) CableConstraint[2] = dcm^2 + wC^2 + x^2 + z^2 + (wN+y)^2 + 2*dcm*z*sin(q2)
+ 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)) - L2^2 - 2*dcm*x*cos
(q1)*cos(q2) - 2*wC*z*sin(q3)*cos(q2) - 2*dcm*sin(q1)*cos(q2)*(wN+y)
- 2*wC*(wN+y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

(76) CableConstraint[3] = C3.GetDistanceSquared( N3 ) - L3^2
-> (77) CableConstraint[3] = dcm^2 + wC^2 + x^2 + z^2 + (wN-y)^2 + 2*dcm*z*sin(q2)
+ 2*wC*z*sin(q3)*cos(q2) + 2*dcm*sin(q1)*cos(q2)*(wN-y) - L3^2 - 2*dcm*
x*cos(q1)*cos(q2) - 2*wC*x*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))
- 2*wC*(wN-y)*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))

(78) %--------------------------------------------------------------------
(79) %   Embed constraints so tensions do not appear in statics equations.
(80) %   Need to identify useful subset of generalized speed.  What worked
(81) %   (from looking at system and using physical insight) was:  x', y', wz
(82) %   Hence, need to solve  z', wx, wy  in terms of  x', y', wz.
(83) SolveDt(  Dt(CableConstraint) = 0,   z',  wx,  wy )
-> (84) z' = -((dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*
cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos
(q3)*wz+wC*x*cos(q1)*cos(q2)*wz+wC*sin(q1)*cos(q2)*(wN+y)*wz+wC*(sin(
q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-wC*z*sin(q2)*wz-dcm*sin(q1)*cos
(q2)*y'-dcm*cos(q1)*cos(q2)*x'-dcm*cos(q1)*cos(q3)*(wN+y)*wz-wC*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz)+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*
(x*x'+wC*z*sin(q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-
y)*wz+wC*sin(q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*
sin(q3))*y'-(wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*
x*cos(q1)*cos(q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-
dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz)+
dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(y*y'+(dC-dcm)*sin(q1)
*cos(q2)*y'+(dC-dcm)*cos(q1)*cos(q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(
dC-dcm)*z*sin(q3)*cos(q2)*wz+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-
dcm)*sin(q1)*cos(q3)*(dN-x)*wz-(dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(
q1)*(dN-x)*wz))/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos
(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)
-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))

-> (85) wx = -(dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos
(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos
(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(y*y'+(dC-dcm)*sin(q1)*cos(q2)*y'+
(dC-dcm)*cos(q1)*cos(q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(dC-dcm)*z*
sin(q3)*cos(q2)*wz+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-dcm)*sin(
q1)*cos(q3)*(dN-x)*wz-(dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)
*wz)-(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*
(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x)))*(x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos(q3)*wz+
wC*x*cos(q1)*cos(q2)*wz+wC*sin(q1)*cos(q2)*(wN+y)*wz+wC*(sin(q1)*cos(
q3)-sin(q2)*sin(q3)*cos(q1))*x'-wC*z*sin(q2)*wz-dcm*sin(q1)*cos(q2)*y'-
dcm*cos(q1)*cos(q2)*x'-dcm*cos(q1)*cos(q3)*(wN+y)*wz-wC*(cos(q1)*cos(
q3)+sin(q1)*sin(q2)*sin(q3))*y'-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y))*wz)-(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)
-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x*x'+wC*z*sin
(q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-y)*wz+wC*sin(
q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-(
wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*x*cos(q1)*cos
(q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-dcm*sin(q3)*(z*
cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz))/(wC*(dcm*(z-(dC-
dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))
-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x)))))

-> (86) wy = -((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
(x*x'+(wN+y)*y'+dcm*x*sin(q1)*cos(q3)*wz+wC*x*cos(q1)*cos(q2)*wz+wC*sin
(q1)*cos(q2)*(wN+y)*wz+wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-
wC*z*sin(q2)*wz-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-dcm*cos(
q1)*cos(q3)*(wN+y)*wz-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y'-
dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz)+(z
-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*x'+
wC*z*sin(q2)*wz+dcm*x*sin(q1)*cos(q3)*wz+dcm*cos(q1)*cos(q3)*(wN-y)*wz+
wC*sin(q1)*cos(q2)*(wN-y)*wz+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(
q3))*y'-(wN-y)*y'-dcm*sin(q1)*cos(q2)*y'-dcm*cos(q1)*cos(q2)*x'-wC*x*
cos(q1)*cos(q2)*wz-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x'-dcm*
sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz)-((z+
dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3))))*(y*y'+(dC-dcm)*sin(q1)*cos(q2)*y'+(dC-dcm)*cos(q1)*cos(
q2)*x'+(dC-dcm)*y*cos(q1)*cos(q3)*wz+(dC-dcm)*z*sin(q3)*cos(q2)*wz+(dC-
dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz-(
dN-x)*x'-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)*wz))/(dcm*(z-(dC-dcm)*
sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z
*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-
(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))

-> (87) z'' = -(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-
dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-
dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-
dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos
(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*
y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*
q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin
(q3)*(dN-x)*wz*q1'+y*y''+(dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*
cos(q2)*x''+(dC-dcm)*y*cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*
wz'+(dC-dcm)*y*sin(q1)*sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(
dN-x)*wz'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*
q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(
dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-
dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(
dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*
cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*
cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(
q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin
(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(
q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*
cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-
sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*
x''-(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*(dN-x)*wz')+(dC-dcm)*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos
(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)
*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin
(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)
*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-
wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(
q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)
*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*
sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)
*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(
q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(
q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm*x*sin(
q1)*cos(q3)*wz'+wC*x*cos(q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+y)*wz'+
wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos(q3)*
wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-
dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*
cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+
x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*
(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(
q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(
q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)
*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(
q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+
y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(
q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(
q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*
q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*
(wN+y)*q1')-wC*z*sin(q2)*wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(
q2)*x''-dcm*cos(q1)*cos(q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
q2)*sin(q3))*y''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(
q2)*(wN+y))*wz')+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin
(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(
q3)*wz*x'+wC*sin(q2)*wz*z'+wC*z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*
q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)
*cos(q3)*wz*q1'+wC*x*sin(q1)*cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'
+wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(
q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos
(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(
q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*
cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-
sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+
x*x''+wC*z*sin(q2)*wz'+dcm*x*sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(
wN-y)*wz'+wC*sin(q1)*cos(q2)*(wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin
(q2)*sin(q3))*y''-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-
wC*cos(q1)*cos(q2)*wz*x'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(
q3)*wz*q3'-dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)
*wz*q3'-wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-
dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+
sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)
*q3'-sin(q3)*cos(q1)*cos(q2)*q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*
cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin
(q2)*sin(q3)*cos(q1)*q1')-dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin
(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*(
(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*
sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(
q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)
*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*
q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*
y''-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(
q2)*wz'-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z
*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz'))/(dcm*(z-(dC-
dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*
(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))
-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))))

-> (88) wx' = -(dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)
*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*
cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*
sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*
cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*
wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin
(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*
cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*
wz*q1'+y*y''+(dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*cos(q2)*x''+
(dC-dcm)*y*cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*wz'+(dC-dcm)*
y*sin(q1)*sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz'-(dC-
dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin
(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*
cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin
(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos
(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*
q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*
cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*
cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(
q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-
x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*x''-(dC-dcm)*sin(q2)*
sin(q3)*cos(q1)*(dN-x)*wz')-(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin
(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin
(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+
z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+
wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*
q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin
(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(
q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*
sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*
x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*
cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm*x*sin(q1)*cos(q3)*wz'+wC*x*cos
(q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+y)*wz'+wC*(sin(q1)*cos(q3)-sin(
q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*
z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*
q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*
sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*
sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*
q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'
-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(
q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)
*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'
+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos
(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*
q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(
q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-
sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(
q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1')-wC*z*sin(q2)
*wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*cos(q2)*x''-dcm*cos(q1)*cos(
q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*sin
(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz')-(dcm*(z-
(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q2)*
wz*z'+wC*z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+wC*x
*sin(q1)*cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'+wC*cos(q1)*cos(q2)*
(wN-y)*wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin
(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(
q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*
q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos
(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*
sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*
(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+x*x''+wC*z*sin(q2)*wz'+
dcm*x*sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(wN-y)*wz'+wC*sin(q1)*cos
(q2)*(wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*
cos(q1)*cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-wC*cos(q1)*cos(q2)*wz*
x'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-dcm*sin(q1)*
cos(q3)*(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'-wC*sin(q1)*sin(
q2)*(wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(
q2)*cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(
q2)*(wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*
q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos
(q2)*q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(
q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')
-dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos
(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*((sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(
q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'
-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+
sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)
*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*y''-dcm*sin(q1)*cos(q2)*y''-
dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(q2)*wz'-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-
sin(q1)*sin(q2)*(wN-y))*wz'))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos
(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*
(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-
(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)
-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))))

-> (89) wy' = (((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)
+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(
dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*
wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+
z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)
*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*
wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(
q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'+y*y''+
(dC-dcm)*sin(q1)*cos(q2)*y''+(dC-dcm)*cos(q1)*cos(q2)*x''+(dC-dcm)*y*
cos(q1)*cos(q3)*wz'+(dC-dcm)*z*sin(q3)*cos(q2)*wz'+(dC-dcm)*y*sin(q1)*
sin(q2)*sin(q3)*wz'+(dC-dcm)*sin(q1)*cos(q3)*(dN-x)*wz'-(dC-dcm)*sin(
q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(
q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*
wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*
q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos
(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-
dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*
cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*
cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)
*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2')-(dN-x)*x''-(dC-dcm)*sin(q2)*sin(q3)
*cos(q1)*(dN-x)*wz')-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*
wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*
cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+
dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)
*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)
*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin
(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(
q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(
q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')+x*x''+(wN+y)*y''+dcm
*x*sin(q1)*cos(q3)*wz'+wC*x*cos(q1)*cos(q2)*wz'+wC*sin(q1)*cos(q2)*(wN+
y)*wz'+wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1))*x''-dcm*cos(q1)*cos
(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*
q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(
q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)
-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*
q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*
cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(
cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin
(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*
cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*
(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+
sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*
cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(
q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(
q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(
q3)*(wN+y)*q1')-wC*z*sin(q2)*wz'-dcm*sin(q1)*cos(q2)*y''-dcm*cos(q1)*
cos(q2)*x''-dcm*cos(q1)*cos(q3)*(wN+y)*wz'-wC*(cos(q1)*cos(q3)+sin(q1)*
sin(q2)*sin(q3))*y''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
sin(q2)*(wN+y))*wz')-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q2)*wz*z'+wC*
z*cos(q2)*wz*q2'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+
dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+wC*x*sin(q1)*
cos(q2)*wz*q1'+wC*x*sin(q2)*cos(q1)*wz*q2'+wC*cos(q1)*cos(q2)*(wN-y)*
wz*q1'+dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*
cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*
cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+
sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)
*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(
q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-
y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')+x*x''+wC*z*sin(q2)*wz'+dcm*x
*sin(q1)*cos(q3)*wz'+dcm*cos(q1)*cos(q3)*(wN-y)*wz'+wC*sin(q1)*cos(q2)*
(wN-y)*wz'+wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*y''-dcm*cos(q1)
*cos(q3)*wz*y'-wC*sin(q1)*cos(q2)*wz*y'-wC*cos(q1)*cos(q2)*wz*x'-dcm*
cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-dcm*sin(q1)*cos(q3)
*(wN-y)*wz*q1'-dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'-wC*sin(q1)*sin(q2)*(
wN-y)*wz*q2'-z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*
cos(q3)*q3')-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(
wN-y))*wz*q3'-wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-
sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)
*q2')-wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*
cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')-
dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(
q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')-wC*wx*((sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(
q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'
-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+
sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)
*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-(wN-y)*y''-dcm*sin(q1)*cos(q2)*y''-
dcm*cos(q1)*cos(q2)*x''-wC*x*cos(q1)*cos(q2)*wz'-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1))*x''-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-
sin(q1)*sin(q2)*(wN-y))*wz'))/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)
-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-
y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-
(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)
-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))

(90) %--------------------------------------------------------------------
(91) %   Add forces from gravity.  It is unnecessary to add tension because tension does
(92) %   not contribute to generalized forces (force is perpendicular to velocity).
-> (94) Force_Ccm> = m*g*Nz>

(95) %--------------------------------------------------------------------
(96) %   Add force to damp translational motion.
(97) vMag = Ccm.GetSpeed( N )
-> (98) vMag = sqrt(x'^2+y'^2+z'^2)

(99) unitVectorVelocity> = CCm.GetVelocity( N ) / ( vMag + epsilonV )
-> (100) unitVectorVelocity> = x'/(epsilonV+vMag)*Nx> + y'/(epsilonV+vMag)*Ny>
+ z'/(epsilonV+vMag)*Nz>

(101) dampingForce> = -bF *  vMag^expDamping * unitVectorVelocity>
-> (102) dampingForce> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag
^expDamping/(epsilonV+vMag)*Ny> - bF*z'*vMag^expDamping/(epsilonV+vMag)*Nz>

-> (104) Force_Ccm> = -bF*x'*vMag^expDamping/(epsilonV+vMag)*Nx> - bF*y'*vMag^expDamping
/(epsilonV+vMag)*Ny> + (m*g-bF*z'*vMag^expDamping/(epsilonV+vMag))*Nz>

(105) %--------------------------------------------------------------------
(106) %   Add torque to damp rotational motion.
(107) wMag = C.GetAngularSpeed( N )
-> (108) wMag = sqrt(wx^2+wy^2+wz^2)

(109) unitVectorAngularVelocity> = C.GetAngularVelocity( N ) / ( wMag + epsilonW )
-> (110) unitVectorAngularVelocity> = wx/(epsilonW+wMag)*Cx> + wy/(epsilonW+wM
ag)*Cy> + wz/(epsilonW+wMag)*Cz>

(111) dampingTorque> =  -bT *  wMag^expDamping * unitVectorAngularVelocity>
-> (112) dampingTorque> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*
wMag^expDamping/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+
wMag)*Cz>

-> (114) Torque_C> = -bT*wx*wMag^expDamping/(epsilonW+wMag)*Cx> - bT*wy*wMag^expDamping
/(epsilonW+wMag)*Cy> - bT*wz*wMag^expDamping/(epsilonW+wMag)*Cz>

(115) %--------------------------------------------------------------------
(116) %   Form dynamic equations with Kane's method.
(117) Dynamics = System.GetDynamicsKane()
-> (118) Dynamics[1] = bT*(wy*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(
sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(
q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*wMag
^expDamping/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))-wx*((
dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(
dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(
wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x
*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)
-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wMag^expDamping
/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+
x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(epsilonW+
wMag) + ((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(
q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(
q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*
cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(
q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(
q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*
((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(x'^2+y'^2+dcm*sin
(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'
+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')
+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(
q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)
*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*
q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*
wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin
(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-
wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
*(wN+y)*q1'))-dcm*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*
cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*
sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*
cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)
*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*
sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*
q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*
sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*
sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(
q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(
q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*
(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*
sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+
sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*
z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(
q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos
(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos
(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*
(dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(dcm*cos(q1)*cos(q3)*wz*
y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(
q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*
q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'
+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')
+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*
q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin
(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos
(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*
q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)
*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-
sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'
+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(
q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*
wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-
dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*
cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'
-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin
(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+
cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*
cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(
q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*
q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*
q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin
(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)
*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(wC*(
dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x
*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)
+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))) + bF*x'*vMag^expDamping
/(epsilonV+vMag) + (Iyy*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-
wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(
sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin
(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2
/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2+Ixx*((dcm*cos(q1)*
cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-
dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-
wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos
(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-dcm)*
sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+
z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin
(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z
*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
(q2)*cos(q1)*cos(q3)*(dN-x))))^2)+m*(1+((dC-dcm)*(x-dcm*cos(q1)*cos(
q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x
*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(
q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*(
(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2/(dcm*(z-(dC-dcm)*
sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+
z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin
(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z
*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
(q2)*cos(q1)*cos(q3)*(dN-x))))^2))*x'' + (m*((dC-dcm)*(x-dcm*cos(q1)*
cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*
sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*
cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*
sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(dcm*((z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos
(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*
cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*
sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*
cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)
*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*
sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-
dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(
dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*
y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'
-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(
dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*
q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*
cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*
sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin
(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(
q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(
dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*
q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(
q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-
x)*q2'))+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(
q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(
q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*
q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm
*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(
z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*
x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*
q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(
sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-
sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*
wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos
(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-
sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*
z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(
q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*
sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)
+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
*(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*cos(q1)*cos(q3)*
wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*
cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)
*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*
wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(
q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-
y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(
q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*
q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*
cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+
dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos
(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(
wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin
(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*
q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)
*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(
q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*
q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(
q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*
wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin
(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+
x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*
q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(
q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*
sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-
y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-
y)*q1')))+((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos
(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((Ixx-Izz)*wx*(
dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x
*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)
+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wz-Iyy*((z-(dC-dcm)*
sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(
q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(
q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*
q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'
+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')
+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(
q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*
y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*
q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(
q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)
*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*
q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*
wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin
(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-
wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin
(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*
q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*
cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z
*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+
x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*
sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*
y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'
-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*
cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)
*(wN+y)*q1'))-((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x
*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)
*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)
*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)
*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'
+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*
cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(
q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*
q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(
dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)
*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z
*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)
*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(
q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(
q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)
*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(
q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(
q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*
cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-
sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(
dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*cos(
q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+
dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*
cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin
(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*
cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*
sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(
q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(
q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(
q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos
(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(
q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos
(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(
q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin
(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin
(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(
cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-
sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-
dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin
(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*
q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*
sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(
q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*
cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)
*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)
*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(
q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(
q1)*cos(q3)*(wN-y)*q1'))))+(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))*((dC-dcm)*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin
(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*
cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin
(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))))*(m*g-bF*z'*vMag^expDamping/(epsilonV+vMag))-
(Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(
q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(
q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(
q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(
wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*
(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(
q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*
(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))))+Ixx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos
(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y
+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(
q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*
cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(
q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(
q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)
*cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*cos(
q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin
(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*
sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))/wC^2+m*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*
cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(
wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((dC-dcm)*(x-dcm*cos(q1)*cos(q2)
-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(
q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*(
(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*
(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))))*y''-(Iyy*((z-(dC-
dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)
*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*
cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-dcm)*
cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(dC-dcm)*((dC-dcm)*(x-
dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(
q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-
dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*
sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))
*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))))-Ixx*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)
+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-
dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*
cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)
*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-
x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin
(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(
dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*wz')/(dcm*(z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))^2

-> (119) Dynamics[2] = bF*y'*vMag^expDamping/(epsilonV+vMag) + (Iyy*((z-(dC-dcm)
*sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)
*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(
q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)
*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2/(dcm*(z-(dC-dcm)*sin(q2))
*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z
*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z
-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)
*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(
dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))))^2+Ixx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)
*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(
q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-
dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm
*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*
(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-
(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))^2)+m*(1+((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*
cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(
wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2/(dcm*(z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))^2))*y'' - bT*(wy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm
*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin
(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))))*wMag^expDamping/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))-wx*((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(
q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*
sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(
q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))*wMag^expDamping/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))))/(epsilonW+wMag) - ((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin
(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-
dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(wN+dcm
*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*
(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-
(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))))*((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*sin(
q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-dcm*((z+dcm*sin(q2)-wC*sin(
q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*
sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-
z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(
wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin
(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin
(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos
(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)
*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(q2))*(sin(
q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))*(
dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*
wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(
q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)
*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-
wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(
q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*
sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-
sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(
q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*
x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*
q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin
(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)
*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2
-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*
sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*
x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*
sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(
q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(
q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin
(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos
(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-
sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*
cos(q1)*cos(q3)*(wN-y)*q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(
q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y
*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))))
- (m*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*
sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(
q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*
cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))))*(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(
q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-
dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-
dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)
*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-
dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*
wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)
*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(
q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*
wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*
wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)
*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-
(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos
(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos
(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)
*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(
dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))+(dC-dcm)*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+
x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-
sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)
-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-
x))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos
(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm
*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*
sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(
q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*
sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*
wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'
+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(
q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)
*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))
-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*
q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*
cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*
q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*
wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(
q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+
x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(
q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(
q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-
sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')))+((z-(dC-dcm)*sin(q2))*(wN+y-dcm*
sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin
(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))))*((Ixx-Izz)*wx*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))*wz-Iyy*((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(
q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(
q1)*cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*
wz*q1'+dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*
wz*q3'+wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(
q2)*sin(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin
(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*
q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*
cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-
sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*
sin(q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)
*cos(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-
dcm*cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos
(q1)*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*
wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-
dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*
q3'-wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(
q3)*z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*
q2'-z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(
q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(
q2)*cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*
q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*
cos(q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
q3)*cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*
q1'+x*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-
sin(q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*
x'-cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(
q1)*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*
cos(q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-((z+dcm*sin(
q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3))))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(
q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(
q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(
q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*
cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(
q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+
dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin
(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin
(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(
q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*
(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-
sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(
q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(
q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(
sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'
-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(
q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)
*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*
cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)
*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-
sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*
sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(
q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*
wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(
q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*
cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos
(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*
cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*
y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(
q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(
q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1'))))+
(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*((dC-dcm)*(wN+dcm*sin
(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)
+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin
(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(m*g-
bF*z'*vMag^expDamping/(epsilonV+vMag))+(Iyy*((z-(dC-dcm)*sin(q2))*(wN+
y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z
*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm
*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))
*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin
(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(
sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(
q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))-(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))+Ixx*
((wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))
*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(
q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(
wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+
y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin
(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm
*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)
+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*
(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*((dcm*cos(q1)
*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-
dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x)))-dcm*(dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-
wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos
(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y)))-(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2+m*((dC-dcm)*(wN+dcm*
sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)
+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin
(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((dC-
dcm)*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(
q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(
wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(
q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(
dN-x-(dC-dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y)))))*x''-(Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(
q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x
*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-
wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin
(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*
((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-
wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-
(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(
q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin
(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z
+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))))-m*(dC-dcm)*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos
(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-
dcm*sin(q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(
wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(
q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x
*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-
wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*
sin(q2)*(wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*
sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))
*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))-Ixx*((wN+y-dcm*sin(
q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-
dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(
q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*
sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(
q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+
dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin
(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin
(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*
cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(
q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+
wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))
-(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)
-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(
q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(
wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(
q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(
q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(
q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-
wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin
(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)
*wz')/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+
x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(
wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2

-> (120) Dynamics[3] = bT*(wz*wMag^expDamping-wy*((z-(dC-dcm)*sin(q2))*(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(
q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)
*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))
+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(
q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*wMag^expDamping/(dcm*(z-(
dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)
*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))))-wx*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)
+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-
sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x
*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(
q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*
sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*
sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*
cos(q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*
sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*
cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*
cos(q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*wMag^
expDamping/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)
*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)
*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(
q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*
cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin
(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*
cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))))/(
epsilonW+wMag) + (dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*
sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)
*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))))*((Iyy-Izz)*wy*wz-Ixx*((dcm*(z-(dC-dcm)*
sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z
+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*
wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*
cos(q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'
+dcm*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+
wC*cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin
(q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(
q2)*q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos
(q2)*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*
cos(q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)
*cos(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*
q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*
cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-
wC*wx*((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*
z'-(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-
z*sin(q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
cos(q3)*q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-
sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(
q3)*q1'))-dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*
cos(q2)*q3'+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x
*sin(q2)*sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(
q1)*sin(q3)*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-
cos(q2)*cos(q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)
*sin(q3)*(wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(
q3)*(wN+y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-dcm*((z+dcm*sin(
q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)
*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*
sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*
cos(q3)*(wN+y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*
sin(q1)*sin(q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(
dC-dcm)*sin(q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(
dC-dcm)*cos(q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(
q1)*sin(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(
dC-dcm)*y*sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-
x)*wz*q1'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(
q1)*cos(q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos
(q2)*x'*q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)
*wz*q1'-(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*
wz*q2'-(dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)
*cos(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-
(dC-dcm)*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos
(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos
(q2)*cos(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)
*(dN-x)*q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*
cos(q1)*cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(
q1)*sin(q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(
dN-x)*q1'-cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))-(dcm*(z-(dC-dcm)*sin(
q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*
sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
dN-x)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)
*cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'
+dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+
wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos
(q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*
sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-
sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(
q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(
q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*
cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*
sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*
wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'
+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*
cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*
cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(
q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)
*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))
-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*
q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*
cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*
q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*
wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(
q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+
x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(
q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(
q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-
sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1')))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z
*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))))/(wC*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(
q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos
(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*
(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*
sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))) + (Izz+Ixx*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*
cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(
q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin
(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(
q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*
cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(
q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*
cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(
q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*
x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)
-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)
*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2/(wC^2*(dcm*(z-(dC-
dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(
q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2)+(Iyy*((z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))^2+m*(dC-dcm)^2*((z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)
*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos
(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos
(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*
cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(
q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin
(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))^2)
/(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin
(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z
*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)
-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)
*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))^2)*wz' + (m*(dC-dcm)*
((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))))*(dcm*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(
q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+
x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-
sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
y)))*(x'^2+y'^2+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(
q2)*sin(q3)*wz*y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(
q3)*cos(q2)*wz*z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(
q2)*q2')+(dC-dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*
cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*
sin(q2)*sin(q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+
(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(
q3)*wz*x'-(dC-dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*
q1'-(dC-dcm)*sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-
(dC-dcm)*y*sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(
dC-dcm)*sin(q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)
*(dN-x)*wz*q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)
*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos
(q3)*x'+cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos
(q3)*q2'+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*
q1'+sin(q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*
cos(q3)*q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(
q2)*sin(q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-
cos(q1)*cos(q2)*cos(q3)*(dN-x)*q2'))+(dC-dcm)*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*
sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))
*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(
q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(q2)*x'*
q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm*sin(
q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*cos(q1)
*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(q3)*q2'-
wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*sin
(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*cos(
q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-sin(
q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(q3)*
cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*q1'-sin(
q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)*q2'-sin
(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(q1)*cos(
q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos(q2)*y'*
q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(
q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(q3)*(z*cos
(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*((sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(
q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin
(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(wN+
y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)*
q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-dcm*wy*(
sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+cos(q1)
*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*sin(q3)*
cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)*x'-sin(
q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(q3)*z'-x
*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(wN+y)*q1'-
x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)*q2'-sin(
q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-(dC-dcm)*(z*cos(q2)*cos(q3)+x*(sin(
q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(
dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*wz*y'+wC*cos(q1)*cos(q2)*
wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(q1)*sin(q3)*wz*q3'+dcm*sin(
q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN-y)*wz*q3'+wC*sin(q1)
*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(q3)*q2'-z'-dcm*cos(q2)*q2'-
wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(
q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*
sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(q3)*
cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*q3'-
sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*sin(
q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*sin(q2)*y'+sin(q2)*cos(q1)*
x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(q2)*q2'-x*sin(q1)*sin(q2)*
q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(q1)*(wN-y)*q1')+wC*wx*((sin
(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*sin(q2)*cos(q3)*q2'+z*sin(
q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'-cos(q2)*cos(q3)*z'-(
wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*sin(q3)
*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-x'^2-y'^2
-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*
sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(q2)*x'*q1'-dcm*sin(q2)*cos(q1)*
x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*x*sin(q1)*cos(q2)*wz*q1'-wC*x*
sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(q2)*(wN-y)*wz*q1'-dcm*wy*(sin(
q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'+x*sin(q3)*cos(q1)*q1'+x*cos(
q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(wN-y)*q3'+sin(q1)*sin(q2)*sin
(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos
(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*sin(q2)*sin(q3)*cos(q1)*q3'-
sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)*cos(q3)*(wN-y)*q2'-sin(q2)*
cos(q1)*cos(q3)*(wN-y)*q1')))+(dC-dcm)*(dcm*(z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+
wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(
q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*
sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)
*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(
dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(
q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*
cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))
+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+
sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos
(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(
q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*
sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)
*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(
wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-
sin(q1)*sin(q2)*cos(q3)*(wN+y))))*(m*g-bF*z'*vMag^expDamping/(epsilonV
+vMag))+(Iyy*((z-(dC-dcm)*sin(q2))*(wN+y-dcm*sin(q1)*cos(q2)-wC*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))-(z-(dC-dcm)*sin(q2))*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(
q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))-(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-
dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(
q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x
*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)
*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin
(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y
*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*
cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(
q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos
(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)
-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(
dC-dcm)*((dC-dcm)*(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(
q1)*sin(q2)*sin(q3)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*
cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*
sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(dC-dcm)*(wN+y-dcm*sin(
q1)*cos(q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)
*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*
sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*
sin(q2)*cos(q3)*(wN+y))))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(
q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(
q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(
q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*
cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(
q1)*sin(q2)*(wN+y)))+(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos
(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos
(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(
q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)
*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos
(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*
(wN-y)))+dcm*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*
sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos
(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*
cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))-Ixx*((wN+y-dcm*sin(q1)*cos(
q2)-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3)))*(dcm*(z-(dC-dcm)*sin
(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+
sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm
*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(
q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(
dN-x)))-dcm*(y+(dC-dcm)*sin(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*
cos(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(
q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(
q2)+wC*sin(q3)*cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
y)))-(wN+dcm*sin(q1)*cos(q2)-y-wC*(cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin
(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin
(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(
q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*
sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(
q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin
(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(
q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*
cos(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x
*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(
q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(
q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)+sin(q1)*sin(q2)*(wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(
q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)
-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(
q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(
q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(
q1)-sin(q1)*sin(q2)*(wN-y)))*(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(
q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(
q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)
*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(
q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*y''
-((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(
dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-
wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-
(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(
q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin
(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)
*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z
+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)
+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))))*((Ixx-Izz)*wx*(dcm*(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*
cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))
+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin
(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*
sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(
q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(dC-dcm)*sin(q2))*(
sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*
cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+dcm*sin(
q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y
*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))))
*wz-Iyy*((z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(x'^2+y'^2+dcm*sin(q1)*cos(q3)*wz*x'+wC*sin(q1)*cos(q2)*wz*y'+
wC*cos(q1)*cos(q2)*wz*x'+dcm*sin(q1)*sin(q2)*y'*q2'+dcm*sin(q1)*cos(
q2)*x'*q1'+dcm*sin(q2)*cos(q1)*x'*q2'+dcm*x*cos(q1)*cos(q3)*wz*q1'+dcm
*sin(q1)*cos(q3)*(wN+y)*wz*q1'+dcm*sin(q3)*cos(q1)*(wN+y)*wz*q3'+wC*
cos(q1)*cos(q2)*(wN+y)*wz*q1'+z'*(z'+dcm*cos(q2)*q2'+wC*sin(q2)*sin(
q3)*q2'-wC*cos(q2)*cos(q3)*q3')+wC*x'*(cos(q1)*cos(q3)*q1'+sin(q1)*sin
(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*cos(q1)*cos(q3)*q3'-sin(
q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(q3)*q1'+sin(q3)*cos(q1)*
q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(q3)*cos(q2)*q2'-sin(q2)*
sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(z*sin(q2)*q2'+x*sin(q1)*sin(q2)*
q1'-sin(q1)*sin(q2)*y'-sin(q2)*cos(q1)*x'-cos(q2)*z'-x*cos(q1)*cos(q2)
*q2'-sin(q1)*cos(q2)*(wN+y)*q2'-sin(q2)*cos(q1)*(wN+y)*q1')-dcm*cos(
q1)*cos(q3)*wz*y'-wC*sin(q2)*wz*z'-wC*z*cos(q2)*wz*q2'-dcm*cos(q1)*cos
(q2)*y'*q1'-dcm*x*sin(q1)*sin(q3)*wz*q3'-wC*x*sin(q1)*cos(q2)*wz*q1'-
wC*x*sin(q2)*cos(q1)*wz*q2'-wC*sin(q1)*sin(q2)*(wN+y)*wz*q2'-dcm*cos(
q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y))*wz*q3'-wC*wx*
((sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))*x'+cos(q2)*cos(q3)*z'-(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'-z*sin(q2)*cos(q3)*q2'-z*sin(
q3)*cos(q2)*q3'-x*(sin(q1)*sin(q2)*cos(q3)*q1'+sin(q2)*sin(q3)*cos(q1)
*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*q1'-cos(q1)*cos(q2)*cos(q3)*
q2')-(wN+y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)*sin(q3)*q3'-sin(q1)*
sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*cos(q1)*cos(q3)*q1'))-
dcm*wy*(sin(q3)*cos(q1)*y'+z*sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'
+cos(q1)*cos(q3)*(wN+y)*q3'+x*sin(q1)*sin(q2)*cos(q3)*q1'+x*sin(q2)*
sin(q3)*cos(q1)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN+y)*q3'-sin(q1)*sin(q3)
*x'-sin(q1)*sin(q2)*cos(q3)*y'-sin(q2)*cos(q1)*cos(q3)*x'-cos(q2)*cos(
q3)*z'-x*sin(q1)*cos(q3)*q3'-x*sin(q3)*cos(q1)*q1'-sin(q1)*sin(q3)*(
wN+y)*q1'-x*cos(q1)*cos(q2)*cos(q3)*q2'-sin(q1)*cos(q2)*cos(q3)*(wN+y)
*q2'-sin(q2)*cos(q1)*cos(q3)*(wN+y)*q1'))-((z+dcm*sin(q2)+wC*sin(q3)*
cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))
-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*
sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)
*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))))*(x'^2+y'^2
+(dC-dcm)*cos(q1)*cos(q3)*wz*y'+(dC-dcm)*sin(q1)*sin(q2)*sin(q3)*wz*
y'+(dC-dcm)*sin(q2)*sin(q3)*cos(q1)*wz*x'+(dC-dcm)*sin(q3)*cos(q2)*wz*
z'+(dC-dcm)*cos(q1)*cos(q2)*y'*q1'+z'*(z'-(dC-dcm)*cos(q2)*q2')+(dC-
dcm)*z*cos(q2)*cos(q3)*wz*q3'+(dC-dcm)*y*sin(q1)*sin(q2)*cos(q3)*wz*
q3'+(dC-dcm)*y*sin(q1)*sin(q3)*cos(q2)*wz*q2'+(dC-dcm)*y*sin(q2)*sin(
q3)*cos(q1)*wz*q1'+(dC-dcm)*cos(q1)*cos(q3)*(dN-x)*wz*q1'+(dC-dcm)*sin
(q1)*sin(q2)*sin(q3)*(dN-x)*wz*q1'-(dC-dcm)*sin(q1)*cos(q3)*wz*x'-(dC-
dcm)*sin(q1)*sin(q2)*y'*q2'-(dC-dcm)*sin(q1)*cos(q2)*x'*q1'-(dC-dcm)*
sin(q2)*cos(q1)*x'*q2'-(dC-dcm)*y*sin(q1)*cos(q3)*wz*q1'-(dC-dcm)*y*
sin(q3)*cos(q1)*wz*q3'-(dC-dcm)*z*sin(q2)*sin(q3)*wz*q2'-(dC-dcm)*sin(
q1)*sin(q3)*(dN-x)*wz*q3'-(dC-dcm)*sin(q2)*cos(q1)*cos(q3)*(dN-x)*wz*
q3'-(dC-dcm)*sin(q3)*cos(q1)*cos(q2)*(dN-x)*wz*q2'-(dC-dcm)*wy*(sin(
q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*y'+sin(q2)*cos(q1)*cos(q3)*x'+
cos(q2)*cos(q3)*z'+y*sin(q1)*sin(q3)*q1'+y*sin(q1)*cos(q2)*cos(q3)*q2'
+y*sin(q2)*cos(q1)*cos(q3)*q1'+sin(q1)*sin(q2)*cos(q3)*(dN-x)*q1'+sin(
q2)*sin(q3)*cos(q1)*(dN-x)*q3'-sin(q3)*cos(q1)*y'-y*cos(q1)*cos(q3)*
q3'-z*sin(q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-y*sin(q1)*sin(q2)*sin(
q3)*q3'-sin(q1)*cos(q3)*(dN-x)*q3'-sin(q3)*cos(q1)*(dN-x)*q1'-cos(q1)*
cos(q2)*cos(q3)*(dN-x)*q2'))-(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x
*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin
(q1)*sin(q2)*cos(q3)))*(dcm*cos(q1)*cos(q3)*wz*y'+wC*sin(q1)*cos(q2)*
wz*y'+wC*cos(q1)*cos(q2)*wz*x'+dcm*cos(q1)*cos(q2)*y'*q1'+dcm*x*sin(
q1)*sin(q3)*wz*q3'+dcm*sin(q1)*cos(q3)*(wN-y)*wz*q1'+dcm*sin(q3)*cos(
q1)*(wN-y)*wz*q3'+wC*sin(q1)*sin(q2)*(wN-y)*wz*q2'+z'*(wC*sin(q2)*sin(
q3)*q2'-z'-dcm*cos(q2)*q2'-wC*cos(q2)*cos(q3)*q3')+dcm*cos(q3)*(z*cos(
q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y))*wz*q3'+wC*x'*(cos(q1)*
cos(q3)*q1'+sin(q1)*sin(q2)*sin(q3)*q1'-sin(q1)*sin(q3)*q3'-sin(q2)*
cos(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*cos(q2)*q2')+wC*y'*(sin(q1)*cos(
q3)*q1'+sin(q3)*cos(q1)*q3'-sin(q1)*sin(q2)*cos(q3)*q3'-sin(q1)*sin(
q3)*cos(q2)*q2'-sin(q2)*sin(q3)*cos(q1)*q1')+dcm*sin(q3)*wz*(sin(q1)*
sin(q2)*y'+sin(q2)*cos(q1)*x'+cos(q2)*z'+x*cos(q1)*cos(q2)*q2'-z*sin(
q2)*q2'-x*sin(q1)*sin(q2)*q1'-sin(q1)*cos(q2)*(wN-y)*q2'-sin(q2)*cos(
q1)*(wN-y)*q1')+wC*wx*((sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3))*y'+z*
sin(q2)*cos(q3)*q2'+z*sin(q3)*cos(q2)*q3'+x*(sin(q1)*sin(q2)*cos(q3)*
q1'+sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*cos(q3)*q3'-sin(q3)*cos(q1)*
q1'-cos(q1)*cos(q2)*cos(q3)*q2')-(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(
q3))*x'-cos(q2)*cos(q3)*z'-(wN-y)*(cos(q1)*cos(q3)*q3'+sin(q1)*sin(q2)
*sin(q3)*q3'-sin(q1)*sin(q3)*q1'-sin(q1)*cos(q2)*cos(q3)*q2'-sin(q2)*
cos(q1)*cos(q3)*q1'))-x'^2-y'^2-dcm*sin(q1)*cos(q3)*wz*x'-wC*sin(q2)*
wz*z'-wC*z*cos(q2)*wz*q2'-dcm*sin(q1)*sin(q2)*y'*q2'-dcm*sin(q1)*cos(
q2)*x'*q1'-dcm*sin(q2)*cos(q1)*x'*q2'-dcm*x*cos(q1)*cos(q3)*wz*q1'-wC*
x*sin(q1)*cos(q2)*wz*q1'-wC*x*sin(q2)*cos(q1)*wz*q2'-wC*cos(q1)*cos(
q2)*(wN-y)*wz*q1'-dcm*wy*(sin(q1)*sin(q3)*x'+sin(q1)*sin(q2)*cos(q3)*
y'+sin(q2)*cos(q1)*cos(q3)*x'+cos(q2)*cos(q3)*z'+x*sin(q1)*cos(q3)*q3'
+x*sin(q3)*cos(q1)*q1'+x*cos(q1)*cos(q2)*cos(q3)*q2'+cos(q1)*cos(q3)*(
wN-y)*q3'+sin(q1)*sin(q2)*sin(q3)*(wN-y)*q3'-sin(q3)*cos(q1)*y'-z*sin(
q2)*cos(q3)*q2'-z*sin(q3)*cos(q2)*q3'-x*sin(q1)*sin(q2)*cos(q3)*q1'-x*
sin(q2)*sin(q3)*cos(q1)*q3'-sin(q1)*sin(q3)*(wN-y)*q1'-sin(q1)*cos(q2)
*cos(q3)*(wN-y)*q2'-sin(q2)*cos(q1)*cos(q3)*(wN-y)*q1'))))-(Iyy*((z-(
dC-dcm)*sin(q2))*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-sin(q2)*
sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(
q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(z-(dC-
dcm)*sin(q2))*(x-dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(
q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))-(dN-x-(dC-
dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*
cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))*((z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))+(z-(dC-dcm)*sin(q2))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin
(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))
*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin
(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(
q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))-(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin
(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*
sin(q3)*cos(q1)*(dN-x))*((z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)
*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*
cos(q1)-sin(q1)*sin(q2)*cos(q3)))+(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(
z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))))-m*(dC-dcm)*((dC-dcm)*(x-
dcm*cos(q1)*cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)
*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))-(dC-dcm)*(dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(
q3)-sin(q2)*sin(q3)*cos(q1)))*(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+
sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(
q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x))-dcm*(dN-x-(dC-
dcm)*cos(q1)*cos(q2))*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*
cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*
sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*
cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(q2)*cos(q3)+x*(
sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(
q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(
q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))))
*((z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)
*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(
q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*
cos(q1)*cos(q3)*(dN-x))*(dcm*x*sin(q1)*cos(q3)+wC*x*cos(q1)*cos(q2)+
wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos(q3)*(wN+y)-dcm*
sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(wN+y)))+(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))*(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+dcm*cos(q1)*cos(
q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(q2)-dcm*sin(q3)*
(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))+dcm*(y*cos(q1)*
cos(q3)+z*sin(q3)*cos(q2)+y*sin(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(
dN-x)-sin(q2)*sin(q3)*cos(q1)*(dN-x))*((z*cos(q2)*cos(q3)+x*(sin(q1)*
sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(q3)*cos(q1)-sin(q1)*sin(
q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*
cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))-(z*cos(
q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))+(wN-y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(sin(q3)*cos(q1)*(wN+y)-x*sin(
q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(
q2)*cos(q3)*(wN+y))))-Ixx*((dcm*cos(q1)*cos(q2)-x-wC*(sin(q1)*cos(q3)-
sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)
+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-
sin(q1)*sin(q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos
(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-
sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-dcm*(dN-x-(dC-
dcm)*cos(q1)*cos(q2))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*
sin(q3)+z*cos(q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(
wN-y)-sin(q1)*sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(
q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin
(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(x-dcm*cos(q1)*
cos(q2)-wC*(sin(q1)*cos(q3)-sin(q2)*sin(q3)*cos(q1)))*(dcm*(z-(dC-dcm)
*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-
x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-dcm)*(z+
dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin(q2)*
cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*cos(
q3)*(dN-x))))*(dcm*(dC-dcm)*(y*cos(q1)*cos(q3)+z*sin(q3)*cos(q2)+y*sin
(q1)*sin(q2)*sin(q3)+sin(q1)*cos(q3)*(dN-x)-sin(q2)*sin(q3)*cos(q1)*(
dN-x))*((z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(x*sin(q1)*sin(q3)+z*cos(
q2)*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*
sin(q2)*cos(q3)*(wN-y))+(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(sin(q3)*
cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*
cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y)))-(dcm*x*sin(q1)*cos(q3)+wC*x*
cos(q1)*cos(q2)+wC*sin(q1)*cos(q2)*(wN+y)-wC*z*sin(q2)-dcm*cos(q1)*cos
(q3)*(wN+y)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)+sin(q1)*sin(q2)*(
wN+y)))*(dcm*(z-(dC-dcm)*sin(q2))*(x*sin(q1)*sin(q3)+z*cos(q2)*cos(q3)
+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(q2)*cos(
q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos(q2)*cos
(q3)+y*sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)
-sin(q2)*cos(q1)*cos(q3)*(dN-x)))-(wC*z*sin(q2)+dcm*x*sin(q1)*cos(q3)+
dcm*cos(q1)*cos(q3)*(wN-y)+wC*sin(q1)*cos(q2)*(wN-y)-wC*x*cos(q1)*cos(
q2)-dcm*sin(q3)*(z*cos(q2)+x*sin(q2)*cos(q1)-sin(q1)*sin(q2)*(wN-y)))*
(dcm*(z-(dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*
cos(q2)*cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+
y))-(dC-dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*
sin(q1)*sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(
q2)*cos(q1)*cos(q3)*(dN-x))))/wC^2)*x'')/(dcm*(z-(dC-dcm)*sin(q2))*(z*
cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(
sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(x*sin(q1)*sin(q3)+z*cos(q2)
*cos(q3)+x*sin(q2)*cos(q1)*cos(q3)+sin(q3)*cos(q1)*(wN-y)-sin(q1)*sin(
q2)*cos(q3)*(wN-y))+(dC-dcm)*(z+dcm*sin(q2)+wC*sin(q3)*cos(q2))*(z*cos
(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*cos(q3))-(wN+y)*(sin(
q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(z*cos(q2)*cos(q3)+y*sin(q1)*sin
(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(q1)*
cos(q3)*(dN-x))-(z*cos(q2)*cos(q3)+x*(sin(q1)*sin(q3)+sin(q2)*cos(q1)*
cos(q3))+(wN-y)*(sin(q3)*cos(q1)-sin(q1)*sin(q2)*cos(q3)))*(dcm*(z-(
dC-dcm)*sin(q2))*(sin(q3)*cos(q1)*(wN+y)-x*sin(q1)*sin(q3)-z*cos(q2)*
cos(q3)-x*sin(q2)*cos(q1)*cos(q3)-sin(q1)*sin(q2)*cos(q3)*(wN+y))-(dC-
dcm)*(z+dcm*sin(q2)-wC*sin(q3)*cos(q2))*(z*cos(q2)*cos(q3)+y*sin(q1)*
sin(q2)*cos(q3)-y*sin(q3)*cos(q1)-sin(q1)*sin(q3)*(dN-x)-sin(q2)*cos(
q1)*cos(q3)*(dN-x))))^2 - (Ixx-Iyy)*wx*wy

(121) %--------------------------------------------------------------------
(122) %   Integration parameters and initial values for variables.
(123) Input  tFinal = 120 sec,  tStep = 0.05 sec,  absError = 1.0E-07
(124) Input  x' = 0 m/s,  y' = 0 m/s,  wz = 0 rad/sec
(125) %--------------------------------------------------------------------
(126) %   Calculate yaw, pitch and roll angles in terms of q1, q2, q3.
(127) Yaw   = GetAngleBetweenUnitVectors( Nx>, Cy> )  -  pi/2
-> (128) Yaw = -1.570796 + acos(sin(q2)*sin(q3)*cos(q1)-sin(q1)*cos(q3))

(129) Pitch = GetAngleBetweenUnitVectors( Nz>, Cx> )  -  pi/2
-> (130) Pitch = 1.570796 - acos(sin(q2))

(131) Roll  = pi/2  -  GetAngleBetweenUnitVectors( Nz>, Cy> )
-> (132) Roll = 1.570796 - acos(sin(q3)*cos(q2))

(133) %--------------------------------------------------------------------
(134) %   Solve for initial values of q1, q2, q3 via initial yaw, pitch, roll angles.
(135) Constant  Yaw0 = 60 deg,  Pitch0 = 0 deg,  Roll0 = 0 deg
(136) SolveSetInput( [Yaw; Pitch; Roll] = [Yaw0; Pitch0; Roll0],  q1 = 60 deg, q2 = 0 deg, q3 = 0 deg )

->    %  INPUT has been assigned as follows:
->    %   q1                        60                      deg
->    %   q2                        0                       deg
->    %   q3                        0                       deg

(137) %--------------------------------------------------------------------
(138) %   Solve for initial values of x, y, z using the CableConstraint (and initial values of q1, q2, q3).
(139) SolveSetInput( CableConstraint = 0,  x = Input(dN)/2 m,  y=0 m,  z = 0.8*Input(L1) m )

->    %  INPUT has been assigned as follows:
->    %   x                         8.166666666666668       m
->    %   y                        -0.2886751345948148      m
->    %   z                         20.74983266331456       m

(140) %--------------------------------------------------------------------
(141) %   List output quantities and solve ODEs.
(142) Distance = Co.GetDistance( No )
-> (143) Distance = sqrt(dcm^2+x^2+y^2+z^2+2*dcm*z*sin(q2)-2*dcm*x*cos(q1)*cos(
q2)-2*dcm*y*sin(q1)*cos(q2))

(144) 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
(145) 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
(146) ODE( Dynamics = 0,  x'',  y'',  wz' )  MGAircraftTrifilarPendulumDynamicsKaneEmbedded.m

(147) %--------------------------------------------------------------------
```