//////////////////////////////////////////////////////////////
// Dynamic Simulation of 8-dof 3D Cooperative WAM System /////
//////////////////////////////////////////////////////////////
/* 2003.11.17 */
/* 2004.4.14 */
/* 2004.4.19 */
/* 2004.4.23 */
/* 2004.5.6 */
/* 2004.5.7 */
/* 2004.10.26 */
/* Impedance control is modified */
/* Dynamic equation is modified */
/* For Advanced Robotics */

Real	l11, l21, l12, l22, c11, c21, c12, c22, m11, m21, m12, m22;
Real	gravity, I11x, I11y, I11z, I21x, I21y, I21z;
Real	I12x, I12y, I12z, I22x, I22y, I22z;
Real	l01, l02, mo, Iox, Ioy, Ioz;
Real	K, r0;
Integer	i, j, k;
Real	force_save11, force_save21, force_save12, force_save22;
Matrix	f1_save, f2_save;
Matrix	Kd, Kp, Mr, Dr, Kr;
Real	Ld11, Ld21, Ld12, Ld22;
Real	thin, mr, dr, kr;
Real	pole1, pole2, beta;

Func void init()
{

	gravity = 9.81;

	/* Robotic Arm */
	l01 = 0.25;
	l02 = - l01;

	l11 = 0.50;
	l21 = 0.50;
	l12 = 0.50;
	l22 = 0.50;

	c11 = 0.50 * l11;
	c21 = 0.50 * l21;
	c12 = 0.50 * l12;
	c22 = 0.50 * l22;

	m11 = 3.00;
	m21 = 3.00;
	m12 = m11;
	m22 = m21;

	thin = 0.03;

	I11x = m11 * (thin^2 / 4.0 + l11^2 / 12.0);
	I11y = 0.50 * m11 * thin^2;
	I11z = I11x;
	I21x = m21 * (thin^2 / 4.0 + l21^2 / 12.0);
	I21y = 0.50 * m21 * thin^2;
	I21z = I21x;
	I12x = m12 * (thin^2 / 4.0 + l12^2 / 12.0);
	I12y = 0.50 * m12 * thin^2;
	I12z = I12x;
	I22x = m22 * (thin^2 / 4.0 + l22^2 / 12.0);
	I22y = 0.50 * m22 * thin^2;
	I22z = I22x;

	/* Object */
	mo = 10.0;
	Iox = 1.0;
	Ioy = 1.0;
	Ioz = 1.0;
	K = 1.0 * 1.0e+6;

	r0 = 0.30;

	i = j = k = 0;

	force_save11 = 0.0;
	force_save21 = 0.0;
	force_save12 = 0.0;
	force_save22 = 0.0;

	f1_save = Z(4, 1);
	f2_save = Z(4, 1);

	pole1 = 100.0;
	pole2 = 50.0;

	beta = 10.0;
	read	beta;

	/* Gains for impedance control (Voluntary) */
	mr = 0.5;
	kr = mr * pole1^2;
	dr = 2.0 * sqrt(kr / mr);

	print	mr, kr, dr;

	Mr = mr * I(4);
	Dr = dr * I(4);
	Kr = kr * I(4);

	/* PD gains for regulation control (Reflex) */
	Kd = 2.0 * pole2 * I(6);
	Kp = pole2^2 * I(6);

}

Func void main()
{

	Matrix	x0;
	Array	T, X, U;
	Real	t0, t1, h, dt;
	void	diff_eqs(), link_eqs(), init();
	
	dt = 0.001;
	h = 0.0001;

	t0 = 0.0;
	t1 = 4.0;
	read	t1;

	init();

	x0 = Z(28, 1);

	x0(2, 1) = - 0.0 * PI; 
	x0(3, 1) = - 0.25 * PI; 
	x0(4, 1) = 0.5 * PI; 

	x0(6, 1) = 0.0 * PI; 
	x0(7, 1) = - 0.25 * PI; 
	x0(8, 1) = 0.5 * PI; 

	x0(9, 1) = 0.0; 
	x0(10, 1) = 0.40;
	x0(11, 1) = 0.20;
	
	{T, X, U} = Ode45HybridAuto(t0, t1, dt, x0, diff_eqs, link_eqs);

	print [[T][X][U]] >> "wam.mat";

}

Func void diff_eqs(DX, T, X, U)
Matrix	DX, X, U;
Real	T;	
{

	Matrix	M1, C1, G1, tau1, h1, q1, dq1, ddq1;
	Matrix	M2, C2, G2, tau2, h2, q2, dq2, ddq2;
	Matrix	Mo, Go, qo, dqo, ddqo, ro, dro;
	Matrix	q, dq, ddq;

	Matrix	Jc11, Jc21, Jc12, Jc22, dJc11, dJc21, dJc12, dJc22;
	Matrix	Jf11, Jf21, Jf12, Jf22, dJf11, dJf21, dJf12, dJf22;
	Matrix	rc11, rc21, rc12, rc22, drc11, drc21, drc12, drc22;
	Matrix	re11, re21, re12, re22, dre11, dre21, dre12, dre22;
	Matrix	rf11, rf21, rf12, rf22, drf11, drf21, drf12, drf22;
	Matrix	a11, a21, a12, a22, b11, b21, b12, b22;
	Matrix	f11, f21, f12, f22;
	Real	r11, r21, r12, r22;

	Matrix	Get_Mmatrix(), Get_Cmatrix(), Get_Gmatrix();
	Matrix	Get_Jmatrix(), Get_dJmatrix();
	Matrix	Get_rvector(), Get_drvector(), Get_force();

	Matrix	f_ext;

	/* Initialized */
	q1 = q2 = dq1 = dq2 = ddq1 = ddq2 = Z(4, 1);
	qo = dqo = ddqo = Z(6, 1);
	q = dq = ddq = Z(14, 1);

	M1 = M2 = C1 = C2 = Z(4);
	G1 = G2 = tau1 = tau2 = h1 = h2 = Z(4, 1);
	Mo = Z(6);
	Go = Z(6, 1);
	Jc11 = Jc21 = Jc12 = Jc22 = Z(3, 4);
	Jf11 = Jf21 = Jf12 = Jf22 = Z(3, 4);

	q = X(1:14, 1);
	dq = X(15:28, 1);

	q1 = q(1:4, 1);
	q2 = q(5:8, 1);
	qo = q(9:14, 1);

	dq1 = dq(1:4, 1);
	dq2 = dq(5:8, 1);
	dqo = dq(9:14, 1);

	tau1 = U(1:4, 1);
	tau2 = U(5:8, 1);
	
	ro = qo(1:3, 1);
	dro = dqo(1:3, 1);

	M1 = Get_Mmatrix(q1, c11, c21, l11, l21, I11x, I11y, I11z,
	I21x, I21y, I21z, m11, m21);
	C1 = Get_Cmatrix(q1, dq1, c11, c21, l11, l21, I11x, I11y,
	I11z, I21x, I21y, I21z, m11, m21);
	G1 = Get_Gmatrix(q1, c11, c21, l11, l21, m11, m21);
	h1 = C1 * dq1 + G1;

	M2 = Get_Mmatrix(q2, c12, c22, l12, l22, I12x, I12y, I12z,
	I22x, I22y, I22z, m12, m22);
	C2 = Get_Cmatrix(q2, dq2, c12, c22, l12, l22, I12x, I12y,
	I12z, I22x, I22y, I22z, m12, m22);
	G2 = Get_Gmatrix(q2, c12, c22, l12, l22, m12, m22);
	h2 = C2 * dq2 + G2;

	Mo = diag(mo, mo, mo, Iox, Ioy, Ioz);
	Go = [0.0, 0.0, mo * gravity, 0.0, 0.0, 0.0]';

	re11 = Get_rvector(q1, l11, 0.0) + [l01, 0.0, 0.0]';
	re21 = Get_rvector(q1, l11, l21) + [l01, 0.0, 0.0]';
	re12 = Get_rvector(q2, l12, 0.0) + [l02, 0.0, 0.0]';
	re22 = Get_rvector(q2, l12, l22) + [l02, 0.0, 0.0]';

	a11 = ro - [l01, 0.0, 0.0]';
	a21 = ro - re11;
	a12 = ro - [l02, 0.0, 0.0]';
	a22 = ro - re12;

	b11 = re11 - [l01, 0.0, 0.0]';
	b21 = re21 - re11;
	b12 = re12 - [l02, 0.0, 0.0]';
	b22 = re22 - re12;

	r11 = norm([a11' * b11 * inv(b11' * b11)](1) * b11);
	r21 = norm([a21' * b21 * inv(b21' * b21)](1) * b21);
	r12 = norm([a12' * b12 * inv(b12' * b12)](1) * b12);
	r22 = norm([a22' * b22 * inv(b22' * b22)](1) * b22);

	rf11 = Get_rvector(q1, r11, 0.0) + [l01, 0.0, 0.0]';
	rf21 = Get_rvector(q1, l11, r21) + [l01, 0.0, 0.0]';
	rf12 = Get_rvector(q2, r12, 0.0) + [l02, 0.0, 0.0]';
	rf22 = Get_rvector(q2, l12, r22) + [l02, 0.0, 0.0]';

	Jf11 = Get_Jmatrix(q1, r11, 0.0);
	Jf21 = Get_Jmatrix(q1, l11, r21);
	Jf12 = Get_Jmatrix(q2, r12, 0.0);
	Jf22 = Get_Jmatrix(q2, l12, r22);

	drf11 = Jf11 * q1;
	drf21 = Jf21 * q1;
	drf12 = Jf12 * q2;
	drf22 = Jf22 * q2;

	f11 = Get_force(q1, ro, rf11, dro, drf11);
	f21 = Get_force(q1, ro, rf21, dro, drf21);
	f12 = Get_force(q2, ro, rf12, dro, drf12);
	f22 = Get_force(q2, ro, rf22, dro, drf22);

	tau1 = tau1 - Jf11' * f11(1:3, 1) - Jf21' * f21(1:3, 1);
	tau2 = tau2 - Jf12' * f12(1:3, 1) - Jf22' * f22(1:3, 1);

	force_save11 = norm(f11(1:3, 1));
	force_save21 = norm(f21(1:3, 1));
	force_save12 = norm(f12(1:3, 1));
	force_save22 = norm(f22(1:3, 1));

	f1_save = Jf11' * f11(1:3, 1) + Jf21' * f21(1:3, 1);
	f2_save = Jf12' * f12(1:3, 1) + Jf22' * f22(1:3, 1);

	f_ext = Z(6, 1);

	if(T >= 2.00){

	f_ext(1, 1) = 150.0;

	}

	ddq1 = inv(M1) * (tau1 - h1);
	ddq2 = inv(M2) * (tau2 - h2);
	ddqo = inv(Mo) * (f11 + f21 + f12 + f22 - Go + f_ext);

	ddq = [[ddq1][ddq2][ddqo]];
	dq = [[dq1][dq2][dqo]];

	DX = [[dq][ddq]];

}

Func void link_eqs(U, T, X)
Matrix	X, U;
Real	T;
{

	Matrix	M1, C1, G1, tau1, h1, q1, dq1, ddq1;
	Matrix	M2, C2, G2, tau2, h2, q2, dq2, ddq2;
	Matrix	Mo, Go, qo, dqo, ddqo, ro, dro;
	Matrix	q, dq, ddq;
	Matrix	Jc11, Jc21, Jc12, Jc22, dJc11, dJc21, dJc12, dJc22;
	Matrix	Jf11, Jf21, Jf12, Jf22, dJf11, dJf21, dJf12, dJf22;
	Matrix	Je11, Je21, Je12, Je22, dJe11, dJe21, dJe12, dJe22;
	Matrix	rc11, rc21, rc12, rc22, drc11, drc21, drc12, drc22;
	Matrix	re11, re21, re12, re22, dre11, dre21, dre12, dre22;
	Matrix	rf11, rf21, rf12, rf22, drf11, drf21, drf12, drf22;
	Matrix	a11, a21, a12, a22, b11, b21, b12, b22;
	Matrix	f11, f21, f12, f22;
	Matrix	a0c11, a0c21, a0c12, a0c22;
	Matrix	a0e11, a0e21, a0e12, a0e22;
	Matrix	a0f11, a0f21, a0f12, a0f22;
	Real	r11, r21, r12, r22;
	Matrix	Get_Mmatrix(), Get_Cmatrix(), Get_Gmatrix();
	Matrix	Get_Jmatrix(), Control();
	Matrix	Get_rvector(), Get_force(), Get_a0vector();
	Matrix	u1, u2, J1, J2, M, h, Le, rod, drod, dLe, Led;
	Matrix	Lc11, Lc21, Lc12, Lc22, dLc11, dLc21, dLc12, dLc22;
	Matrix	Lf11, Lf21, Lf12, Lf22, dLf11, dLf21, dLf12, dLf22;
	Matrix	A1, A2, B1, B2, tau;

	/* Initialized */
	q1 = q2 = dq1 = dq2 = ddq1 = ddq2 = Z(4, 1);
	qo = dqo = ddqo = Z(6, 1);
	q = dq = ddq = Z(14, 1);

	M1 = M2 = C1 = C2 = Z(4);
	G1 = G2 = tau1 = tau2 = h1 = h2 = Z(4, 1);
	Mo = Z(6);
	Go = Z(6, 1);

	q = X(1:14, 1);
	dq = X(15:28, 1);

	q1 = q(1:4, 1);
	q2 = q(5:8, 1);
	qo = q(9:14, 1);

	dq1 = dq(1:4, 1);
	dq2 = dq(5:8, 1);
	dqo = dq(9:14, 1);

	/* Information of CoM(o) */
	ro = qo(1:3, 1);
	dro = dqo(1:3, 1);

	/* Dynamics of the robotic arm */
	M1 = Get_Mmatrix(q1, c11, c21, l11, l21, I11x, I11y, I11z,
	I21x, I21y, I21z, m11, m21);
	C1 = Get_Cmatrix(q1, dq1, c11, c21, l11, l21, I11x, I11y,
	I11z, I21x, I21y, I21z, m11, m21);
	G1 = Get_Gmatrix(q1, c11, c21, l11, l21, m11, m21);
	h1 = C1 * dq1 + G1;

	M2 = Get_Mmatrix(q2, c12, c22, l12, l22, I12x, I12y, I12z,
	I22x, I22y, I22z, m12, m22);
	C2 = Get_Cmatrix(q2, dq2, c12, c22, l12, l22, I12x, I12y,
	I12z, I22x, I22y, I22z, m12, m22);
	G2 = Get_Gmatrix(q2, c12, c22, l12, l22, m12, m22);
	h2 = C2 * dq2 + G2;

	/* Information of CoM(ij) */
	Jc11 = Get_Jmatrix(q1, c11, 0.0);
	Jc21 = Get_Jmatrix(q1, l11, c21);
	Jc12 = Get_Jmatrix(q2, c12, 0.0);
	Jc22 = Get_Jmatrix(q2, l12, c22);

	rc11 = Get_rvector(q1, c11, 0.0) + [l01, 0.0, 0.0]';
	rc21 = Get_rvector(q1, l11, c21) + [l01, 0.0, 0.0]';
	rc12 = Get_rvector(q2, c12, 0.0) + [l02, 0.0, 0.0]';
	rc22 = Get_rvector(q2, l12, c22) + [l02, 0.0, 0.0]';

	drc11 = Jc11 * dq1;
	drc21 = Jc21 * dq1;
	drc12 = Jc12 * dq2;
	drc22 = Jc22 * dq2;

	/* Information of contact point(ij) */
	re11 = Get_rvector(q1, l11, 0.0) + [l01, 0.0, 0.0]';
	re21 = Get_rvector(q1, l11, l21) + [l01, 0.0, 0.0]';
	re12 = Get_rvector(q2, l12, 0.0) + [l02, 0.0, 0.0]';
	re22 = Get_rvector(q2, l12, l22) + [l02, 0.0, 0.0]';

	a11 = ro - [l01, 0.0, 0.0]';
	a21 = ro - re11;
	a12 = ro - [l02, 0.0, 0.0]';
	a22 = ro - re12;

	b11 = re11 - [l01, 0.0, 0.0]';
	b21 = re21 - re11;
	b12 = re12 - [l02, 0.0, 0.0]';
	b22 = re22 - re12;

	r11 = norm([a11' * b11 * inv(b11' * b11)](1) * b11);
	r21 = norm([a21' * b21 * inv(b21' * b21)](1) * b21);
	r12 = norm([a12' * b12 * inv(b12' * b12)](1) * b12);
	r22 = norm([a22' * b22 * inv(b22' * b22)](1) * b22);

	rf11 = Get_rvector(q1, r11, 0.0) + [l01, 0.0, 0.0]';
	rf21 = Get_rvector(q1, l11, r21) + [l01, 0.0, 0.0]';
	rf12 = Get_rvector(q2, r12, 0.0) + [l02, 0.0, 0.0]';
	rf22 = Get_rvector(q2, l12, r22) + [l02, 0.0, 0.0]';

	/* Information of EE(ij) */
	Je11 = Get_Jmatrix(q1, l11, 0.0);
	Je21 = Get_Jmatrix(q1, l11, l21);
	Je12 = Get_Jmatrix(q2, l12, 0.0);
	Je22 = Get_Jmatrix(q2, l12, l22);

	M = [[M1, Z(4)][Z(4), M2]];
	h = [[h1][h2]];

	/* Desired CoM(o) */
	rod = Z(3, 1);
	drod = Z(3, 1);
	rod = [0.0, 0.35, 0.0]';

	/* Distance between CoM(ij) and desired CoM(o) */
	Lc11 = rod - rc11;
	Lc21 = rod - rc21;
	Lc12 = rod - rc12;
	Lc22 = rod - rc22;

	dLc11 = drod - drc11;
	dLc21 = drod - drc21;
	dLc12 = drod - drc12;
	dLc22 = drod - drc22;

	a0c11 = Get_a0vector(q1, dq1, c11, 0.0);
	a0c21 = Get_a0vector(q1, dq1, l11, c21);
	a0c12 = Get_a0vector(q2, dq2, c12, 0.0);
	a0c22 = Get_a0vector(q2, dq2, l12, c22);

	a0e11 = Get_a0vector(q1, dq1, l11, 0.0);
	a0e21 = Get_a0vector(q1, dq1, l11, l21);
	a0e12 = Get_a0vector(q2, dq2, l12, 0.0);
	a0e22 = Get_a0vector(q2, dq2, l12, l22);

	/* Distance between contact points and desired CoM(o) */
	Lf11 = rod - rf11;
	Lf21 = rod - rf21;
	Lf12 = rod - rf12;
	Lf22 = rod - rf22;

	a0f11 = Get_a0vector(q1, dq1, r11, 0.0);
	a0f21 = Get_a0vector(q1, dq1, l11, r21);
	a0f12 = Get_a0vector(q2, dq2, r12, 0.0);
	a0f22 = Get_a0vector(q2, dq2, l12, r22);

	Jf11 = Get_Jmatrix(q1, r11, 0.0);
	Jf21 = Get_Jmatrix(q1, l11, r21);
	Jf12 = Get_Jmatrix(q2, r12, 0.0);
	Jf22 = Get_Jmatrix(q2, l12, r22);

	drf11 = Jf11 * dq1;
	drf21 = Jf21 * dq1;
	drf12 = Jf12 * dq2;
	drf22 = Jf22 * dq2;

	dLf11 = drod - drf11;
	dLf21 = drod - drf21;
	dLf12 = drod - drf12;
	dLf22 = drod - drf22;

	/* Control: Part I */
	A1 = - [[Lc11' * Jc11 * inv(M1) / norm(Lc11), Z(1, 4)]
	[Lc21' * Jc21 * inv(M1) / norm(Lc21), Z(1, 4)]
	[Z(1, 4), Lc12' * Jc12 * inv(M2) / norm(Lc12)]
	[Z(1, 4), Lc22' * Jc22 * inv(M2) / norm(Lc22)]];

	B1 = [[Lc11' * (a0c11 - Jc11 * inv(M1) * h1) / norm(Lc11)]
	[Lc21' * (a0c21 - Jc21 * inv(M1) * h1) / norm(Lc21)]
	[Lc12' * (a0c12 - Jc12 * inv(M2) * h2) / norm(Lc12)]
	[Lc22' * (a0c22 - Jc22 * inv(M2) * h2) / norm(Lc22)]];

	if(T == 0){

	Ld11 = norm(rod - rc11);
	Ld21 = norm(rod - rc21);
	Ld12 = norm(rod - rc12);
	Ld22 = norm(rod - rc22);

	print Ld11, Ld21, Ld12, Ld22;

	Ld11 = norm(ro - rf11);
	Ld21 = norm(ro - rf21);
	Ld12 = norm(ro - rf12);
	Ld22 = norm(ro - rf22);

	Ld11 = 0.32;
	Ld21 = 0.32;
	Ld12 = 0.32;
	Ld22 = 0.32;

	print "OK!";

	}

	B1 = B1 - inv(Mr) * Dr * [[dLc11' * Lc11 / norm(Lc11)]
	[dLc21' * Lc21 / norm(Lc21)]
	[dLc12' * Lc12 / norm(Lc12)]
	[dLc22' * Lc22 / norm(Lc22)]] 
	- inv(Mr) * Kr * [[norm(Lc11) - Ld11]
	[norm(Lc21) - Ld21]
	[norm(Lc12) - Ld12]
	[norm(Lc22) - Ld22]] + inv(Mr) * 
	[[force_save11][force_save21][force_save12][force_save22]];

	/* Control: Part II */
	Le = [[re11 - re12][re21 - re22]];
	J2 = [[Je11, - Je12][Je21, - Je22]];
	dLe = J2 * [[dq1][dq2]];

	Led = Z(6, 1);
	Led(1, 1) = 0.50 * cos(atan(beta * ro(1, 1)));
	Led(3, 1) = 0.50 * sin(atan(beta * ro(1, 1)));
	Led(4, 1) = 0.40 * cos(atan(beta * ro(1, 1)));
	Led(6, 1) = 0.40 * sin(atan(beta * ro(1, 1)));

	A2 = J2 * inv(M);
	B2 = J2 * inv(M) * h - [[a0e11 - a0e12][a0e21 - a0e22]];
	B2 = B2 - Kd * dLe - Kp * (Le - Led);

	/* Optimal control input */
	tau = Control(A1, A2, B1, B2);

	tau = tau + [[f1_save][f2_save]];

	U = [[tau]	// 37
	[Le - Led]	// 38-43
	[norm(Lc11)]	// 44	
	[norm(Lc21)]	// 45	
	[norm(Lc12)]	// 46	
	[norm(Lc22)]	// 47	
	[force_save11]		// 48
	[force_save21]		// 49
	[force_save12]		// 50
	[force_save22]		// 51
	[0.5 * norm(A2 * tau - B2)^2]	// 52	value of J
	[norm(ro - rf11)]
	[norm(ro - rf21)]
	[norm(ro - rf12)]
	[norm(ro - rf22)]];

}

Func Matrix Control(A1, A2, B1, B2)
Matrix	A1, A2, B1, B2;
{

	Matrix	tau, U, Sigma, Sigma1, V, V1, V2, tau10, tau20;

	{U, Sigma, V} = svd(A1);
	Sigma1 = Sigma(1:4, 1:4);
	V1 = V(1:8, 1:4);
	V2 = V(1:8, 5:8);

	tau10 = inv(Sigma1) * U' * B1;
	tau20 = inv(V2' * A2' * A2 * V2) * (A2 * V2)'
	 * (B2 - A2 * V1 * inv(Sigma1) * U' * B1);

	tau = V * [[tau10][tau20]];

	return	tau;

}

Func Matrix Get_rvector(q, l1, l2)
Matrix	q;
Real	l1, l2;
{

	Matrix	r;
	Real	q1, q2, q3, q4;

	r = Z(3, 1);

	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	r(1, 1) = l1*(-(cos(q3)*sin(q1)) + cos(q1)*sin(q2)*sin(q3)) +
l2*(cos(q4)*(-(cos(q3)*sin(q1)) + cos(q1)*sin(q2)*sin(q3)) +
(cos(q1)*cos(q3)*sin(q2) + sin(q1)*sin(q3))*sin(q4));

	r(2, 1) = l1*(cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)) +
l2*(cos(q4)*(cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3)) +
(cos(q3)*sin(q1)*sin(q2) - cos(q1)*sin(q3))*sin(q4));

	r(3, 1) = l1*cos(q2)*sin(q3) + l2*(cos(q2)*cos(q4)*sin(q3) +
cos(q2)*cos(q3)*sin(q4));

	return	r;

}

Func Matrix Get_force(q, ro, rf, dro, drf)
Matrix	q, ro, rf, dro, drf;
{

	Matrix	f;
	Real	dist;

	f = Z(6, 1);

	dist = norm(ro - rf);

	if((r0 + thin) >= dist){

	f(1:3, 1) = (ro - rf) * K * (r0 + thin - dist)^2 / dist;

	}

	return	f;

}

Func Matrix Get_Jmatrix(q, l1, l2)
Matrix	q;
Real	l1, l2;
{

	Matrix	J;
	Real	q1, q2, q3, q4;

	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	J = Z(3, 4);
   
	J(1, 1) = -(cos(q1)*(l1*cos(q3) + l2*cos(q3 + q4))) -
sin(q1)*sin(q2)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(1, 2) = cos(q1)*cos(q2)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(1, 3) = cos(q1)*(l1*cos(q3) + l2*cos(q3 + q4))* sin(q2) +
sin(q1)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(1, 4) = l2*(cos(q1)*cos(q3 + q4)*sin(q2) + sin(q1)*sin(q3 +
q4));

	J(2, 1) = -((l1*cos(q3) + l2*cos(q3 + q4))*sin(q1)) +
cos(q1)*sin(q2)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(2, 2) = cos(q2)*sin(q1)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(2, 3) = (l1*cos(q3) + l2*cos(q3 + q4))*sin(q1)* sin(q2) -
cos(q1)*(l1*sin(q3) + l2*sin(q3 + q4));
	J(2, 4) = l2*(cos(q3 + q4)*sin(q1)*sin(q2) - cos(q1)*sin(q3 +
q4));

	J(3, 2) = -(sin(q2)*(l1*sin(q3) + l2*sin(q3 + q4)));
	J(3, 3) = cos(q2)*(l1*cos(q3) + l2*cos(q3 + q4));
	J(3, 4) = l2*cos(q2)*cos(q3 + q4);

	return J;

}

Func Matrix Get_a0vector(q, dq, l1, l2)
Matrix	q, dq;
Real	l1, l2;
{

	Matrix	a0;
	Real	q1, q2, q3, q4;
	Real	dq1, dq2, dq3, dq4;
	
	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	dq1 = dq(1, 1);
	dq2 = dq(2, 1);
	dq3 = dq(3, 1);
	dq4 = dq(4, 1);

	a0 = Z(3, 1);

	a0(1, 1) = -(cos(q1)*(-2*dq2*cos(q2)*(dq3*l1*cos(q3) + (dq3 +
dq4)*l2* cos(q3 + q4)) + (-2*dq1*dq3*l1 + (dq1^2 + dq2^2 + dq3^2)*l1*
sin(q2) + l2*cos(q4)*(-2*dq1*(dq3 + dq4) + (dq1^2 + dq2^2 + (dq3 +
dq4)^2)*sin(q2)))*sin(q3) + l2*cos(q3)*(-2*dq1*(dq3 + dq4) + (dq1^2 +
dq2^2 + (dq3 + dq4)^2)* sin(q2))*sin(q4))) +
sin(q1)*(l1*cos(q3)*(dq1^2 + dq3^2 - 2*dq1*dq3*sin(q2)) + l2*cos(q3 +
q4) *(dq1^2 + (dq3 + dq4)^2 - 2*dq1*(dq3 + dq4)*sin(q2)) -
2*dq1*dq2*cos(q2)*(l1*sin(q3) + l2*sin(q3 + q4))); 

	a0(2, 1) = -(sin(q1)*(-2*dq2*cos(q2)*(dq3*l1*cos(q3) + (dq3 +
dq4)*l2* cos(q3 + q4)) + (-2*dq1*dq3*l1 + (dq1^2 + dq2^2 + dq3^2)*l1*
sin(q2) + l2*cos(q4)*(-2*dq1*(dq3 + dq4) + (dq1^2 + dq2^2 + (dq3 +
dq4)^2)*sin(q2)))*sin(q3) + l2*cos(q3)*(-2*dq1*(dq3 + dq4) + (dq1^2 +
dq2^2 + (dq3 + dq4)^2)* sin(q2))*sin(q4))) + cos(q1)*
(-(l1*cos(q3)*(dq1^2 + dq3^2 - 2*dq1*dq3*sin(q2))) - l2*cos(q3 +
q4)*(dq1^2 + (dq3 + dq4)^2 - 2*dq1*(dq3 + dq4)*sin(q2)) +
2*dq1*dq2*cos(q2)*(l1*sin(q3) + l2*sin(q3 + q4))); 

	a0(3, 1) = -2*dq2*dq3*l1*cos(q3)*sin(q2) - 2*dq2*(dq3 +
dq4)*l2*cos(q3 + q4)* sin(q2) + cos(q2)*(-((dq2^2 + dq3^2)*l1*sin(q3))
- (dq2^2 + (dq3 + dq4)^2)*l2*sin(q3 + q4));

	return	a0;
	
}

Func Matrix Get_Mmatrix(q, r1, r2, l1, l2, I1x, I1y, I1z, I2x, I2y,
I2z, m1, m2)
Matrix	q;
Real	r1, r2, l1, l2, I1x, I1y, I1z, I2x, I2y, I2z, m1, m2;
{

	Matrix	M;
	Real	q1, q2, q3, q4;
	
	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	M = Z(4);

	M(1, 1) = (I1z + m1*r1^2)*cos(q2)^2*cos(q3)^2 + (I1x +
m1*r1^2)*sin(q2)^2 - l1*cos(q4)*sin(q2)*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2)) - sin(q2)*(-((I2x + m2*r2^2)*sin(q2)) -
l1*m2*r2*cos(q4)*sin(q2)) + I1y*cos(q2)^2*sin(q3)^2 +
l1^2*m2*sin(q2)^2*sin(q4)^2 + I2y*(cos(q2)*cos(q4)*sin(q3) +
cos(q2)*cos(q3)*sin(q4))^2 -
l1*cos(q2)*cos(q3)*(-(l1*m2*cos(q2)*cos(q3)) -
m2*r2*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))) +
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)* (cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4)));

	M(1, 2) = (2*I1y*cos(q2)*cos(q3)*sin(q3) - 2*(I1z +
m1*r1^2)*cos(q2)*cos(q3)*sin(q3) + 2*I2y*(cos(q2)*cos(q4)*sin(q3)
+cos(q2)*cos(q3)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) -
l1*cos(q2)*cos(q3)* (l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3))
-cos(q3)*sin(q4))) + (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))* (-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(-(l1*m2*cos(q2)*cos(q3)) - m2*r2*(cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)* sin(q4))))/2.0;

	M(1, 3) = (-2*(I1x + m1*r1^2)*sin(q2) -	(I2x +
m2*r2^2)*sin(q2) - l1*m2*r2*cos(q4)*sin(q2) - l1*cos(q4)*(m2*r2 +
l1*m2*cos(q4))*sin(q2) - (I2x + m2*r2^2 + l1*m2*r2*cos(q4))*sin(q2)
+l1*cos(q4)*(-(m2*r2*sin(q2)) - l1*m2*cos(q4)*sin(q2)) -
2*l1^2*m2*sin(q2)*sin(q4)^2)/2.0;

	M(1, 4) = -((I2x + m2*r2^2)*sin(q2)) -
l1*m2*r2*cos(q4)*sin(q2);

	M(2, 2) = I1y*cos(q3)^2 + (I1z + m1*r1^2)*sin(q3)^2
+I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))^2 +l1*sin(q3)*(l1*m2*sin(q3)
- m2*r2*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) + (-(cos(q4)*sin(q3))
- cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4)));

	M(3, 3) = I1x + I2x + m1*r1^2 + m2*r2^2 +l1*m2*r2*cos(q4) +
l1*cos(q4)*(m2*r2 + l1*m2*cos(q4)) + l1^2*m2*sin(q4)^2;

	M(3, 4) = I2x + m2*r2^2 + l1*m2*r2*cos(q4);

	M(4, 4) = I2x + m2*r2^2;

	M(2, 1) = M(1, 2);
	M(3, 1) = M(1, 3);
	M(3, 2) = M(2, 3);
	M(4, 1) = M(1, 4);
	M(4, 2) = M(2, 4);
	M(4, 3) = M(3, 4);

	return	M;

}
 
Func Matrix Get_Cmatrix(q, dq, r1, r2, l1, l2, I1x, I1y, I1z, I2x,
I2y, I2z, m1, m2)
Matrix	q, dq;
Real	r1, r2, l1, l2, I1x, I1y, I1z, I2x, I2y, I2z, m1, m2;
{

	Matrix	C;
	Real	q1, q2, q3, q4;
	Real	dq1, dq2, dq3, dq4;

	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	dq1 = dq(1, 1);
	dq2 = dq(2, 1);
	dq3 = dq(3, 1);
	dq4 = dq(4, 1);

	C = Z(4);

	C(1, 2) = dq4*(-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4)) + (dq3*(-2*(I1x + m1*r1^2)*cos(q2) - (I2x +
m2*r2^2)*cos(q2) - l1*m2*r2*cos(q2)*cos(q4) - l1*cos(q2)*cos(q4)*
(m2*r2 + l1*m2*cos(q4)) - cos(q2)*(I2x + m2*r2^2 + l1*m2*r2*cos(q4)) +
l1*cos(q4)*(-(m2*r2*cos(q2)) - l1*m2*cos(q2)*cos(q4)) -
2*l1^2*m2*cos(q2)*sin(q4)^2))/2.0 + (dq2*(-2*I1y*cos(q3)*sin(q2)*sin(q3)
+ 2*(I1z + m1*r1^2)*cos(q3)*sin(q2)*sin(q3) +
2*I2y*(-(cos(q4)*sin(q2)*sin(q3)) -
cos(q3)*sin(q2)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) +
l1*cos(q3)*sin(q2)*(l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)*sin(q2)) +
sin(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q3)*sin(q2) - m2*r2*(-(cos(q3)*cos(q4)* sin(q2))
+ sin(q2)*sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q3)*sin(q2)) + (I2z +
m2*r2^2)*(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4)))))/2.0 +
dq1*(2*(I1x + m1*r1^2)*cos(q2)*sin(q2) - 2*(I1z + m1*r1^2)*cos(q2)*
cos(q3)^2*sin(q2) - l1*cos(q4)*(-(m2*r2*cos(q2)) -
l1*m2*cos(q2)*cos(q4))*sin(q2) - (-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4))*sin(q2) - l1*cos(q2)*cos(q4)*
(-(m2*r2*sin(q2)) - l1*m2*cos(q4)*sin(q2)) - cos(q2)*(-((I2x +
m2*r2^2)*sin(q2)) - l1*m2*r2*cos(q4)*sin(q2)) -
2*I1y*cos(q2)*sin(q2)*sin(q3)^2 + 2*l1^2*m2*cos(q2)*sin(q2)* sin(q4)^2
+ 2*I2y*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*
sin(q4))*(-(cos(q4)*sin(q2)*sin(q3)) - cos(q3)*sin(q2)*sin(q4)) +
l1*cos(q3)*sin(q2)*(-(l1*m2*cos(q2)*cos(q3)) -
m2*r2*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))) +
(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))) - l1*cos(q2)*cos(q3)*(l1*m2*cos(q3)*sin(q2)
- m2*r2*(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4))) +
(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)*sin(q2)) + (I2z +
m2*r2^2)* (-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4))));

	C(1, 3) = dq1*(2*I1y*cos(q2)^2*cos(q3)*sin(q3) - 2*(I1z +
m1*r1^2)*cos(q2)^2* cos(q3)*sin(q3) + 2*I2y*(cos(q2)*cos(q4)*sin(q3) +
cos(q2)*cos(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) - l1*cos(q2)*cos(q3)* (l1*m2*cos(q2)*sin(q3)
- m2*r2*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) +
(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q2)*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) +
l1*cos(q2)*sin(q3)*(-(l1*m2*cos(q2)*cos(q3)) -
m2*r2*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))) +
(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))) +
(dq2*(2*I1y*cos(q2)*cos(q3)^2 - 2*(I1z + m1*r1^2)*cos(q2)*cos(q3)^2 -
2*I1y*cos(q2)*sin(q3)^2 + 2*(I1z + m1*r1^2)*cos(q2)*sin(q3)^2 +
2*I2y*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*(cos(q2)*cos(q4)*sin(q3)
+ cos(q2)*cos(q3)*sin(q4)) + 2*I2y*(cos(q3)*cos(q4) -
sin(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)) +
l1*cos(q2)*sin(q3)*(l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q2)*sin(q3) - m2*r2*(-(cos(q2)*cos(q4)* sin(q3))
- cos(q2)*cos(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q2)*sin(q3)) + (I2z + m2*r2^2)*
(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) -
l1*cos(q2)*cos(q3)*(l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)) + (I2z +
m2*r2^2)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))) +
l1*cos(q3)*(-(l1*m2*cos(q2)*cos(q3)) - m2*r2*(cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0;

	C(1, 4) = dq4*l1*m2*r2*sin(q2)*sin(q4) +
(dq3*(2*l1*m2*r2*sin(q2)*sin(q4) - 2*l1^2*m2*cos(q4)*sin(q2)*sin(q4) +
l1*(m2*r2 + l1*m2*cos(q4))*sin(q2)*sin(q4) - l1*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2))*sin(q4)))/2.0 +
dq1*(-(l1*m2*r2*sin(q2)^2*sin(q4)) + l1^2*m2*cos(q4)*sin(q2)^2*
sin(q4) + l1*sin(q2)*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2))*sin(q4) +
l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + (I2z + m2*r2^2)*(-(cos(q2)*cos(q4)*sin(q3))
- cos(q2)*cos(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + 2*I2y*(cos(q2)*cos(q4)*sin(q3) +
cos(q2)*cos(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))) +
(dq2*(-(l1*m2*r2*sin(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))) + (I2z + m2*r2^2)* (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4))
+ l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) +
2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))
+ (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4))
+ sin(q3)*sin(q4))* (l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0;

	C(2, 1) = (-(dq4*(-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4))) - (dq3*(-2*(I1x + m1*r1^2)*cos(q2) - (I2x +
m2*r2^2)*cos(q2) - l1*m2*r2*cos(q2)*cos(q4) - l1*cos(q2)*cos(q4)*
(m2*r2 + l1*m2*cos(q4)) - cos(q2)*(I2x + m2*r2^2 + l1*m2*r2*cos(q4)) +
l1*cos(q4)*(-(m2*r2*cos(q2)) - l1*m2*cos(q2)*cos(q4)) -
2*l1^2*m2*cos(q2)*sin(q4)^2))/2.0 - (dq2*(-2*I1y*cos(q3)*sin(q2)*sin(q3)
+ 2*(I1z + m1*r1^2)*cos(q3)* sin(q2)*sin(q3) +
2*I2y*(-(cos(q4)*sin(q2)*sin(q3)) -
cos(q3)*sin(q2)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) +
l1*cos(q3)*sin(q2)*(l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)*sin(q2)) +
sin(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q3)*sin(q2) - m2*r2*(-(cos(q3)*cos(q4)*sin(q2))
+ sin(q2)*sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)*sin(q2)) + (I2z + m2*r2^2)*
(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4)))))/2.0 -
dq1*(2*(I1x + m1*r1^2)*cos(q2)*sin(q2) - 2*(I1z + m1*r1^2)*cos(q2)*
cos(q3)^2*sin(q2) - l1*cos(q4)*(-(m2*r2*cos(q2)) -
l1*m2*cos(q2)*cos(q4))*sin(q2) - (-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4))*sin(q2) - l1*cos(q2)*cos(q4)*
(-(m2*r2*sin(q2)) - l1*m2*cos(q4)*sin(q2)) - cos(q2)*(-((I2x +
m2*r2^2)*sin(q2)) - l1*m2*r2*cos(q4)*sin(q2)) -
2*I1y*cos(q2)*sin(q2)*sin(q3)^2 + 2*l1^2*m2*cos(q2)*sin(q2)* sin(q4)^2
+ 2*I2y*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*
sin(q4))*(-(cos(q4)*sin(q2)*sin(q3)) - cos(q3)*sin(q2)* sin(q4)) +
l1*cos(q3)*sin(q2)*(-(l1*m2*cos(q2)*cos(q3)) -
m2*r2*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))) +
(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))) - l1*cos(q2)*cos(q3)*(l1*m2*cos(q3)*sin(q2)
- m2*r2*(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4))) +
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))*
(-(l1*m2*r2*cos(q3)*sin(q2)) + (I2z + m2*r2^2)*
(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4)))))/2.0;

	C(2, 2) = (dq1*(-2*I1y*cos(q3)*sin(q2)*sin(q3) + 2*(I1z +
m1*r1^2)*cos(q3)* sin(q2)*sin(q3) + 2*I2y*(-(cos(q4)*sin(q2)*sin(q3))
- cos(q3)*sin(q2)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) +
l1*cos(q3)*sin(q2)*(l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)*sin(q2)) +
sin(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q3)*sin(q2) - m2*r2*(-(cos(q3)*cos(q4)*sin(q2))
+ sin(q2)*sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)*sin(q2)) + (I2z + m2*r2^2)*
(-(cos(q3)*cos(q4)*sin(q2)) + sin(q2)*sin(q3)*sin(q4)))))/4.0;

	C(2, 3) = -(dq1*(-2*(I1x + m1*r1^2)*cos(q2) - (I2x +
m2*r2^2)*cos(q2) - l1*m2*r2*cos(q2)*cos(q4) - l1*cos(q2)*cos(q4)*
(m2*r2 + l1*m2*cos(q4)) - cos(q2)*(I2x + m2*r2^2 + l1*m2*r2*cos(q4)) +
l1*cos(q4)*(-(m2*r2*cos(q2)) - l1*m2*cos(q2)*cos(q4)) -
2*l1^2*m2*cos(q2)*sin(q4)^2))/4.0 + dq2*(-2*I1y*cos(q3)*sin(q3) + 2*(I1z
+ m1*r1^2)*cos(q3)*sin(q3) + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) +
l1*cos(q3)*(l1*m2*sin(q3) - m2*r2*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q3)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4)))) + (dq1*(2*I1y*cos(q2)*cos(q3)^2 - 2*(I1z +
m1*r1^2)*cos(q2)*cos(q3)^2 - 2*I1y*cos(q2)*sin(q3)^2 + 2*(I1z +
m1*r1^2)*cos(q2)*sin(q3)^2 + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)* sin(q4))
+ 2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4)) + l1*cos(q2)*sin(q3)*(l1*m2*sin(q3) -
m2*r2*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q2)*sin(q3) - m2*r2*(-(cos(q2)*cos(q4)* sin(q3))
- cos(q2)*cos(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q2)*sin(q3)) + (I2z + m2*r2^2)*
(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) -
l1*cos(q2)*cos(q3)*(l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)) + (I2z +
m2*r2^2)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))) +
l1*cos(q3)*(-(l1*m2*cos(q2)*cos(q3)) - m2*r2*(cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0;

	C(2, 4) = -(dq1*(-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4)))/2.0 + dq2*(2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(cos(q3)*cos(q4) - sin(q3)*sin(q4)) -
l1*m2*r2*sin(q3)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4)) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4)))) +
(dq1*(-(l1*m2*r2*sin(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))) + (I2z + m2*r2^2)*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4))
+ l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) +
2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))
+ (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4))
+ sin(q3)*sin(q4))* (l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0;

	C(3, 1) = (-(dq1*(2*I1y*cos(q2)^2*cos(q3)*sin(q3) - 2*(I1z +
m1*r1^2)*cos(q2)^2* cos(q3)*sin(q3) + 2*I2y*(cos(q2)*cos(q4)*sin(q3) +
cos(q2)*cos(q3)*sin(q4))*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) - l1*cos(q2)*cos(q3)* (l1*m2*cos(q2)*sin(q3)
- m2*r2*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) +
(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q2)*sin(q3)) + (I2z + 
m2*r2^2)*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)* sin(q4))) +
l1*cos(q2)*sin(q3)*(-(l1*m2*cos(q2)*cos(q3)) -
m2*r2*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))) +
(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))))) - (dq2*(2*I1y*cos(q2)*cos(q3)^2 - 2*(I1z +
m1*r1^2)*cos(q2)*cos(q3)^2 - 2*I1y*cos(q2)*sin(q3)^2 + 2*(I1z +
m1*r1^2)*cos(q2)*sin(q3)^2 + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4))
+ 2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4)) + l1*cos(q2)*sin(q3)*(l1*m2*sin(q3) -
m2*r2*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q2)*sin(q3) - m2*r2*(-(cos(q2)*cos(q4)* sin(q3))
- cos(q2)*cos(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q2)*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) -
l1*cos(q2)*cos(q3)*(l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))*(-(l1*m2*r2*cos(q3)) + (I2z +
m2*r2^2)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))) +
l1*cos(q3)*(-(l1*m2*cos(q2)*cos(q3)) - m2*r2*(cos(q2)*cos(q3)* cos(q4)
- cos(q2)*sin(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)* sin(q4)))))/2.0)/2.0;

	C(3, 2) = (dq1*(-2*(I1x + m1*r1^2)*cos(q2) - (I2x +
m2*r2^2)*cos(q2) - l1*m2*r2*cos(q2)*cos(q4) -
l1*cos(q2)*cos(q4)*(m2*r2 + l1*m2*cos(q4)) - cos(q2)*(I2x + m2*r2^2 +
l1*m2*r2*cos(q4)) + l1*cos(q4)*(-(m2*r2*cos(q2)) -
l1*m2*cos(q2)*cos(q4)) - 2*l1^2*m2*cos(q2)*sin(q4)^2))/2.0 +
(-(dq2*(-2*I1y*cos(q3)*sin(q3) + 2*(I1z + m1*r1^2)*cos(q3)*sin(q3) +
2*I2y*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*(cos(q3)*cos(q4) -
sin(q3)*sin(q4)) + l1*cos(q3)*(l1*m2*sin(q3) -
m2*r2*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*
(-(l1*m2*r2*cos(q3)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))))) - (dq1*(2*I1y*cos(q2)*cos(q3)^2 - 2*(I1z +
m1*r1^2)*cos(q2)*cos(q3)^2 - 2*I1y*cos(q2)*sin(q3)^2 + 2*(I1z +
m1*r1^2)*cos(q2)*sin(q3)^2 + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4)) +
2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + l1*cos(q2)*sin(q3)*(l1*m2*sin(q3) -
m2*r2*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))*(-(l1*m2*r2*sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))) +
l1*sin(q3)*(l1*m2*cos(q2)*sin(q3) - m2*r2*(-(cos(q2)*cos(q4)* sin(q3))
- cos(q2)*cos(q3)*sin(q4))) + (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(-(l1*m2*r2*cos(q2)* sin(q3)) + (I2z +
m2*r2^2)*(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))) -
l1*cos(q2)*cos(q3)* (l1*m2*cos(q3) - m2*r2*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))) + (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4))* (-(l1*m2*r2*cos(q3)) + (I2z +
m2*r2^2)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))) +
l1*cos(q3)*(-(l1*m2*cos(q2)*cos(q3)) - m2*r2*(cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0)/2.0;

	C(3, 4) = -(dq4*l1*m2*r2*sin(q4)) + dq3*(-(l1*m2*r2*sin(q4)) +
l1^2*m2*cos(q4)*sin(q4) - l1*(m2*r2 + l1*m2*cos(q4))*sin(q4)) +
(dq1*(2*l1*m2*r2*sin(q2)*sin(q4) - 2*l1^2*m2*cos(q4)*sin(q2)*sin(q4) +
l1*(m2*r2 + l1*m2*cos(q4))*sin(q2)*sin(q4) - l1*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2))*sin(q4)))/2.0;

	C(4, 1) = (-(dq4*l1*m2*r2*sin(q2)*sin(q4)) -
(dq3*(2*l1*m2*r2*sin(q2)*sin(q4) - 2*l1^2*m2*cos(q4)*sin(q2)* sin(q4)
+ l1*(m2*r2 + l1*m2*cos(q4))*sin(q2)*sin(q4) - l1*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2))*sin(q4)))/2.0 -
dq1*(-(l1*m2*r2*sin(q2)^2*sin(q4)) + l1^2*m2*cos(q4)*sin(q2)^2*
sin(q4) + l1*sin(q2)*(-(m2*r2*sin(q2)) - l1*m2*cos(q4)*sin(q2))*
sin(q4) + l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + (I2z + m2*r2^2)*
(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))*
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)) +
2*I2y*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4))*
(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)) +
(-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)* (cos(q2)*cos(q3)*cos(q4)
- cos(q2)*sin(q3)*sin(q4)))) -
(dq2*(-(l1*m2*r2*sin(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))) + (I2z + m2*r2^2)* (-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4))
+ l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) +
2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))
+ (-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))*
(-(l1*m2*r2*sin(q3)) + (I2z + m2*r2^2)*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))*
(l1*m2*r2*cos(q2)*cos(q3) + (I2z + m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)))))/2.0)/2.0;

	C(4, 2) = dq1*(-((I2x + m2*r2^2)*cos(q2)) -
l1*m2*r2*cos(q2)*cos(q4)) + (-(dq2*(2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))* (cos(q3)*cos(q4) - sin(q3)*sin(q4)) -
l1*m2*r2*sin(q3)* (-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) + (I2z +
m2*r2^2)* (-(cos(q4)*sin(q3)) - cos(q3)*sin(q4))*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4)) + (-(cos(q3)*cos(q4)) + sin(q3)*sin(q4))*
(-(l1*m2*r2*sin(q3)) + (I2z + m2*r2^2)*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))))) -
(dq1*(-(l1*m2*r2*sin(q3)*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4))) + (I2z + m2*r2^2)*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(-(cos(q2)*cos(q4)*sin(q3)) -
cos(q2)*cos(q3)*sin(q4)) + 2*I2y*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))*(cos(q2)*cos(q4)*sin(q3) + cos(q2)*cos(q3)*sin(q4)) +
l1*m2*r2*cos(q2)*cos(q3)*(-(cos(q3)*cos(q4)) + sin(q3)*sin(q4)) +
2*I2y*(cos(q3)*cos(q4) - sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) -
cos(q2)*sin(q3)*sin(q4)) + (I2z + m2*r2^2)*(-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))* (cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4))
+ (-(cos(q2)*cos(q4)*sin(q3)) - cos(q2)*cos(q3)*sin(q4))*
(-(l1*m2*r2*sin(q3)) + (I2z + m2*r2^2)*(-(cos(q4)*sin(q3)) -
cos(q3)*sin(q4))) + (-(cos(q3)*cos(q4)) +
sin(q3)*sin(q4))*(l1*m2*r2*cos(q2)*cos(q3) + (I2z +
m2*r2^2)*(cos(q2)*cos(q3)*cos(q4) - cos(q2)*sin(q3)*sin(q4)))))/2.0)/2.0;

	C(4, 3) = (dq4*l1*m2*r2*sin(q4) - dq3*(-(l1*m2*r2*sin(q4)) +
l1^2*m2*cos(q4)*sin(q4) - l1*(m2*r2 + l1*m2*cos(q4))*sin(q4)) -
(dq1*(2*l1*m2*r2*sin(q2)*sin(q4) - 2*l1^2*m2*cos(q4)*sin(q2)* sin(q4)
+ l1*(m2*r2 + l1*m2*cos(q4))*sin(q2)*sin(q4) - l1*(-(m2*r2*sin(q2)) -
l1*m2*cos(q4)*sin(q2))*sin(q4)))/2.0)/2.0;

	C(4, 4) = -(dq3*l1*m2*r2*sin(q4)) +
dq1*l1*m2*r2*sin(q2)*sin(q4) +(dq3*l1*m2*r2*sin(q4) -
dq1*l1*m2*r2*sin(q2)*sin(q4))/2.0;

	return	C;
	
}

Func Matrix Get_Gmatrix(q, r1, r2, l1, l2, m1, m2)
Matrix	q;
Real	r1, r2, l1, l2, m1, m2;
{

	Matrix	G;
	Real	q1, q2, q3, q4;
	
	q1 = q(1, 1);
	q2 = q(2, 1);
	q3 = q(3, 1);
	q4 = q(4, 1);

	G = Z(4, 1);

	G(2, 1) = - gravity * sin(q2)*((l1*m2 + m1*r1)*sin(q3) +
m2*r2*sin(q3 + q4));

	G(3, 1) = gravity * cos(q2)*((l1*m2 + m1*r1)*cos(q3) +
m2*r2*cos(q3 + q4));

	G(4, 1) = gravity * m2*r2*cos(q2)*cos(q3 + q4);

	return	G;

}