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 ;
}