Homec4science
Diffusion MSM (command_line) Browse

msm/ShoulderModel_3DOF/EquationsofMotion.m
ece361673202command_line

Last Change
Raw File

EquationsofMotion.m
View Options

MDYN(1,1) = Iyy*cos(q2)^2 + Izz*cos(q3)^2*sin(q2)^2 + Ixx*sin(q2)^2*sin(q3)^2;
MDYN(1,2) = cos(q3)*sin(q2)*sin(q3)*(Ixx - Izz);
MDYN(1,3) = Iyy*cos(q2);
MDYN(2,1) = cos(q3)*sin(q2)*sin(q3)*(Ixx - Izz);
MDYN(2,2) = Ixx - Ixx*sin(q3)^2 + Izz*sin(q3)^2;
MDYN(2,3) = 0;
MDYN(3,1) = Iyy*cos(q2);
MDYN(3,2) = 0;
MDYN(3,3) = Iyy;
RHS(1,1) = Iyy*dq2*dq3*sin(q2) - (Ixx*dq1*dq2*sin(2*q2))/2 + Iyy*dq1*dq2*sin(2*q2) - Ixx*dq2*dq3*cos(q3)^2*sin(q2) + Izz*dq2*dq3*cos(q3)^2*sin(q2) + Ixx*dq2*dq3*sin(q2)*sin(q3)^2 - Izz*dq2*dq3*sin(q2)*sin(q3)^2 - Ixx*dq2^2*cos(q2)*cos(q3)*sin(q3) + Izz*dq2^2*cos(q2)*cos(q3)*sin(q3) + Ixx*dq1*dq2*cos(q2)*cos(q3)^2*sin(q2) - 2*Izz*dq1*dq2*cos(q2)*cos(q3)^2*sin(q2) - Ixx*dq1*dq2*cos(q2)*sin(q2)*sin(q3)^2 - 2*Ixx*dq1*dq3*cos(q3)*sin(q2)^2*sin(q3) + 2*Izz*dq1*dq3*cos(q3)*sin(q2)^2*sin(q3);
RHS(2,1) = Ixx*dq2*dq3*sin(2*q3) - CG*M*g*sin(q2) - Iyy*dq1*dq3*sin(q2) - (Iyy*dq1^2*sin(2*q2))/2 - Izz*dq2*dq3*sin(2*q3) + Izz*dq1^2*cos(q2)*cos(q3)^2*sin(q2) + Ixx*dq1^2*cos(q2)*sin(q2)*sin(q3)^2 - Ixx*dq1*dq3*cos(q3)^2*sin(q2) + Izz*dq1*dq3*cos(q3)^2*sin(q2) + Ixx*dq1*dq3*sin(q2)*sin(q3)^2 - Izz*dq1*dq3*sin(q2)*sin(q3)^2;
RHS(3,1) = (Izz*dq2^2*sin(2*q3))/2 - (Ixx*dq2^2*sin(2*q3))/2 + Iyy*dq1*dq2*sin(q2) + Ixx*dq1^2*cos(q3)*sin(q2)^2*sin(q3) - Izz*dq1^2*cos(q3)*sin(q2)^2*sin(q3) + Ixx*dq1*dq2*cos(q3)^2*sin(q2) - Izz*dq1*dq2*cos(q3)^2*sin(q2) - Ixx*dq1*dq2*sin(q2)*sin(q3)^2 + Izz*dq1*dq2*sin(q2)*sin(q3)^2;
c4science · Help