Выкладываю конечный вариант моей расчетной работы

Исправлено:

1. Kij не равно Kji, это выполняется только для A.
2. Момент инерции стержней m(2*L)^2/12 т.к. длина стержня 2*L
3. p12, p21, p22, p32
4. f
5. M'

6. Кинетическая энергия тел (energy.m):
W1=((omega10'*(A01*J1*A10)*omega10)/2)+m*V1_2/2;
W2=((omega20'*(A02*J2*A20)*omega20)/2)+m*V2_2/2;
W3=((omega30'*(A03*J3*A30)*omega30)/2)+m*V3_2/2;
omega10, omega20, omega30 заданы в 0 СК, следовательно тензоры инерции
тоже должны быть в 0 СК.

На результат это не повлияло, энергия не сохраняется.