The following is the dynamics and kinematics of the three robot models which was generated form the gendyn program. These dynamics equations are not simplified.
/* Dynamics equations for the first model */ #include <math.h> #include "dyn1.h" void dyn1Dyn (M, V, G, F, J_pos, J_vel, B_acc, External_F, External_M) double **M, *V, *G, *F, *J_pos, *J_vel, *B_acc, *External_F, *External_M; { double external_force_x = External_F[ 0]; double external_force_y = External_F[ 1]; double external_force_z = External_F[ 2]; double external_moment_x = External_M[ 0]; double external_moment_y = External_M[ 1]; double external_moment_z = External_M[ 2]; double base_x = B_acc[ 0]; double base_y = B_acc[ 1]; double base_z = B_acc[ 2]; double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double D3 = J_pos[ 2]; double vel_T1 = J_vel[ 0]; double vel_T2 = J_vel[ 1]; double vel_D3 = J_vel[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); M[ 0][ 0] = 2 * -JXY * cosT2+90 * sinT2+90 - 2.0 * -KXZ * cosT2+90 * sinT2+90 + 0.5*L2 * 0.5*L2 * M2 * cosT2+90 * cosT2+90 + A2 * A2 * M3 * cosT2+90 * cosT2+90 + A2 * C3+D3 * M3 * cosT2+90 * sinT2+90 + 2 * A2 * C4-0.5*L3 * M3 * cosT2+90 * sinT2+90 + 2.0 * C3+D3 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 + C4-0.5*L3 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 + IZZ + JXX * sinT2+90 * sinT2+90 + JYY * cosT2+90 * cosT2+90 + KXX * sinT2+90 * sinT2+90 + KZZ * cosT2+90 * cosT2+90 ; M[ 0][ 1] = -JXZ * sinT2+90 + -JYZ * cosT2+90 + -KXY * sinT2+90 - 1.0 * -KYZ * cosT2+90 ; M[ 0][ 2] = 0 ; M[ 1][ 0] = -JXZ * sinT2+90 + -JYZ * cosT2+90 + -KXY * sinT2+90 - 1.0 * -KYZ * cosT2+90 ; M[ 1][ 1] = 0.5*L2 * 0.5*L2 * M2 + A2 * A2 * M3 + 2.0 * C3+D3 * C4-0.5*L3 * M3 + C4-0.5*L3 * C4-0.5*L3 * M3 + JZZ + KYY ; M[ 1][ 2] = -1.0 * A2 * M3 ; M[ 2][ 0] = 0 ; M[ 2][ 1] = -1.0 * A2 * M3 ; M[ 2][ 2] = M3 ; V[ 0] = 2 * -JXY * cosT2+90 * cosT2+90 * vel_T1 * vel_T2 - 2 * -JXY * sinT2+90 * sinT2+90 * vel_T1 * vel_T2 + -JXZ * cosT2+90 * vel_T2 * vel_T2 - 1 * -JYZ * sinT2+90 * vel_T2 * vel_T2 + -KXY * cosT2+90 * vel_T2 * vel_T2 - 2.0 * -KXZ * cosT2+90 * cosT2+90 * vel_T1 * vel_T2 + 2.0 * -KXZ * sinT2+90 * sinT2+90 * vel_T1 * vel_T2 + -KYZ * sinT2+90 * vel_T2 * vel_T2 - 2 * 0.5*L2 * 0.5*L2 * M2 * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 - 2 * A2 * A2 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 + 2.0 * A2 * C3+D3 * M3 * cosT2+90 * cosT2+90 * vel_T1 * vel_T2 + 2.0 * A2 * C4-0.5*L3 * M3 * cosT2+90 * cosT2+90 * vel_T1 * vel_T2 - 2 * A2 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 * vel_T1 * vel_T2 + 2 * A2 * M3 * cosT2+90 * sinT2+90 * vel_D3 * vel_T1 + 3.0 * C3+D3 * C4-0.5*L3 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 + 2.0 * C4-0.5*L3 * C4-0.5*L3 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 + 2 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 * vel_D3 * vel_T1 + 2 * JXX * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 - 2 * JYY * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 + 2.0 * KXX * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 - 2.0 * KZZ * cosT2+90 * sinT2+90 * vel_T1 * vel_T2 ; V[ 1] = -1 * -JXY * cosT2+90 * cosT2+90 * vel_T1 * vel_T1 + -JXY * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 + -KXZ * cosT2+90 * cosT2+90 * vel_T1 * vel_T1 - 1 * -KXZ * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 + 0.5*L2 * 0.5*L2 * M2 * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 + A2 * A2 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 + A2 * C3+D3 * M3 * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 + A2 * C3+D3 * M3 * vel_T2 * vel_T2 - 1 * A2 * C4-0.5*L3 * M3 * cosT2+90 * cosT2+90 * vel_T1 * vel_T1 + A2 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1.0 * C3+D3 * C4-0.5*L3 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1.0 * C4-0.5*L3 * C4-0.5*L3 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 + 2 * C4-0.5*L3 * M3 * cosT2+90 * vel_D3 * vel_T1 - 1 * JXX * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 + JYY * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1.0 * KXX * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 + KZZ * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 ; V[ 2] = -1.0 * A2 * M3 * cosT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1.0 * C3+D3 * M3 * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1.0 * C3+D3 * M3 * vel_T2 * vel_T2 - 1 * C4-0.5*L3 * M3 * sinT2+90 * sinT2+90 * vel_T1 * vel_T1 - 1 * C4-0.5*L3 * M3 * vel_T2 * vel_T2 ; G[ 0] = 0.5*L2 * GRAVITY * M2 * cosT1 * cosT2+90 + A2 * GRAVITY * M3 * cosT1 * cosT2+90 + C4-0.5*L3 * GRAVITY * M3 * cosT1 * sinT2+90 ; G[ 1] = -1 * 0.5*L2 * GRAVITY * M2 * sinT1 * sinT2+90 - 1.0 * A2 * GRAVITY * M3 * sinT1 * sinT2+90 + C4-0.5*L3 * GRAVITY * M3 * cosT2+90 * sinT1 ; G[ 2] = GRAVITY * M3 * sinT1 * sinT2+90 ; /* Torque due to external moments */ F[ 0] = -1.0 * cosT2+90 * external_moment_z + external_moment_x * sinT2+90 ; F[ 1] = external_moment_y ; F[ 2] = 0 ; /* Torque due to external forces */ F[ 0] += -1 * A2 * cosT2+90 * external_force_y - 1.0 * C3+D3 * external_force_y * sinT2+90 - 1 * C4 * external_force_y * sinT2+90 ; F[ 1] += -1.0 * A2 * external_force_z + C3+D3 * external_force_x + C4 * external_force_x ; F[ 2] += external_force_z ; /* Torque due to base movement */ F[ 0] += 0.5*L2 * M2 * base_y * cosT1 * cosT2+90 + A2 * M3 * base_y * cosT1 * cosT2+90 + C4-0.5*L3 * M3 * base_y * cosT1 * sinT2+90 ; F[ 1] += -1 * 0.5*L2 * M2 * base_x * cosT1 * sinT2+90 - 1 * 0.5*L2 * M2 * base_y * sinT1 * sinT2+90 + 0.5*L2 * M2 * base_z * cosT2+90 - 1.0 * A2 * M3 * base_x * cosT1 * sinT2+90 - 1.0 * A2 * M3 * base_y * sinT1 * sinT2+90 + A2 * M3 * base_z * cosT2+90 + C4-0.5*L3 * M3 * base_x * cosT1 * cosT2+90 + C4-0.5*L3 * M3 * base_y * cosT2+90 * sinT1 + C4-0.5*L3 * M3 * base_z * sinT2+90 ; F[ 2] += M3 * base_x * cosT1 * sinT2+90 + M3 * base_y * sinT1 * sinT2+90 - 1.0 * M3 * base_z * cosT2+90 ; /* Force due to friction forces */ F[ 0] += 0.0 ; F[ 1] += 0.0 ; F[ 2] += 0.0 ; } void dyn1Frm (TransformList, J_pos) double ***TransformList, *J_pos; { double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double D3 = J_pos[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); double **T; T = TransformList[ 0]; T[ 0][ 0] = cosT1 ; T[ 0][ 1] = -1 * sinT1 ; T[ 0][ 2] = 0 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = sinT1 ; T[ 1][ 1] = cosT1 ; T[ 1][ 2] = 0 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = 0 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = 1 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 1]; T[ 0][ 0] = cosT1 * cosT2+90 ; T[ 0][ 1] = -1 * cosT1 * sinT2+90 ; T[ 0][ 2] = sinT1 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = cosT2+90 * sinT1 ; T[ 1][ 1] = -1 * sinT1 * sinT2+90 ; T[ 1][ 2] = -1.0 * cosT1 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = sinT2+90 ; T[ 2][ 1] = cosT2+90 ; T[ 2][ 2] = 0 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 2]; T[ 0][ 0] = cosT1 * cosT2+90 ; T[ 0][ 1] = sinT1 ; T[ 0][ 2] = cosT1 * sinT2+90 ; T[ 0][ 3] = A2 * cosT1 * cosT2+90 + C3+D3 * cosT1 * sinT2+90 ; T[ 1][ 0] = cosT2+90 * sinT1 ; T[ 1][ 1] = -1.0 * cosT1 ; T[ 1][ 2] = sinT1 * sinT2+90 ; T[ 1][ 3] = A2 * cosT2+90 * sinT1 + C3+D3 * sinT1 * sinT2+90 ; T[ 2][ 0] = sinT2+90 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = -1.0 * cosT2+90 ; T[ 2][ 3] = A2 * sinT2+90 - 1.0 * C3+D3 * cosT2+90 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 3]; T[ 0][ 0] = cosT1 * cosT2+90 ; T[ 0][ 1] = sinT1 ; T[ 0][ 2] = cosT1 * sinT2+90 ; T[ 0][ 3] = C4 * cosT1 * sinT2+90 + A2 * cosT1 * cosT2+90 + C3+D3 * cosT1 * sinT2+90 ; T[ 1][ 0] = cosT2+90 * sinT1 ; T[ 1][ 1] = -1.0 * cosT1 ; T[ 1][ 2] = sinT1 * sinT2+90 ; T[ 1][ 3] = C4 * sinT1 * sinT2+90 + A2 * cosT2+90 * sinT1 + C3+D3 * sinT1 * sinT2+90 ; T[ 2][ 0] = sinT2+90 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = -1.0 * cosT2+90 ; T[ 2][ 3] = -1.0 * C4 * cosT2+90 + A2 * sinT2+90 - 1.0 * C3+D3 * cosT2+90 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; } /****************************************************************************/ /* Dynamics equations for the second model */ #include <math.h> #include "dyn2.h" void dyn2Dyn (M, V, G, F, J_pos, J_vel, B_acc, External_F, External_M) double **M, *V, *G, *F, *J_pos, *J_vel, *B_acc, *External_F, *External_M; { double external_force_x = External_F[ 0]; double external_force_y = External_F[ 1]; double external_force_z = External_F[ 2]; double external_moment_x = External_M[ 0]; double external_moment_y = External_M[ 1]; double external_moment_z = External_M[ 2]; double base_x = B_acc[ 0]; double base_y = B_acc[ 1]; double base_z = B_acc[ 2]; double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double D3 = J_pos[ 2]; double vel_T1 = J_vel[ 0]; double vel_T2 = J_vel[ 1]; double vel_D3 = J_vel[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); M[ 0][ 0] = 2 * -JXY * cosT2 * sinT2 + 2 * -KXZ * cosT2 * sinT2 + 0.5*L2 * 0.5*L2 * M2 * cosT2 * cosT2 + A2 * A2 * M3 * cosT2 * cosT2 - 2.0 * A2 * C4-0.5*L3 * M3 * cosT2 * sinT2 - 2.0 * A2 * D3 * M3 * cosT2 * sinT2 + C4-0.5*L3 * C4-0.5*L3 * M3 * sinT2 * sinT2 + 2.0 * C4-0.5*L3 * D3 * M3 * sinT2 * sinT2 + D3 * D3 * M3 * sinT2 * sinT2 + IZZ + JXX * sinT2 * sinT2 + JYY * cosT2 * cosT2 + KXX * sinT2 * sinT2 + KZZ * cosT2 * cosT2 ; M[ 0][ 1] = -JXZ * sinT2 + -JYZ * cosT2 - 1.0 * -KXY * sinT2 - 1.0 * -KYZ * cosT2 ; M[ 0][ 2] = 0 ; M[ 1][ 0] = -JXZ * sinT2 + -JYZ * cosT2 - 1.0 * -KXY * sinT2 - 1.0 * -KYZ * cosT2 ; M[ 1][ 1] = 0.5*L2 * 0.5*L2 * M2 + A2 * A2 * M3 + C4-0.5*L3 * C4-0.5*L3 * M3 + 2.0 * C4-0.5*L3 * D3 * M3 + D3 * D3 * M3 + JZZ + KYY ; M[ 1][ 2] = A2 * M3 ; M[ 2][ 0] = 0 ; M[ 2][ 1] = A2 * M3 ; M[ 2][ 2] = M3 ; V[ 0] = 2 * -JXY * cosT2 * cosT2 * vel_T1 * vel_T2 - 2 * -JXY * sinT2 * sinT2 * vel_T1 * vel_T2 + -JXZ * cosT2 * vel_T2 * vel_T2 - 1 * -JYZ * sinT2 * vel_T2 * vel_T2 - 1.0 * -KXY * cosT2 * vel_T2 * vel_T2 + 2.0 * -KXZ * cosT2 * cosT2 * vel_T1 * vel_T2 - 2.0 * -KXZ * sinT2 * sinT2 * vel_T1 * vel_T2 + -KYZ * sinT2 * vel_T2 * vel_T2 - 2 * 0.5*L2 * 0.5*L2 * M2 * cosT2 * sinT2 * vel_T1 * vel_T2 - 2.0 * A2 * A2 * M3 * cosT2 * sinT2 * vel_T1 * vel_T2 - 2.0 * A2 * C4-0.5*L3 * M3 * cosT2 * cosT2 * vel_T1 * vel_T2 + 2.0 * A2 * C4-0.5*L3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T2 - 2.0 * A2 * D3 * M3 * cosT2 * cosT2 * vel_T1 * vel_T2 + 2.0 * A2 * D3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T2 - 2.0 * A2 * M3 * cosT2 * sinT2 * vel_D3 * vel_T1 + 2.0 * C4-0.5*L3 * C4-0.5*L3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T2 + 4.0 * C4-0.5*L3 * D3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T2 + 2 * C4-0.5*L3 * M3 * sinT2 * sinT2 * vel_D3 * vel_T1 + 2.0 * D3 * D3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T2 + 2.0 * D3 * M3 * sinT2 * sinT2 * vel_D3 * vel_T1 + 2 * JXX * cosT2 * sinT2 * vel_T1 * vel_T2 - 2 * JYY * cosT2 * sinT2 * vel_T1 * vel_T2 + 2.0 * KXX * cosT2 * sinT2 * vel_T1 * vel_T2 - 2.0 * KZZ * cosT2 * sinT2 * vel_T1 * vel_T2 ; V[ 1] = -1 * -JXY * cosT2 * cosT2 * vel_T1 * vel_T1 + -JXY * sinT2 * sinT2 * vel_T1 * vel_T1 - 1.0 * -KXZ * cosT2 * cosT2 * vel_T1 * vel_T1 + -KXZ * sinT2 * sinT2 * vel_T1 * vel_T1 + 0.5*L2 * 0.5*L2 * M2 * cosT2 * sinT2 * vel_T1 * vel_T1 + A2 * A2 * M3 * cosT2 * sinT2 * vel_T1 * vel_T1 + A2 * C4-0.5*L3 * M3 * cosT2 * cosT2 * vel_T1 * vel_T1 - 1 * A2 * C4-0.5*L3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T1 + A2 * D3 * M3 * cosT2 * cosT2 * vel_T1 * vel_T1 - 1 * A2 * D3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T1 - 1.0 * C4-0.5*L3 * C4-0.5*L3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T1 - 2.0 * C4-0.5*L3 * D3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T1 - 2.0 * C4-0.5*L3 * M3 * cosT2 * vel_D3 * vel_T1 - 1 * D3 * D3 * M3 * cosT2 * sinT2 * vel_T1 * vel_T1 - 2 * D3 * M3 * cosT2 * vel_D3 * vel_T1 - 1 * JXX * cosT2 * sinT2 * vel_T1 * vel_T1 + JYY * cosT2 * sinT2 * vel_T1 * vel_T1 - 1.0 * KXX * cosT2 * sinT2 * vel_T1 * vel_T1 + KZZ * cosT2 * sinT2 * vel_T1 * vel_T1 ; V[ 2] = A2 * M3 * cosT2 * sinT2 * vel_T1 * vel_T1 - 1 * C4-0.5*L3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T1 - 1.0 * C4-0.5*L3 * M3 * vel_T2 * vel_T2 - 1 * D3 * M3 * sinT2 * sinT2 * vel_T1 * vel_T1 - 1 * D3 * M3 * vel_T2 * vel_T2 ; G[ 0] = 0.5*L2 * GRAVITY * M2 * cosT1 * cosT2 + A2 * GRAVITY * M3 * cosT1 * cosT2 - 1.0 * C4-0.5*L3 * GRAVITY * M3 * cosT1 * sinT2 - 1.0 * D3 * GRAVITY * M3 * cosT1 * sinT2 ; G[ 1] = -1 * 0.5*L2 * GRAVITY * M2 * sinT1 * sinT2 - 1 * A2 * GRAVITY * M3 * sinT1 * sinT2 - 1.0 * C4-0.5*L3 * GRAVITY * M3 * cosT2 * sinT1 - 1 * D3 * GRAVITY * M3 * cosT2 * sinT1 ; G[ 2] = -1 * GRAVITY * M3 * sinT1 * sinT2 ; /* Torque due to external moments */ F[ 0] = cosT2 * external_moment_z + external_moment_x * sinT2 ; F[ 1] = -1.0 * external_moment_y ; F[ 2] = 0 ; /* Torque due to external forces */ F[ 0] += A2 * cosT2 * external_force_y - 1 * C4 * external_force_y * sinT2 - 1.0 * D3 * external_force_y * sinT2 ; F[ 1] += A2 * external_force_z - 1.0 * C4 * external_force_x - 1 * D3 * external_force_x ; F[ 2] += external_force_z ; /* Torque due to base movement */ F[ 0] += 0.5*L2 * M2 * base_y * cosT1 * cosT2 + A2 * M3 * base_y * cosT1 * cosT2 - 1.0 * C4-0.5*L3 * M3 * base_y * cosT1 * sinT2 - 1.0 * D3 * M3 * base_y * cosT1 * sinT2 ; F[ 1] += -1 * 0.5*L2 * M2 * base_x * cosT1 * sinT2 - 1 * 0.5*L2 * M2 * base_y * sinT1 * sinT2 + 0.5*L2 * M2 * base_z * cosT2 - 1 * A2 * M3 * base_x * cosT1 * sinT2 - 1 * A2 * M3 * base_y * sinT1 * sinT2 + A2 * M3 * base_z * cosT2 - 1.0 * C4-0.5*L3 * M3 * base_x * cosT1 * cosT2 - 1.0 * C4-0.5*L3 * M3 * base_y * cosT2 * sinT1 - 1.0 * C4-0.5*L3 * M3 * base_z * sinT2 - 1 * D3 * M3 * base_x * cosT1 * cosT2 - 1 * D3 * M3 * base_y * cosT2 * sinT1 - 1 * D3 * M3 * base_z * sinT2 ; F[ 2] += -1 * M3 * base_x * cosT1 * sinT2 - 1 * M3 * base_y * sinT1 * sinT2 + M3 * base_z * cosT2 ; /* Force due to friction forces */ F[ 0] += 0.0 ; F[ 1] += 0.0 ; F[ 2] += 0.0 ; } void dyn2Frm (TransformList, J_pos) double ***TransformList, *J_pos; { double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double D3 = J_pos[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); double **T; T = TransformList[ 0]; T[ 0][ 0] = cosT1 ; T[ 0][ 1] = -1 * sinT1 ; T[ 0][ 2] = 0 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = sinT1 ; T[ 1][ 1] = cosT1 ; T[ 1][ 2] = 0 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = 0 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = 1 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 1]; T[ 0][ 0] = cosT1 * cosT2 ; T[ 0][ 1] = -1 * cosT1 * sinT2 ; T[ 0][ 2] = sinT1 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = cosT2 * sinT1 ; T[ 1][ 1] = -1 * sinT1 * sinT2 ; T[ 1][ 2] = -1.0 * cosT1 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = sinT2 ; T[ 2][ 1] = cosT2 ; T[ 2][ 2] = 0 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 2]; T[ 0][ 0] = cosT1 * cosT2 ; T[ 0][ 1] = -1.0 * sinT1 ; T[ 0][ 2] = -1 * cosT1 * sinT2 ; T[ 0][ 3] = A2 * cosT1 * cosT2 - 1 * D3 * cosT1 * sinT2 ; T[ 1][ 0] = cosT2 * sinT1 ; T[ 1][ 1] = cosT1 ; T[ 1][ 2] = -1 * sinT1 * sinT2 ; T[ 1][ 3] = A2 * cosT2 * sinT1 - 1 * D3 * sinT1 * sinT2 ; T[ 2][ 0] = sinT2 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = cosT2 ; T[ 2][ 3] = A2 * sinT2 + D3 * cosT2 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 3]; T[ 0][ 0] = cosT1 * cosT2 ; T[ 0][ 1] = -1.0 * sinT1 ; T[ 0][ 2] = -1 * cosT1 * sinT2 ; T[ 0][ 3] = -1 * C4 * cosT1 * sinT2 + A2 * cosT1 * cosT2 - 1 * D3 * cosT1 * sinT2 ; T[ 1][ 0] = cosT2 * sinT1 ; T[ 1][ 1] = cosT1 ; T[ 1][ 2] = -1 * sinT1 * sinT2 ; T[ 1][ 3] = -1 * C4 * sinT1 * sinT2 + A2 * cosT2 * sinT1 - 1 * D3 * sinT1 * sinT2 ; T[ 2][ 0] = sinT2 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = cosT2 ; T[ 2][ 3] = C4 * cosT2 + A2 * sinT2 + D3 * cosT2 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; } /****************************************************************************/ /* Dynamics equations for the third model */ #include <math.h> #include "dyn3.h" void dyn3Dyn (M, V, G, F, J_pos, J_vel, B_acc, External_F, External_M) double **M, *V, *G, *F, *J_pos, *J_vel, *B_acc, *External_F, *External_M; { double external_force_x = External_F[ 0]; double external_force_y = External_F[ 1]; double external_force_z = External_F[ 2]; double external_moment_x = External_M[ 0]; double external_moment_y = External_M[ 1]; double external_moment_z = External_M[ 2]; double base_x = B_acc[ 0]; double base_y = B_acc[ 1]; double base_z = B_acc[ 2]; double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double T3 = J_pos[ 2]; double vel_T1 = J_vel[ 0]; double vel_T2 = J_vel[ 1]; double vel_T3 = J_vel[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); double sinT3 = sin(T3); double cosT3 = cos(T3); M[ 0][ 0] = 2 * -JXY * cosT2 * sinT2 + 2 * -KXY * cosT2 * cosT2 * cosT3 * sinT3 + 2 * -KXY * cosT2 * cosT3 * cosT3 * sinT2 - 2 * -KXY * cosT2 * sinT2 * sinT3 * sinT3 - 2 * -KXY * cosT3 * sinT2 * sinT2 * sinT3 + 0.5*L2 * 0.5*L2 * M2 * cosT2 * cosT2 + 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT2 * cosT3 * cosT3 - 1 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT3 * sinT2 * sinT3 + 2 * 0.5*L3 * A2 * M3 * cosT2 * cosT2 * cosT3 - 1 * 0.5*L3 * A2 * M3 * cosT2 * sinT2 * sinT3 + A2 * A2 * M3 * cosT2 * cosT2 + IZZ + JXX * sinT2 * sinT2 + JYY * cosT2 * cosT2 + KXX * cosT2 * cosT2 * sinT3 * sinT3 + 2 * KXX * cosT2 * cosT3 * sinT2 * sinT3 + KXX * cosT3 * cosT3 * sinT2 * sinT2 + KYY * cosT2 * cosT2 * cosT3 * cosT3 - 2 * KYY * cosT2 * cosT3 * sinT2 * sinT3 + KYY * sinT2 * sinT2 * sinT3 * sinT3 ; M[ 0][ 1] = -JXZ * sinT2 + -JYZ * cosT2 + -KXZ * cosT2 * sinT3 + -KXZ * cosT3 * sinT2 + -KYZ * cosT2 * cosT3 - 1 * -KYZ * sinT2 * sinT3 ; M[ 0][ 2] = -KXZ * cosT2 * sinT3 + -KXZ * cosT3 * sinT2 + -KYZ * cosT2 * cosT3 ; M[ 1][ 0] = -JXZ * sinT2 + -JYZ * cosT2 + -KXZ * cosT2 * sinT3 + -KXZ * cosT3 * sinT2 + -KYZ * cosT2 * cosT3 - 1 * -KYZ * sinT2 * sinT3 ; M[ 1][ 1] = 0.5*L2 * 0.5*L2 * M2 + 0.5*L3 * 0.5*L3 * M3 + 2 * 0.5*L3 * A2 * M3 * cosT3 + A2 * A2 * M3 * cosT3 * cosT3 + A2 * A2 * M3 * sinT3 * sinT3 + JZZ + KZZ ; M[ 1][ 2] = 0.5*L3 * 0.5*L3 * M3 + 0.5*L3 * A2 * M3 * cosT3 + KZZ ; M[ 2][ 0] = -KXZ * cosT2 * sinT3 + -KXZ * cosT3 * sinT2 + -KYZ * cosT2 * cosT3 - 1 * -KYZ * sinT2 * sinT3 ; M[ 2][ 1] = 0.5*L3 * 0.5*L3 * M3 + 0.5*L3 * A2 * M3 * cosT3 + KZZ ; M[ 2][ 2] = 0.5*L3 * 0.5*L3 * M3 + KZZ ; V[ 0] = 2 * -JXY * cosT2 * cosT2 * vel_T1 * vel_T2 - 2 * -JXY * sinT2 * sinT2 * vel_T1 * vel_T2 + -JXZ * cosT2 * vel_T2 * vel_T2 - 1 * -JYZ * sinT2 * vel_T2 * vel_T2 + 2 * -KXY * cosT2 * cosT2 * cosT3 * cosT3 * vel_T1 * vel_T2 + 2 * -KXY * cosT2 * cosT2 * cosT3 * cosT3 * vel_T1 * vel_T3 - 2 * -KXY * cosT2 * cosT2 * sinT3 * sinT3 * vel_T1 * vel_T2 - 2 * -KXY * cosT2 * cosT2 * sinT3 * sinT3 * vel_T1 * vel_T3 - 7 * -KXY * cosT2 * cosT3 * sinT2 * sinT3 * vel_T1 * vel_T2 - 7 * -KXY * cosT2 * cosT3 * sinT2 * sinT3 * vel_T1 * vel_T3 - 2 * -KXY * cosT3 * cosT3 * sinT2 * sinT2 * vel_T1 * vel_T2 - 2 * -KXY * cosT3 * cosT3 * sinT2 * sinT2 * vel_T1 * vel_T3 + -KXY * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T2 + -KXY * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T3 - 1 * -KXZ * cosT2 * cosT2 * sinT2 * sinT3 * sinT3 * sinT3 * vel_T1 * vel_T1 - 2 * -KXZ * cosT2 * cosT3 * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + -KXZ * cosT2 * cosT3 * vel_T2 * vel_T2 + 2 * -KXZ * cosT2 * cosT3 * vel_T2 * vel_T3 + -KXZ * cosT2 * cosT3 * vel_T3 * vel_T3 - 1 * -KXZ * cosT3 * cosT3 * sinT2 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 - 1 * -KYZ * cosT2 * cosT2 * cosT3 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * -KYZ * cosT2 * cosT3 * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 + -KYZ * cosT2 * sinT2 * sinT2 * sinT3 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * -KYZ * cosT2 * sinT3 * vel_T2 * vel_T2 - 2 * -KYZ * cosT2 * sinT3 * vel_T2 * vel_T3 - 1 * -KYZ * cosT2 * sinT3 * vel_T3 * vel_T3 + -KYZ * cosT3 * sinT2 * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * -KYZ * cosT3 * sinT2 * vel_T2 * vel_T2 - 2 * -KYZ * cosT3 * sinT2 * vel_T2 * vel_T3 - 1 * -KYZ * cosT3 * sinT2 * vel_T3 * vel_T3 - 2 * 0.5*L2 * 0.5*L2 * M2 * cosT2 * sinT2 * vel_T1 * vel_T2 - 2 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T2 - 2 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T3 - 2 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T2 - 2 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T3 - 2 * 0.5*L3 * A2 * M3 * cosT2 * cosT2 * sinT3 * vel_T1 * vel_T2 - 2 * 0.5*L3 * A2 * M3 * cosT2 * cosT2 * sinT3 * vel_T1 * vel_T3 - 4 * 0.5*L3 * A2 * M3 * cosT2 * cosT3 * sinT2 * vel_T1 * vel_T2 - 2 * 0.5*L3 * A2 * M3 * cosT2 * cosT3 * sinT2 * vel_T1 * vel_T3 - 2 * A2 * A2 * M3 * cosT2 * sinT2 * vel_T1 * vel_T2 + 2 * JXX * cosT2 * sinT2 * vel_T1 * vel_T2 - 2 * JYY * cosT2 * sinT2 * vel_T1 * vel_T2 + 2 * KXX * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T2 + 2 * KXX * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T3 + 2 * KXX * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T2 + 2 * KXX * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T3 - 1 * KXX * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T2 - 1 * KXX * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T3 - 1 * KXX * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T2 - 1 * KXX * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T3 - 2 * KYY * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T2 - 2 * KYY * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T3 - 2 * KYY * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T2 - 2 * KYY * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T3 + 2 * KYY * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T2 + 2 * KYY * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T3 + 2 * KYY * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T2 + 2 * KYY * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T3 - 1 * KZZ * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T2 - 1 * KZZ * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T3 - 1 * KZZ * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T2 - 1 * KZZ * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T3 ; V[ 1] = -1 * -JXY * cosT2 * cosT2 * vel_T1 * vel_T1 + -JXY * sinT2 * sinT2 * vel_T1 * vel_T1 - 1 * -KXY * cosT2 * cosT2 * cosT3 * cosT3 * vel_T1 * vel_T1 + -KXY * cosT2 * cosT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + 4 * -KXY * cosT2 * cosT3 * sinT2 * sinT3 * vel_T1 * vel_T1 + -KXY * cosT3 * cosT3 * sinT2 * sinT2 * vel_T1 * vel_T1 - 1 * -KXY * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + 0.5*L2 * 0.5*L2 * M2 * cosT2 * sinT2 * vel_T1 * vel_T1 + 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 - 1 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * 0.5*L3 * 0.5*L3 * M3 * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT2 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT3 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT3 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT3 * sinT2 * vel_T1 * vel_T1 - 1 * 0.5*L3 * A2 * M3 * cosT3 * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 - 1 * 0.5*L3 * A2 * M3 * sinT2 * sinT2 * sinT3 * sinT3 * sinT3 * vel_T1 * vel_T1 - 2 * 0.5*L3 * A2 * M3 * sinT3 * vel_T2 * vel_T3 - 1 * 0.5*L3 * A2 * M3 * sinT3 * vel_T3 * vel_T3 + A2 * A2 * M3 * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 + A2 * A2 * M3 * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * JXX * cosT2 * sinT2 * vel_T1 * vel_T1 + JYY * cosT2 * sinT2 * vel_T1 * vel_T1 - 1 * KXX * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 - 1 * KXX * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 + KXX * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + KXX * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 + KYY * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 + KYY * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 - 1 * KYY * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * KYY * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 ; V[ 2] = -1 * -KXY * cosT2 * cosT2 * cosT3 * cosT3 * vel_T1 * vel_T1 + -KXY * cosT2 * cosT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + 4 * -KXY * cosT2 * cosT3 * sinT2 * sinT3 * vel_T1 * vel_T1 + -KXY * cosT3 * cosT3 * sinT2 * sinT2 * vel_T1 * vel_T1 - 1 * -KXY * sinT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * 0.5*L3 * M3 * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 - 1 * 0.5*L3 * 0.5*L3 * M3 * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * 0.5*L3 * 0.5*L3 * M3 * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT2 * sinT3 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * cosT2 * cosT3 * sinT2 * vel_T1 * vel_T1 + 0.5*L3 * A2 * M3 * sinT3 * vel_T2 * vel_T2 - 1 * KXX * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 - 1 * KXX * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 + KXX * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 + KXX * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 + KYY * cosT2 * cosT2 * cosT3 * sinT3 * vel_T1 * vel_T1 + KYY * cosT2 * cosT3 * cosT3 * sinT2 * vel_T1 * vel_T1 - 1 * KYY * cosT2 * sinT2 * sinT3 * sinT3 * vel_T1 * vel_T1 - 1 * KYY * cosT3 * sinT2 * sinT2 * sinT3 * vel_T1 * vel_T1 ; G[ 0] = 0.5*L2 * GRAVITY * M2 * cosT1 * cosT2 + 0.5*L3 * GRAVITY * M3 * cosT1 * cosT2 * cosT3 + A2 * GRAVITY * M3 * cosT1 * cosT2 ; G[ 1] = -1 * 0.5*L2 * GRAVITY * M2 * sinT1 * sinT2 - 1 * 0.5*L3 * GRAVITY * M3 * cosT3 * sinT1 * sinT2 + A2 * GRAVITY * M3 * cosT2 * cosT3 * sinT1 * sinT3 - 1 * A2 * GRAVITY * M3 * cosT3 * cosT3 * sinT1 * sinT2 - 1 * A2 * GRAVITY * M3 * sinT1 * sinT2 * sinT3 * sinT3 ; G[ 2] = -1 * 0.5*L3 * GRAVITY * M3 * cosT3 * sinT1 * sinT2 ; /* Torque due to external moments */ F[ 0] = cosT2 * cosT3 * external_moment_y + cosT2 * external_moment_x * sinT3 + cosT3 * external_moment_x * sinT2 ; F[ 1] = external_moment_z ; F[ 2] = external_moment_z ; /* Torque due to external forces */ F[ 0] += -1 * A2 * cosT2 * external_force_z - 1 * A3 * cosT2 * cosT3 * external_force_z ; F[ 1] += A2 * cosT3 * external_force_y + A2 * external_force_x * sinT3 + A3 * external_force_y ; F[ 2] += A3 * external_force_y ; /* Torque due to base movement */ F[ 0] += 0.5*L2 * M2 * base_y * cosT1 * cosT2 + 0.5*L3 * M3 * base_y * cosT1 * cosT2 * cosT3 + A2 * M3 * base_y * cosT1 * cosT2 ; F[ 1] += -1 * 0.5*L2 * M2 * base_x * cosT1 * sinT2 - 1 * 0.5*L2 * M2 * base_y * sinT1 * sinT2 + 0.5*L2 * M2 * base_z * cosT2 - 1 * 0.5*L3 * M3 * base_x * cosT1 * cosT3 * sinT2 - 1 * 0.5*L3 * M3 * base_y * cosT3 * sinT1 * sinT2 + 0.5*L3 * M3 * base_z * cosT2 * cosT3 + A2 * M3 * base_x * cosT1 * cosT2 * cosT3 * sinT3 - 1 * A2 * M3 * base_x * cosT1 * cosT3 * cosT3 * sinT2 - 1 * A2 * M3 * base_x * cosT1 * sinT2 * sinT3 * sinT3 + A2 * M3 * base_y * cosT2 * cosT3 * sinT1 * sinT3 - 1 * A2 * M3 * base_y * cosT3 * cosT3 * sinT1 * sinT2 - 1 * A2 * M3 * base_y * sinT1 * sinT2 * sinT3 * sinT3 + A2 * M3 * base_z * cosT2 * cosT3 * cosT3 + A2 * M3 * base_z * cosT2 * sinT3 * sinT3 + A2 * M3 * base_z * cosT3 * sinT2 * sinT3 ; F[ 2] += -1 * 0.5*L3 * M3 * base_x * cosT1 * cosT3 * sinT2 - 1 * 0.5*L3 * M3 * base_y * cosT3 * sinT1 * sinT2 + 0.5*L3 * M3 * base_z * cosT2 * cosT3 ; /* Force due to friction forces */ F[ 0] += 0.0 ; F[ 1] += 0.0 ; F[ 2] += 0.0 ; } void dyn3Frm (TransformList, J_pos) double ***TransformList, *J_pos; { double T1 = J_pos[ 0]; double T2 = J_pos[ 1]; double T3 = J_pos[ 2]; double sinT1 = sin(T1); double cosT1 = cos(T1); double sinT2 = sin(T2); double cosT2 = cos(T2); double sinT3 = sin(T3); double cosT3 = cos(T3); double **T; T = TransformList[ 0]; T[ 0][ 0] = cosT1 ; T[ 0][ 1] = -1 * sinT1 ; T[ 0][ 2] = 0 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = sinT1 ; T[ 1][ 1] = cosT1 ; T[ 1][ 2] = 0 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = 0 ; T[ 2][ 1] = 0 ; T[ 2][ 2] = 1 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 1]; T[ 0][ 0] = cosT1 * cosT2 ; T[ 0][ 1] = -1 * cosT1 * sinT2 ; T[ 0][ 2] = sinT1 ; T[ 0][ 3] = 0 ; T[ 1][ 0] = cosT2 * sinT1 ; T[ 1][ 1] = -1 * sinT1 * sinT2 ; T[ 1][ 2] = -1.0 * cosT1 ; T[ 1][ 3] = 0 ; T[ 2][ 0] = sinT2 ; T[ 2][ 1] = cosT2 ; T[ 2][ 2] = 0 ; T[ 2][ 3] = 0 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 2]; T[ 0][ 0] = cosT1 * cosT2 * cosT3 - 1 * cosT1 * sinT2 * sinT3 ; T[ 0][ 1] = -1 * cosT1 * cosT2 * sinT3 - 1 * cosT1 * cosT3 * sinT2 ; T[ 0][ 2] = sinT1 ; T[ 0][ 3] = A2 * cosT1 * cosT2 ; T[ 1][ 0] = cosT2 * cosT3 * sinT1 - 1 * sinT1 * sinT2 * sinT3 ; T[ 1][ 1] = -1 * cosT2 * sinT1 * sinT3 - 1 * cosT3 * sinT1 * sinT2 ; T[ 1][ 2] = -1.0 * cosT1 ; T[ 1][ 3] = A2 * cosT2 * sinT1 ; T[ 2][ 0] = cosT3 * sinT2 + cosT2 * sinT3 ; T[ 2][ 1] = -1 * sinT2 * sinT3 + cosT2 * cosT3 ; T[ 2][ 2] = 0 ; T[ 2][ 3] = A2 * sinT2 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; T = TransformList[ 3]; T[ 0][ 0] = cosT1 * cosT2 * cosT3 - 1 * cosT1 * sinT2 * sinT3 ; T[ 0][ 1] = -1 * cosT1 * cosT2 * sinT3 - 1 * cosT1 * cosT3 * sinT2 ; T[ 0][ 2] = sinT1 ; T[ 0][ 3] = A3 * cosT1 * cosT2 * cosT3 - 1 * A3 * cosT1 * sinT2 * sinT3 + A2 * cosT1 * cosT2 ; T[ 1][ 0] = cosT2 * cosT3 * sinT1 - 1 * sinT1 * sinT2 * sinT3 ; T[ 1][ 1] = -1 * cosT2 * sinT1 * sinT3 - 1 * cosT3 * sinT1 * sinT2 ; T[ 1][ 2] = -1.0 * cosT1 ; T[ 1][ 3] = A3 * cosT2 * cosT3 * sinT1 - 1 * A3 * sinT1 * sinT2 * sinT3 + A2 * cosT2 * sinT1 ; T[ 2][ 0] = cosT3 * sinT2 + cosT2 * sinT3 ; T[ 2][ 1] = -1 * sinT2 * sinT3 + cosT2 * cosT3 ; T[ 2][ 2] = 0 ; T[ 2][ 3] = A3 * cosT3 * sinT2 + A3 * cosT2 * sinT3 + A2 * sinT2 ; T[ 3][ 0] = 0 ; T[ 3][ 1] = 0 ; T[ 3][ 2] = 0 ; T[ 3][ 3] = 1 ; }