The following are the simplified dynamics using Mathematica and also using some manual simplifications for the trig functions.
/* Simplified dynamics equations for the first model */
M[1][1] = 2.*K3 + A2*K3 + K4 + 2*A2*K4 + K4*D3 + IZZ - 0.5*K4*L3 +
JYY*s2*s2 + KZZ*s2*s2 + 0.25*L2*L2*m2*s2*s2 -
A2*A2*m3*s2*s2 + JXX*c2*c2 + KXX c2*c2 -
L3*m3*c2 + JXY*sin(2*T2) + 0.5*KXZ*sin(2*T2) -
0.5*D3*m3*sin(2*T2) + 0.25*L3*m3*sin(2*T2) ;
M[1][2] = -1. + JYZ*s2 + KYZ*s2 - JXZ*c2 - KXY*c2 ;
M[1][3] = 0 ;
M[2][1] = -1. + JYZ*s2 + KYZ*s2 - JXZ*c2 - KXY*c2 ;
M[2][2] = 2.*K3 + K4 + K4*D3 + JZZ + KYY - 0.5*K4*L3 + 0.25*L2*m2 + A2*m3 - L3*m3 ;
M[2][3] = - A2*m3 ;
M[3][1] = 0 ;
M[3][2] = -A2*m3 ;
M[3][3] = m3 ;
=============================================================================
V[1] = -1. + 3.*K3 + 2.A2*K3 + 4.*K4 + K4*D3 - 0.5*K4*L3 -
0.25*L3*m3*vel_D3*vel_T1 +
0.25*L3*m3*cos(180+2*T2)*vel_D3*vel_T1 -
JXY*vel_T1*vel_T2 - KXZ*vel_T1*vel_T2 +
0.5*D3*m3*vel_T1*vel_T2 - 0.5*L3*m3*vel_T1*vel_T2 +
0.5*D3*m3*cos(180+2*T2)*vel_T1*vel_T2 +
JXZ*s2*vel_T2*vel_T2 + KXY*s2*vel_T2*vel_T2 -
JYZ*vel_T2*vel_T2*c2 - KYZ*vel_T2*vel_T2*c2 +
A2*m3*vel_D3*vel_T1*sin(180+2*T2) +
JXX*vel_T1*vel_T2*sin(180+2*T2) -
JYY*vel_T1*vel_T2*sin(180+2*T2) +
KXX*vel_T1*vel_T2*sin(180+2*T2) -
KZZ*vel_T1*vel_T2*sin(180+2*T2) -
0.25*L2*L2*m2*vel_T1*vel_T2*sin(180+2*T2) -
A2*A2*m3*vel_T1*vel_T2*sin(180+2*T2) -
0.5*L3*m3*vel_T1*vel_T2*sin(180+2*T2) ;
V[2] = -2. - K3 + 2.*A2*K3 + K4 + K4*D3 - 0.5 K4*L3 +
0.5*L3*m3*s2*vel_D3*vel_T1 - JXY*vel_T1*vel_T1
KXZ*vel_T1*vel_T1 + 0.5*D3*m3*vel_T1*vel_T1 -
0.5*L3*m3*vel_T1*vel_T1 -
0.5*D3*m3*cos(180+2*T2)*vel_T1*vel_T1 + D3*m3*vel_T2*vel_T2 -
0.5*JXX*vel_T1*vel_T1*sin(180+2*T2) + 0.5*JYY*vel_T1*vel_T1*sin(180+2 T2)
0.5*KXX*vel_T1*vel_T1*sin(180+2*T2) + 0.5*KZZ*vel_T1*vel_T1*sin(180+2*T2)
0.125*L2*L2*m2*vel_T1*vel_T1*sin(180+2*T2) +
0.5*A2*A2*m3*vel_T1*vel_T1*sin(180+2*T2) -
0.5*L3*m3*vel_T1*vel_T1*sin(180+2*T2) ;
V[3] = -2.*K3 - 2.*K4 + D3*m3*vel_T2*vel_T2 - 0.5*L3*m3*vel_T2*vel_T2 +
D3*m3*vel_T1*vel_T1*c2*c2 -
0.5*L3*m3*vel_T1*vel_T1*c2*c2 -
0.5*A2*m3*vel_T1*vel_T1*sin(180+2*T2) ;
===========================================================================
G[1] = -2.*K3 - 2.*K4 + D3*m3*vel_T2*vel_T2 - 0.5*L3*m3*vel_T2*vel_T2 +
D3*m3*vel_T1*vel_T1*c2*c2 -
0.5*L3*m3*vel_T1*vel_T1*c2*c2 -
0.5*A2*m3*vel_T1*vel_T1*sin(180+2*T2) ;
G[2] = K4 + 0.5*g*L3*m3*s2*s1 -
0.5*g*L2*m2*s1*c2 - A2*g*m3*s1*c2 ;
G[3] = g*m3*s1*c2 ;
============================================================================
/* Simplified dynamics equations for the second model */
M[1][1] = 4. + 3.*C4 - 2.*A2*C4 + IZZ - 0.5*C4*L3 +
JYY*cosT2*cosT2 + KZZ*cosT2*cosT2 + 0.25*L2*L2*m2*cosT2*cosT2 +
A2*A2*m3*cosT2*cosT2 + JXX*sinT2*sinT2 + KXX*sinT2*sinT2 +
D3*D3*m3*sinT2*sinT2 - 0.5*L3*m3*sinT2*sinT2 - 0.5*D3*L3*m3*sinT2*sinT2 -
0.5*JXY*sin2T2 - 0.5*KXZ*sin2T2 - A2*D3*m3*sin2T2 -
0.25*L3*m3*sin2T2) ;
M[1][2] = -2. - JYZ*cosT2 - KYZ*cosT2 - JXZ*sinT2 - KXY*sinT2 ;
M[1][3] = 0 ;
M[2][1] = -2. - JYZ*cosT2 - KYZ*cosT2 - JXZ*sinT2 - KXY*sinT2 ;
M[2][2] = 0.5*(6.*C4 + 2.*JZZ + 2.*KYY - C4*L3 + 0.5*L2*m2 + 2.*A2*m3 +
2.*D3*D3*m3 - L3*m3 - D3*L3*m3) ;
M[2][3] = A2*m3 ;
M[3][1] = 0 ;
M[3][2] = A2*m3 ;
M[3][3] = m3 ;
============================================================================
V[1] = 0.5*(-4. + 16.*C4 - C4*L3 + 2.*D3*m3*(vel_D3)*(vel_T1) -
0.5*L3*m3*(vel_D3)*(vel_T1) - 2.*D3*m3*cos2T2*(vel_D3)*(vel_T1) +
0.5*L3*m3*cos2T2*(vel_D3)*(vel_T1) - 2.*JXY*(vel_T1)*(vel_T2) -
2.*KXZ*(vel_T1)*(vel_T2) - L3*m3*(vel_T1)*(vel_T2) -
4.*A2*D3*m3*cos2T2*(vel_T1)*(vel_T2) - 2.*JXZ*cosT2*(vel_T2)*(vel_T2) -
2.*KXY*cosT2*(vel_T2)*(vel_T2) - 2.*JYZ*(vel_T2)*(vel_T2)*sinT2 -
2.*KYZ*(vel_T2)*(vel_T2)*sinT2 - 2.*A2*m3*(vel_D3)*(vel_T1)*sin2T2 +
2.*JXX*(vel_T1)*(vel_T2)*sin2T2 -
2.*JYY*(vel_T1)*(vel_T2)*sin2T2 +
2.*KXX*(vel_T1)*(vel_T2)*sin2T2 -
2.*KZZ*(vel_T1)*(vel_T2)*sin2T2 -
0.5*L2*L2*m2*(vel_T1)*(vel_T2)*sin2T2 -
2.*A2*A2*m3*(vel_T1)*(vel_T2)*sin2T2 +
2.*D3*D3*m3*(vel_T1)*(vel_T2)*sin2T2 -
0.5*L3*m3*(vel_T1)*(vel_T2)*sin2T2 -
0.5*D3*L3*m3*(vel_T1)*(vel_T2)*sin2T2) ;
V[2] = 0.5*(-4. - 10.*C4 - C4*L3 - 4.*D3*m3*cosT2*(vel_D3)*(vel_T1) -
L3*m3*cosT2*(vel_D3)*(vel_T1) - 2.*JXY*(vel_T1)*(vel_T1) -
2.*KXZ*(vel_T1)*(vel_T1) - L3*m3*(vel_T1)*(vel_T1) +
2.*A2*D3*m3*cos2T2*(vel_T1)*(vel_T1) - JXX*(vel_T1)*(vel_T1)*sin2T2 +
JYY*(vel_T1)*(vel_T1)*sin2T2 - KXX*(vel_T1)*(vel_T1)*sin2T2 +
KZZ*(vel_T1)*(vel_T1)*sin2T2 + 0.25*L2*L2*m2*(vel_T1)*(vel_T1)*sin2T2 +
A2*A2*m3*(vel_T1)*sin2T2 - D3*D3*m3*(vel_T1)*(vel_T1)*sin2T2 -
0.5*L3*m3*(vel_T1)*(vel_T1)*sin2T2 -
0.5*D3*L3*m3*(vel_T1)*(vel_T1)*sin2T2) ;
V[3] = 2.*(-C4 - 0.5*D3*m3*(vel_T2)*(vel_T2) -
0.25*L3*m3*(vel_T2)*(vel_T2) -
0.5*D3*m3*(vel_T1)*(vel_T1)*sinT2*sinT2 -
0.25*L3*m3*(vel_T1)*(vel_T1)*sinT2*sinT2 +
0.25*A2*m3*(vel_T1)*(vel_T1)*sin2T2) ;
=============================================================================
G[1] = -C4 + 0.5*g*L2*m2*cosT1*cosT2 +
A2*g*m3*cosT1*cosT2 - D3*g*m3*cosT1*sinT2 -
0.5*g*L3*m3*cosT1*sinT2 ;
G[2] = -C4 - D3*g*m3*cosT2*sinT1 -
0.5*g*L3*m3*cosT2*sinT1 -
0.5*g*L2*m2*sinT1*sinT2 - A2*g*m3*sinT1*sinT2 ;
G[3] = -g*m3*sinT1*sinT2 ;
=========================================================================
/* Simplified dynamics equations for the third model */
M[1][1] = 2 + IZZ + (JXX+JYY+KXX+KYY)/2.0 + 0.125*L2*L2*m2 +
A2*A2*m3/2.0 + 0.0625*L3*L3*m3 -
(JXX*cos2T2+JYY*cos2T2)/2.0 + 0.125*L2*L2*m2*cos2T2 +
A2*A2*m3*cos2T2/2.0 + 0.0625*L3*L3*m3*cos2T2 +
0.125*A2*L3*m3*cos2T2mT3 + 0.5*A2*L3*m3*cosT3 +
0.0625*L3*L3*m3*cos2T3 - (KXX cos2_T2pT3 +
KYY cos2_T2pT3)/2.0 + 0.0625*L3*L3*m3*cos2_T2pT3 +
0.375*A2*L3*m3*cos2T2pT3 -
(JXY*sin2T2 - KXY*sin2T2 - KXY*sin2T3)/2.0 ;
M[1][2] = -1 - JYZ*cosT2 - KYZ*cosT2mT3 - JXZ*sinT2 - KXZ*sinT2pT3 ;
M[1][3] = -(KYZ*cosT2*cosT3) - KXZ*cosT3*sinT2 - KXZ*cosT2*sinT3 ;
M[2][1] = -1 - JYZ*cosT2 - KYZ*cosT2mT3 - JXZ*sinT2 - KXZ*sinT2pT3
M[2][2] = JZZ + KZZ + 0.25*L2*L2*m2 + A2*A2*m3 + 0.25*L3*L3*m3 + *A2*L3*m3*cosT3 ;
M[2][3] = KZZ + 0.25*L3*L3*m3 + 0.5*A2*L3*m3*cosT3 ;
M[3][1] = -1 - KYZ*cosT2mT3 - KXZ*sinT2pT3 ;
M[3][2] = JZZ + KZZ + 0.25*L2*L2*m2 + A2*A2*m3 + 0.25*L3*L3*m3 + A2*L3*m3*cosT3 ;
M[3][3] = KZZ + 0.25*L3*L3*m3 ;
/* ============================================================================== */
V[1] = (-992 - KXZ*cosT2m3T3*theta_dot[1]*theta_dot[1] -
7*KXZ*cosT2mT3*theta_dot[1]*theta_dot[1] +
KXZ*cos3_T2mT3*theta_dot[1]*theta_dot[1] -
KXZ*cos3T2mT3*theta_dot[1]*theta_dot[1] +
5*KXZ*cosT2pT3*theta_dot[1]*theta_dot[1] -
3*KXZ*cos3_T2pT3*theta_dot[1]*theta_dot[1] +
3*KXZ*cos3T3pT3*theta_dot[1]*theta_dot[1] +
3*KXZ*cosT2p3T3*theta_dot[1]*theta_dot[1] -
32*JXY*theta_dot[1]*theta_dot[2] -
32*KXY*theta_dot[1]*theta_dot[2] -
4*KXY*cos2_T2mT3*theta_dot[1]*theta_dot[2] +
4*KXY*cos2_T2pT3*theta_dot[1]*theta_dot[2] -
32*JXZ*cosT2*theta_dot[2]*theta_dot[2] -
16*KXZ*cosT2mT3*theta_dot[2]*theta_dot[2] -
16*KXZ*cosT2pT3*theta_dot[2]*theta_dot[2] -
32*KXY*theta_dot[1]*theta_dot[3] -
4*KXY*cos2_T2mT3*theta_dot[1]*theta_dot[3] +
4*KXY*cos2_T2pT3*theta_dot[1]*theta_dot[3] -
16*KXZ*cosT2mT3*theta_dot[2]*theta_dot[3] -
16*KXZ*cosT2pT3*theta_dot[2]*theta_dot[3] -
16*KXZ*cosT2mT3*theta_dot[3]*theta_dot[3] -
16*KXZ*cosT2pT3*theta_dot[3]*theta_dot[3] -
32*JYZ*theta_dot[2]*theta_dot[2]*sinT2 +
32*JXX*theta_dot[1]*theta_dot[2]*sin2T2 -
32*JYY*theta_dot[1]*theta_dot[2]*sin2T2 +
8*KXX*theta_dot[1]*theta_dot[2]*sin2T2 -
8*KZZ*theta_dot[1]*theta_dot[2]*sin2T2 -
8.*L2*L2*m2*theta_dot[1]*theta_dot[2]*sin2T2 -
32*A2*A2*m3*theta_dot[1]*theta_dot[2]*sin2T2 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[2]*sin2T2 +
8*KXX*theta_dot[1]*theta_dot[3]*sin2T2 -
8*KZZ*theta_dot[1]*theta_dot[3]*sin2T2 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[3]*sin2T2 +
4*KYZ*theta_dot[1]*theta_dot[1]*sinT2m3T3 -
8.*A2*L3*m3*theta_dot[1]*theta_dot[2]*sin2T2mT3 -
4*KYZ*theta_dot[1]*theta_dot[1]*sin3T2mT3 -
16.*A2*L3*m3*theta_dot[1]*theta_dot[2]*sinT3 -
16.*A2*L3*m3*theta_dot[1]*theta_dot[3]*sinT3 +
8*KXX*theta_dot[1]*theta_dot[2]*sin2T3 -
8*KZZ*theta_dot[1]*theta_dot[2]*sin2T3 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[2]*sin2T3 +
8*KXX*theta_dot[1]*theta_dot[3]*sin2T3 -
8*KZZ*theta_dot[1]*theta_dot[3]*sin2T3 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[3]*sin2T3 -
8*KYZ*theta_dot[1]*theta_dot[1]*sinT2pT3 -
32*KYZ*theta_dot[2]*theta_dot[2]*sinT2pT3 -
32*KYZ*theta_dot[2]*theta_dot[3]*sinT2pT3 -
32*KYZ*theta_dot[3]*theta_dot[3]*sinT2pT3 +
24*KXX*theta_dot[1]*theta_dot[2]*sin2_T2pT3 -
32*KYY*theta_dot[1]*theta_dot[2]*sin2_T2pT3 +
8*KZZ*theta_dot[1]*theta_dot[2]*sin2_T2pT3 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[2]*sin2_T2pT3 +
24*KXX*theta_dot[1]*theta_dot[3]*sin2_T2pT3 -
32*KYY*theta_dot[1]*theta_dot[3]*sin2_T2pT3 +
8*KZZ*theta_dot[1]*theta_dot[3]*sin2_T2pT3 -
4.*L3*L3*m3*theta_dot[1]*theta_dot[3]*sin2_T2pT3 -
24.*A2*L3*m3*theta_dot[1]*theta_dot[2]*sin2T2pT3 -
16.*A2*L3*m3*theta_dot[1]*theta_dot[3]*sin2T2pT3 +
4*KYZ*theta_dot[1]*theta_dot[1]*sin3T3pT3 +
4*KYZ*theta_dot[1]*theta_dot[1]*sinT2p3T3) / 32 ;
V[2] = 1 - JXY*theta_dot[1]*theta_dot[1] -
KXY*theta_dot[1]*theta_dot[1] -
(KXY*cos2_T2mT3*theta_dot[1]*theta_dot[1])/8 +
KXY*cos2_T2pT3*theta_dot[1]*theta_dot[1]/8 -
JXX*theta_dot[1]*theta_dot[1]*sin2T2/2
JYY*theta_dot[1]*theta_dot[1]*sin2T2/2 +
0.125 L2*L2 m2 (theta_dot[1]*theta_dot[1]*sin2T2 +
A2*A2*m3*theta_dot[1]*theta_dot[1]*sin2T2/2 -
A2*L3*m3*theta_dot[2]*theta_dot[3]*sinT3 -
0.5*A2*L3*m3*theta_dot[3]*theta_dot[3]*sinT3 -
KXX*theta_dot[1]*theta_dot[1]*sin2_T2pT3/2 +
KYY*theta_dot[1]*theta_dot[1]*sin2_T2pT3/2 +
0.125*L3*L3*m3*theta_dot[1]*theta_dot[1]*sin2_T2pT3 +
0.5*A2*L3*m3*theta_dot[1]*theta_dot[1]*sin2T2pT3 ;
V[3] = (16 - 8*KXY*theta_dot[1]*theta_dot[1] -
KXY*cos2_T2mT3*theta_dot[1]*theta_dot[1] +
KXY*cos2_T2pT3*theta_dot[1]*theta_dot[1] +
2.*A2*L3*m3*theta_dot[1]*theta_dot[1]*sinT3 +
4.*A2*L3*m3*theta_dot[2]*theta_dot[2]*sinT3 -
4*KXX*theta_dot[1]*theta_dot[1]*sin2_T2pT3 +
4*KYY*theta_dot[1]*theta_dot[1]*sin2_T2pT3 +
L3*L3*m3*theta_dot[1]*theta_dot[1]*sin2_T2pT3 +
2.*A2*L3*m3*theta_dot[1]*theta_dot[1]*sin2T2pT3) / 8 ;
============================================================================
G[1] = 0.5*(g*L2*m2*cosT1*cosT2 +
2.*A2*g*m3*cosT1*cosT2 +
g*L3*m3*cosT1*cosT2*cosT3) ;
G[2] = 0.5*(-1. g*L2*m2*sinT1*sinT2 -
g*L3*m3*cosT3*sinT1*sinT2 -
2.*A2*g*m3*cosT3*cosT3*sinT1*sinT2 -
2.*A2*g*m3*sinT1*sinT2*sinT3*sinT3 +
A2*g*m3*cosT2*sinT1*sin2T3) ;
G[3] = -0.5*g*L3*m3*cosT3*sinT1*sinT2 ;
==============================================================================