NewtonianFrame Wall
RigidFrame F, T
RigidBody Airplane
Variable x'', y''  	% Plane cm position
Variable qA''       % Airplane pitch
Variable qH'        % Hip angle (angle between Fx> and Ax>)
Variable qK'        % Knee angle (angle between Fx> and Tx>) 
Variable qS'        % Spine extension along Tx>
Variable qHSm'      % Hip angle (angle between Fx> and Ax>)
Variable qKSm'      % Knee angle (angle between Fx> and Tx>) 
Variable qSSm'      % Spine extension along Tx>
Constant g          % Gravity
Constant Ltailx     % Distance from CM to the tail along -Ax>  
Constant Ltaily     % Distance from CM to the tail along -Ay> 
Constant Lf         % Length of the femur
Constant Lt         % Length of the tibia
Constant LHip       % Lenght from CM to the hip along -Ax> 
Constant ev         % Friction model (small velocity)
Constant uk         % Kinetic coulomb friction
Constant Lkv        % Lenght of the virtual knee... 

% Aerodynamics
Specified qe		% Elevator angle (up = positive)
Constant LwCA		% Distance CM to the wing aerodynamics center along -Ax>
Constant LeCA		% Distance CM to the elevator aerodynamics center along -Ax>
Constant rho		% Air density
Constant Awing		% Wing area
Constant Atail		% Tail area
Specified alphaw, alphae % Angle of attack of wing and elevator

% Contact points parameters
Constant kcontact, bcontact 
Specified onSpringTail, onDamperTail, onSpringKnee, onDamperKnee
Specified onSpringShearTail, yTailContact''

% Foot contact parameters
Constant bHip, qHzero               % Hip properties
Specified kHip                      % Hip stiffness
Constant kKnee, bKnee, qKzero		% Knee properties
Constant kSpine, bSpine             % Spine properties
Specified onFoot                    % Enable forces at the foot in the normal direction
Specified onFootShear               % Enable forces at the foot in the shear direction
Specified yFootContact''            % Coordinate of foot contact point with the wall	


%%%%%%%%%%
% Points %
%%%%%%%%%%
Point		Tail(Airplane)	  % Tail Elevator and contact point
Point		Foot		  % Foot location (no load) 
Point		Hip		  % Hip Position
Point		Contact(Airplane) % Foot contact point
Point 		Knee(Airplane)    % Knee contact point
Point 		KneeVir		  % Virtual knee joint
Point 		BaseSpines	  % Base of spines
Point 		wCA(Airplane)	  % Wing Aerodynamics Center
Point		eCA(Airplane)	  % Elevator Aerodynamics center

%%%%%%%%%%%%%%%%%%%%
% Mass and Inertia %
%%%%%%%%%%%%%%%%%%%%
Airplane.SetMass(mAirplane)
Airplane.SetInertia(Airplanecm,IxxAirplane,IyyAirplane,IzzAirplane)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rotation and angular velocities %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Airplane.RotateZ(Wall,qa)
F.RotateZ(Airplane, -qH)
T.RotateZ(F, qK)

% Frame angular velocity and acceleration
Airplane.GetAngularVelocity(Wall)
Airplane.GetAngularAcceleration(Wall)

F.GetAngularVelocity(Wall)
%F.GetAngularAcceleration(Wall)

T.GetAngularVelocity(Wall)
%T.GetAngularAcceleration(Wall)

%%%%%%%%%%%%%%%%%%%%
% Position vectors %
%%%%%%%%%%%%%%%%%%%%
Airplanecm.SetPosition(Wallo, x*Wallx> + y*Wally>)
Airplanecm.SetVelocity(Wall,Wallo)

Tail.SetPosition(Airplanecm, -Ltailx*Airplanex> - Ltaily*Airplaney> )
Tail.SetVelocity(Wall, Airplanecm, Airplane) 

Hip.SetPosition(Airplanecm, -LHip*Airplanex>)
Hip.SetVelocity(Wall, Airplanecm, Airplane)

Knee.SetPosition(Hip, Lf*Fx>)
KneeVir.SetPosition(Knee, Lkv*Fy>)

BaseSpines.SetPosition(KneeVir, Lt*Tx>)
BaseSpines.SetVelocity(Wall, Airplanecm)

Foot.SetPosition(KneeVir, (Lt+qS)*Tx>)
Foot.SetVelocity(Wall, Airplanecm)

Contact.SetPosition(Wallo, yFootContact*Wally>)
Contact.SetVelocity(Wall, Wallo, Wall)

wCA.SetPosition(Airplanecm, -LwCA*Airplanex>)
wCA.SetVelocity(Wall, Airplanecm, Airplane)

eCA.SetPosition(Airplanecm, -LeCA*Airplanex>)
eCA.SetVelocity(Wall, Airplanecm, Airplane)

% Angle of attack are specified in the matlab code using these components 
% for atan2: 
wAlphaATAN1st = -Dot(wCA.GetVelocity(Wall),Airplaney>)	% From velocity vector to wing... 
wAlphaATAN2nd = Dot(wCA.GetVelocity(Wall),Airplanex>)
eAlphaATAN1st = -Dot(eCA.GetVelocity(Wall),Airplaney>)
eAlphaATAN2nd = Dot(eCA.GetVelocity(Wall),Airplanex>)


%%%%%%%%%%%%%%%%%%%
% Constraint Eqns %
%%%%%%%%%%%%%%%%%%%

autoz ON

% Jacobian matrix => [vx_wall; vy_wall] = Jacobian*[qHip; qKnee; qSpine]
Jacobian[1,1] = D(Dot(Foot.GetVelocity(Wall),Wallx>), qH')
Jacobian[1,2] = D(Dot(Foot.GetVelocity(Wall),Wallx>), qK')
Jacobian[1,3] = D(Dot(Foot.GetVelocity(Wall),Wallx>), qS')
Jacobian[2,1] = D(Dot(Foot.GetVelocity(Wall),Wally>), qH')
Jacobian[2,2] = D(Dot(Foot.GetVelocity(Wall),Wally>), qK')
Jacobian[2,3] = D(Dot(Foot.GetVelocity(Wall),Wally>), qS')

KnownJacobian[1,1] = D(Dot(Foot.GetVelocity(Wall),Wallx>), x')
KnownJacobian[1,2] = D(Dot(Foot.GetVelocity(Wall),Wallx>), y')
KnownJacobian[1,3] = D(Dot(Foot.GetVelocity(Wall),Wallx>), qA')
KnownJacobian[2,1] = D(Dot(Foot.GetVelocity(Wall),Wally>), x')
KnownJacobian[2,2] = D(Dot(Foot.GetVelocity(Wall),Wally>), y')
KnownJacobian[2,3] = D(Dot(Foot.GetVelocity(Wall),Wally>), qA')

KnownVelocity = KnownJacobian*[x';y';qA']

StiffnessMatrix = [kHip, 0, 0; 0, kKnee, 0; 0, 0, kSpine]
DampingMatrix = [bHip, 0, 0; 0, bKnee, 0; 0, 0, bSpine]

InverseJt = GetInverse(Jacobian*Transpose(Jacobian))*Jacobian
NullSpace = [1, 0, 0; 0, 1, 0; 0, 0, 1]-Transpose(Jacobian)*InverseJt
LeftSideForces = NullSpace*(StiffnessMatrix)*([qH-qHzero;qK-qKzero;qS])
RightSideForces = -Nullspace*(DampingMatrix)
LeftSide = [-KnownVelocity; LeftSideForces[1]]
AugmentedMatrix = [Jacobian; [RightSideForces[1,1], RightSideForces[1,2], RightSideForces[1,3]]]
SolveAugmentedMatrix = GetInverse(AugmentedMatrix)*LeftSide
qH' = SolveAugmentedMatrix[1]
qK' = SolveAugmentedMatrix[2]
qS' = SolveAugmentedMatrix[3]

%%%%%%%%%%%%%%%%%%%%
% Force Definition %
%%%%%%%%%%%%%%%%%%%%
Gravity> 	= -mAirplane*g*Wally>					% Gravity

TailPenetration = Dot(Tail.GetPosition(Wallo), Wallx>)
TailPenetrationVel = Dt(TailPenetration)
Fspringtail> = -kcontact*TailPenetration*Wallx>*onSpringTail		% Tail contact
Fdampertail> = -bcontact*TailPenetrationVel*Wallx>*onDamperTail
TailNormal> = Fspringtail> + Fdampertail>
TailNormal = Dot(TailNormal>,Wallx>)

TailContactVerticalSliding = Dot(Tail.GetPosition(Wallo), Wally>) - yTailContact
TailContactVerticalSlidingVel = Dt(TailContactVerticalSliding) 
TailSpringShear> = onSpringShearTail*(-kcontact*TailContactVerticalSliding - bcontact*TailContactVerticalSlidingVel)*Wally>

KneePenetration = Dot(Knee.GetPosition(Wallo), Wallx>)
KneePenetrationVel = Dt(KneePenetration)
Fspringknee> = -kcontact*KneePenetration*Wallx>*onSpringKnee		% Knee contact 
Fdamperknee> = -bcontact*KneePenetrationVel*Wallx>*onDamperKnee

TailVerticalVel = Dot(Tail.GetVelocity(Wall),Wally>) 
TailFriction> = -uk*TailVerticalVel/(abs(TailVerticalVel) + ev)*GetMagnitude(TailNormal>)*Wally>
TailFriction = Dot(TailFriction>,Wally>)

% Spines forces
% Joint torques are always active, but if qH = qHzero and qH' = 0 then JointTorques = 0
JointTorques[1] = -kHip*(qH-qHzero) - bHip*qH'		
JointTorques[2] = -kKnee*(qK-qKzero) - bKnee*qK'
JointTorques[3] = -kSpine*qS - bSpine*qS'
SpinesForces = -onFoot*InverseJt*JointTorques               % Least-square... 

% Aerodynamics forces
wDrag>	 	= -rho*Awing*wCA.GetSpeed(Wall)^2*sin(alphaw)^2*GetUnitVector(wCA.GetVelocity(Wall))
wLift> 		= rho*Awing*wCA.GetSpeed(Wall)^2*sin(alphaw)*cos(alphaw)*Cross(Airplanez>,GetUnitVector(wCA.GetVelocity(Wall)))
eDrag>	 	= -rho*Atail*eCA.GetSpeed(Wall)^2*sin(alphae)^2*GetUnitVector(eCA.GetVelocity(Wall))
eLift> 		= rho*Atail*eCA.GetSpeed(Wall)^2*sin(alphae)*cos(alphae)*Cross(Airplanez>,GetUnitVector(eCA.GetVelocity(Wall)))


%%%%%%%%%%%%%%%%%
% Adding Forces %
%%%%%%%%%%%%%%%%%
Airplanecm.AddForce(Gravity>)						% Gravity 
wCA.AddForce(wDrag> + wLift>)						% Wing aerodynamics forces
eCA.AddForce(eDrag> + eLift>)						% Elevator aerodynamics forces
Tail.AddForce(TailNormal> + TailFriction> + TailSpringShear>)		% Tail contact forces
Knee.AddForce(Fspringknee>+Fdamperknee>)			% Tail contact forces
Contact.AddForce(SpinesForces[1]*Wallx> + SpinesForces[2]*Wally>)	% Suspension forces


%%%%%%%%%%
% EoM    %
%%%%%%%%%%
% Equation of motion for a free rotating object
ZeroEuler> = System.GetDynamics(Airplanecm)
eqnEuler[1] = Dot(ZeroEuler>,Wallz>)
solve(eqnEuler,qa'')

% Equation of motion for a free falling object
ZeroNewton> = System.GetDynamics()
eqnNewton[1] = Dot(ZeroNewton>,Wallx>)
eqnNewton[2] = Dot(ZeroNewton>,Wally>)
solve(eqnNewton,x'',y'')


%%%%%%%%%%
% Output %
%%%%%%%%%%
autoz OFF

Hipx = Dot(Hip.GetPosition(Wallo), Wallx>)
Hipy = Dot(Hip.GetPosition(Wallo), Wally>)
Tailx = Dot(Tail.GetPosition(Wallo), Wallx>)
Taily = Dot(Tail.GetPosition(Wallo), Wally>)
Kneex = Dot(Knee.GetPosition(Wallo), Wallx>)
Kneey = Dot(Knee.GetPosition(Wallo), Wally>)
KneeVirx = Dot(KneeVir.GetPosition(Wallo), Wallx>)
KneeViry = Dot(KneeVir.GetPosition(Wallo), Wally>)
BaseSpinesx = Dot(BaseSpines.GetPosition(Wallo), Wallx>)
BaseSpinesy = Dot(BaseSpines.GetPosition(Wallo), Wally>)
Footx = Dot(Foot.GetPosition(Wallo), Wallx>)
Footy = Dot(Foot.GetPosition(Wallo), Wally>)
KneeForce = Dot(Fspringknee>+Fdamperknee>, Wallx>)


%%%%%%%%%%
% Values %
%%%%%%%%%%
UnitSystem kg, m, sec
Input tInitial = 0 sec, tFinal = 5 sec, integStp = 0.001
Input g = 9.81 m/sec^2, mAirplane = 0.4 kg, IzzAirplane = 0.0164 kg*m^2
Input Ltailx = 0.57 m, Ltaily = 0.065 m, Lf = 0.065 m, Lt = 0.235 m, LHip = -0.030 m
Input kcontact = 2000 N/m, bcontact = 0 N*sec/m
Input bHip = 0.0017 N*m*sec/deg, qHzero = 135 deg
Input kKnee = 0.062 N*m/deg, bKnee = 0.0001 N*m*sec/deg, qKzero = 90 deg

% Initial conditions
Input qA = 90 deg, qa' = 0 deg/sec
Input qH = 135 deg, qK = 90 deg
Input x = -0.2 m, x' = 2 m/sec
Input y = 0 m, y' = 0 m/sec

Output t sec, x m, y m, qa deg, qH deg, qK deg, qS m, x' m/s, y' m/s, qa' deg/s, qH' deg/s, qK' deg/s, qS' m/s
Output Tailx m, Taily m, x m, y m, Hipx m, Hipy m, Kneex m, Kneey m, KneeVirx m, KneeViry m, BaseSpinesx m, BaseSpinesy m, Footx m, Footy m
Output SpinesForces[1] N, SpinesForces[2] N, KneeForce N, TailNormal N, TailFriction N


Code ODE() AirplaneSuspension3DOF.m