Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/EquationsofMotion_6DOF.m
73754adfee53master

Last Change
Raw File

EquationsofMotion_6DOF.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(1,4) = 0;
MDYN(1,5) = 0;
MDYN(1,6) = 0;
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(2,4) = 0;
MDYN(2,5) = 0;
MDYN(2,6) = 0;
MDYN(3,1) = Iyy*cos(q2);
MDYN(3,2) = 0;
MDYN(3,3) = Iyy;
MDYN(3,4) = 0;
MDYN(3,5) = 0;
MDYN(3,6) = 0;
MDYN(4,1) = 0;
MDYN(4,2) = 0;
MDYN(4,3) = 0;
MDYN(4,4) = M;
MDYN(4,5) = 0;
MDYN(4,6) = 0;
MDYN(5,1) = 0;
MDYN(5,2) = 0;
MDYN(5,3) = 0;
MDYN(5,4) = 0;
MDYN(5,5) = M;
MDYN(5,6) = 0;
MDYN(6,1) = 0;
MDYN(6,2) = 0;
MDYN(6,3) = 0;
MDYN(6,4) = 0;
MDYN(6,5) = 0;
MDYN(6,6) = M;
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;
RHS(4,1) = 0;
RHS(5,1) = -M*g;
RHS(6,1) = 0;
c4science · Help