ClearAll;
  
  (*wbeta[t] =
  wepsilon[t] =
  b = 0.09
  a = 0.04
  mM = 1.500
  RM = 0.02
  LM = 0.22
 mK = 1.500
 RK = 0.03
 LK = 0.12*)
 
 
 "Kräfte"
 
 KraftO = { FOx[t ], FOy[t] , FOz[t] };
 KraftB = { FBx[t] , FBy[t] , FBz[t] };
 
 (* Gewichtskraft im K System : *)
 
 KraftG = {  0, 0 , -(mM + mK)*9.81 };
 
 MatrixForm[KraftO]
 MatrixForm[KraftB]
 MatrixForm[KraftG]
 
 
 "Winkel"
 
 alpha = 45 \[Degree]
 beta = wbeta[t]
 gamma = 55 \[Degree]
 epsilon = wepsilon[t]
 delta = 25 \[Degree]
 
 "Winkel-Geschwindigkeiten"
 
 valpha = { 0 , 0, 0 }
 vbeta = { 0, 0, D[beta, t] }
 vgamma = { 0, 0, 0}
 vepsilon = { 0, 0, D[epsilon, t]}
 
 "Winkel-Beschleunigungen"
 
 aalpha = { 0, 0, 0}
 abeta = { 0, 0, D[D[beta, t]]}
 agamma = { 0, 0, 0}
 aepsilon = { 0, 0, D[D[epsilon, t]] }
 
 "1. Rotation um y (math negativ)"
 
 AAI = {{ Cos[alpha] , 0 , Sin[alpha] } , {0, 1, 0}, {-Sin[alpha], 0,
     Cos[alpha]}};
 
 MatrixForm[AAI]
 
 "2. Erstes Gelenk: Rotation um z (math negativ)"
 
 ABA = {{ Cos[beta] , Sin[beta] , 0 } , {-Sin[beta] , Cos[beta] ,
     0}, {0 , 0 , 1 }};
 
 MatrixForm[ABA]
 
 "3. Winkel im Stab: Rotation um y (math positiv)"
 
 ACB = {{Cos[gamma] , 0 , -Sin[gamma] }, { 0, 1 , 0 }, { Sin[gamma],
     0 , Cos[gamma] }};
 
 MatrixForm[ACB]
 
 "4. Zweites Gelenk: Rotation um z (math positiv)"
 
 AKC = {{ Cos[epsilon] , Sin[epsilon] , 0 }, { -Sin[epsilon] ,
     Cos[epsilon] , 0 }, { 0 , 0 , 1 }};
 
 MatrixForm[AKC]
 
 "5. Drehung von K System zum Kopf um y ( math positiv)"
 
 AEK = {{ Cos[delta] , 0 , -Sin[delta]}, {0, 1, 0}, {Sin[delta] , 0 ,
     Cos[delta]}};
 
 MatrixForm[AEK]
 
 "Ortsvektoren"
 
 ra =  { x_s , 0 , z_s };
 rb = {b, 0, 0};
 rc = {a, 0, 0}  ;
 
 MatrixForm[ra]
 MatrixForm[rb]
 MatrixForm[rc]
 
 
 "Resultierender Ortsvektor bestimmen"
 
 rOS = ra + (AKC . rb) + (AKC . ACB . rc);

rOB =  ra + (AKC . rb);

MatrixForm[rOS]

"abgeleiteter Ortsvektor"

rOSabl = D[rOS, t];



MatrixForm[rOSabl]

"Winkelgeschwindigkeit von S im K-System"

omegaS = vepsilon + AKC . vgamma + AKC . ACB . vbeta +
   AKC . ACB . ABA . valpha;



MatrixForm[omegaS]

"Geschwindigkeiten von S im K-System"

vS = rOSabl + omegaS \[Cross] rOS;



MatrixForm[vS]

"abgeleiteter Geschwindigkeitsvektor "

vOSabl = D[vS, t];



MatrixForm[vOSabl]

"Beschleunigung von S im K-System"

aS = vOSabl + omegaS  \[Cross] vS;



MatrixForm[aS]

"Winkelbeschleunigung von S im K-System"

PsiS = D[omegaS, t];

MatrixForm[PsiS]

"Trägheitstensor Motor"

ThetaM = {{(3/20) mM  RM^2 + (3/80) mM LM^2, 0,
    0}, {0, (3/20) mM RM^2 + (3/80) mM LM^2  , 0} , { 0,
    0, (3/10) mM RM^2 }};



MatrixForm[ThetaM]

"Trägheitstensor Kopf"

ThetaK = {{(3/20) mK  RK^2 + (3/80) mK LK^2, 0,
    0}, {0, (3/20) mK RK^2 + (3/80) mK LK^2  , 0} , { 0,
    0, (3/10) mK RK^2 }};


MatrixForm[ThetaK]

"Transformation vom Kopf ins K-System"

ThetaKK = AEK . ThetaK . Transpose[AEK];



MatrixForm[ThetaKK]

"Totales Trägheitsmoment"

ThetaTot = ThetaM + ThetaKK ;

MatrixForm[ThetaTot]




"Momentengleichgewicht im Ursprung"

MO = rOB  \[Cross] KraftB + rOS \[Cross] KraftG;

MatrixForm[MO]


"Impulssatz bezüglich Schwerpunkt"

p = (mM + mK) * vS;

MatrixForm[p]

"Drallsatz bezügliche Schwerpunkt"
(* Drall muss immer bezüglich eines festen Punktes gemacht werden, \
dieser ist O *)

LO = rOS \[Cross] p + ThetaTot . omegaS;

MatrixForm[LO]

"Ableitung des Impulssatzes"

pabl =  D[p, t] + omegaS \[Cross] p;

MatrixForm[pabl]

"Ableitung des Drallsatzes"

LOabl = D[pabl, t] + omegaS \[Cross] LO;

MatrixForm[LOabl]


"Nach Kräften im Punkt B auflösen/ Solving for forces, everything works fine until here, the error occures" (* Idee funktioniert*)

Solve[ {LOabl[[1]] == MO[[1]] , LOabl[[2]] == MO[[2]] ,
    LOabl[[3]] == MO[[3]] } {FBx[t] , FBy[t] , FBz[t]}];


"Kräfte im Punkt 0 im A - System"

FOA = - AKC . ACB . ABA . (-KraftB);

MatrixForm[FOA]