% Gimbal joint problem demonstrating a 3D rotation
% Euler parameters used instead of orientation angles
% Generalized speeds defined as Wi-s
% N1> to the left, N3> upward, N2> = N3> x N1>
%
% Set up problem
OVERWRITE ON
NEWTONIAN N
FRAMES A,B
BODIES C
POINTS O
CONSTANTS RHO,R,H,PI,G
CONSTANTS M,I1,I2,I3,TC
CONSTANTS QI1=0.01,QI2=0,QI3=0.01
V=PI*R^2*H
M=RHO*V
MASS C=M
I1=M*(3*R^2+H^2)/12
I2=I1
I3=M*R^2/2
INERTIA C,I1,I2,I3
VARIABLES E{4}',W{3}',Q{3}
VARIABLES KE,PE,NRG
%
% Rotation matrices for initial orientation
SIMPROT(N,A,1,QI1)
SIMPROT(A,B,2,QI2)
SIMPROT(B,C,3,QI3)
%
% Initial Euler parameters
EI4=0.5*(1+N_C[1,1]+N_C[2,2]+N_C[3,3])^0.5
EI1=(N_C[3,2]-N_C[2,3])/(4*EI4)
EI2=(N_C[1,3]-N_C[3,1])/(4*EI4)
EI3=(N_C[2,1]-N_C[1,2])/(4*EI4)
%
% Rotation matrix - from Euler parameters
N_C[1,1]=1-2*E2^2-2*E3^2
N_C[1,2]=2*(E1*E2-E3*E4)
N_C[1,3]=2*(E3*E1+E2*E4)
N_C[2,1]=2*(E1*E2+E3*E4)
N_C[2,2]=1-2*E3^2-2*E1^2
N_C[2,3]=2*(E2*E3-E1*E4)
N_C[3,1]=2*(E3*E1-E2*E4)
N_C[3,2]=2*(E2*E3+E1*E4)
N_C[3,3]=1-2*E1^2-2*E2^2
%
% Corresponding Q's just for comparison
Q2=ASIN(N_C[1,3])
Q1=ASIN(-N_C[2,3])/COS(Q2)
Q3=ASIN(-N_C[1,2])/COS(Q2)
%
% Rotational kinematics
W_C_N>=W1*C1>+W2*C2>+W3*C3>
E1'=0.5*(W1*E4-W2*E3+W3*E2)
E2'=0.5*(W1*E3+W2*E4-W3*E1)
E3'=0.5*(-W1*E2+W2*E1+W3*E4)
E4'=-0.5*(W1*E1+W2*E2+W3*E3)
ALF_C_N>=DT(W_C_N>,C)
%
% Translational kinematics
P_O_CO>=0.5*H*C3>
V_O_N>=0>
V2PTS(N,C,O,CO)
A_O_N>=0>
A2PTS(N,C,O,CO)
%
% Contact, distance, and inertia forces
D_FORCE>=-M*G*N3>
I_FORCE>=-M*A_CO_N>
C_TORQUE>=TC*C2>
I_TORQUE>=TSTAR(C,N)
%
% Motion Law - the set of all contact, distance, and
% inertia forces acting on C is a zero set
MOMENT>=CROSS(P_O_CO>,D_FORCE>+I_FORCE>)+C_TORQUE>+I_TORQUE>
ZERO=DOT(MOMENT>,[C1>;C2>;C3>])
SOLVE(ZERO,W1',W2',W3')
%
% Energy check - should be constant when TC = 0
KE=0.5*M*DOT(V_CO_N>,V_CO_N>)+0.5*DOT(W_C_N>,DOT(I_C_CO>>,W_C_N>))
PE=M*G*DOT(P_O_CO>,N3>)
NRG=KE+PE
%
% Output simulation code
INPUT RHO=2794,R=0.1,H=2,PI=4.0*ATAN(1.0),G=9.8
INPUT E1=0.004999916667083333
INPUT E2=-2.499979166736111E-05
INPUT E3=0.004999916667083333
INPUT E4=0.9999750002083326
INPUT W1=0,W2=3,W3=0,TC=0
INPUT TFINAL=1,INTEGSTP=0.01
UNITS RHO=KG/M^3,[R,H]=M,G=M/S^2
UNITS [Q{1:3}]=RAD,[W{1:3}]=RAD/S
UNITS T=S,TC=N*M
OUTPUT T,Q1,Q2,Q3
OUTPUT T,W1,W2,W3
OUTPUT T,KE,PE,NRG
OUTPUT T,E1,E2,E3,E4
CODE ODE() EULERPARAMS.M