Kevin % Menear % Multibody Dynamics - Spring '08 % Autonomous Satellite Capture by a Space Robot % % Body J - Main robot body % Body K - Robot arm directly attached to main body % Body L - End robot arm % Point O - Newtonian origin % Point SO - COM of system % Point JO - COM of body J % Point KO - COM of body K % Point LO - COM of body L % Point U - Point coincident with bodies J and K % Point V - Point coincident with bodies K and L % Point P - Point representing satellite body, following a prescribed path % Point PO - Point about which point P rotates % Point Q - Point on body L that will be coincident with point P % MJ - Mass of body J % MKL - Mass of bodies K and L % Q1 - Angle between N and J reference frames % Q2 - Angle between J and K reference frames % Q3 - Angle between K and L reference frames % Q4 - Magnitude of position vector from O to JO in the N1 direction % Q5 - Magnitude of position vector from O to JO in the N2 direction % THETA - Angle between N and P references frames % R - Distance from point O to point PO % L - 1/2 length of each side of body J, length of bodies K, L % W - Width of bodies K, L PAUSE 0 BODIES J,K,L FRAMES Pref POINTS O,U,V,P,PO,Q CONSTANTS MJ=100, MKL=1, R=.25, L=2, W=.15 VARIABLES Q{5}',U{5}', Qv % Tell AUTOLEV that THETA is a specified quantity SPECIFIED THETA',THETA'' THETA=2*pi*t % Define Newtonian Reference Frame NEWTONIAN N % Define Masses of bodies J,K,L MASS J=MJ,K=MKL, L=MKL % Define Orientation Bodies of and Reference Frames SIMPROT(N,J,3,Q1) SIMPROT(J,K,3,Q2) SIMPROT(K,L,-3,Q3) SIMPROT(N,Pref,3,THETA) % Define positions of key points
P_O_JO>=Q4*N1>+Q5*N2> P_JO_U>=L*J1>+L*J2> P_U_KO>=1/2*L*K1> P_KO_V>=1/2*L*K1> P_V_LO>=1/2*L*L1> P_LO_Q>=1/2*L*L1> P_O_PO>=2*L*N1>+L*N2> P_PO_P>=R*Pref2> P_Q_P>=0*N1> % Position Constraint Equation for Point Q % Define Inertia Properties for Bodies J,K,L INERTIA J, 1/3*MJ*L^2, 1/3*MJ*L^2, 2/3*MJ*L^2 INERTIA K, 1/12*MKL*W^2, 1/12*MKL*L^2, 1/12*MKL*(W^2+L^2) INERTIA L, 1/12*MKL*W^2, 1/12*MKL*L^2, 1/12*MKL*(W^2+L^2) % Create Kinematical Differential Equations Q1'=U1 Q2'=U2 Q3'=U3 Q4'=U4 Q5'=U5 % State velocities and angular velocities of key points V_O_N>=0*N1> V_JO_N>=U4*N1>+U5*N2> V2PTS(N,J,JO,U) V2PTS(N,K,U,KO) V2PTS(N,K,U,V) V2PTS(N,L,V,LO) V2PTS(N,L,LO,Q) W_J_N>=U1*N3> W_K_J>=U2*N3> W_L_K>=U3*N3> W_Pref_N>=THETA'*N3> % Solve for generalized active and generalized inertia forces ZERO=FR()+FRSTAR() % Form Kane's Dynamical Equations KANE() % Specify the units for all variables and constants UNITS MJ=kg, MKL=kg, THETA'=rad/sec, R=m, L=m, W=m, & Q1=rad, Q2=rad, Q3=rad, Q4=m, Q5=m, T=S, & U1=rad/s, U2=rad/s, U3=rad/s, U4=m/s, U5=m/s % State quantities to be output OUTPUT T, Q1,Q2,Q3,Q4,Q5,Theta' OUTPUT T, U1,U2,U3,U4,U5 % Write computer code associated with dynamics analysis CODE DYNAMICS() ASC2.C