function result = mdi_P3RPFlex(qa,qe,qed)
	
	% declaration of the constant parameters
	global d3 d4 d5 d6 d7 d8 d10 d11 d12 d13

	global zz2 zz3 zz4 zz5 zz6 zz7 zz9 zz10 zz11 zz12 
	global zz13
	global mx2 mx3 mx4 mx5 mx6 mx7 mx9 mx10 mx11 mx12 
	global mx13
	global my2 my3 my4 my5 my6 my7 my9 my10 my11 my12
	global my13
	global m1 m2 m3 m4 m5 m6 m7 m8 m9 m10 m11 m12 m13
	global ia1 ia2 ia8 ia9 ia12
	global fv1 fv2 fv8 fv9 fv12
	global fs1 fs2 fs8 fs9 fs12
	global afs
	
	global msde31x msde32y msde33y msde61x msde62y 
	global msde63y msde101x msde102y msde103y

	global mre31xx mre32xy mre32yx mre33xy
	global mre61xx mre62xy mre62yx mre63xy
	global mre101xx mre102xy mre102yx mre103xy

	global mee311xx mee312xy mee313xy mee321yx  
	global mee322xx mee322yy mee323xx mee323yy  
	global mee331yx mee332xx mee332yy mee333xx  
	global mee333yy
	global mee611xx mee612xy mee613xy mee621yx  
	global mee622xx mee622yy mee623xx mee623yy  
	global mee631yx mee632xx mee632yy mee633xx  
	global mee633yy
	global mee1011xx mee1012xy mee1013xy mee1021yx  
	global mee1022xx mee1022yy mee1023xx mee1023yy  
	global mee1031yx mee1032xx mee1032yy mee1033xx  
	global mee1033yy

	global ke3xx ke3yy ke3yz ke3zy ke3zz
	global ke6xx ke6yy ke6yz ke6zy ke6zz
	global ke10xx ke10yy ke10yz ke10zy ke10zz
	
% Value of the shape functions at the beam
% extremity
	de31x=1;
	de32y=1;
	de61x=1;
	de62y=1;
	de101x=1;
	de102y=1;
	
	the33z=1;
	the63z=1;
	the103z=1;

% model inputs
	q1=qa(1);
	q4=qa(2);

	qe21=qe(1) ;
	qe22=qe(2) ;
	qe23=qe(3) ;
	qe51=qe(4) ;
	qe52=qe(5) ;
	qe53=qe(6) ;

	qd1=qa(3);
	qd4=qa(4);

	qed21=qed(1) ;
	qed22=qed(2) ;
	qed23=qed(3) ;
	qed51=qed(4) ;
	qed52=qed(5) ;
	qed53=qed(6) ;

	qdd1=qa(5);
	qdd4=qa(6);

	un = 1 ;
	zer = 0 ;
	
%% Geometric model (elasticity is considered
%% negligible for geometry only)
% Passive joint coordinates
	q14 = q1-q4;
	a1 = -2.*l.*d.*un ;
	b1 = 2.*l.*q14;
	c1 = d.^2.*un + q14.^2 ;

	q2 = 2.*atan((-b1 - sqrt(b1.^2 - c1.^2 +...
		a1.^2))./(c1-a1));
	s2 = sin(q2) ;
	c2 = cos(q2) ;
	ls2 = l.*s2 ;
	lc2 = l.*c2 ;

	q5 = atan2(ls2 + q14, lc2-d.*un);
	s5 = sin(q5) ;
	c5 = cos(q5) ;
	ls5 = l.*s5 ;
	lc5 = l.*c5 ;

	q3 = q5 - q2 - pi.*un ;

%% First-order kinematics
% Jacobian matrix of the pkm
	jp11 = -ls2 ;
	jp21 = lc2 ;
	jp13 = ls5 ;
	jp23 = -lc5 ;

	detjp = -jp11.*jp23+jp21.*jp13 ;
	jpinv11 = -jp23./detjp ;
	jpinv12 = jp13./detjp ;
	jpinv21 = (jp21+jp23)./detjp ;
	jpinv22 = (-jp11-jp13)./detjp ;
	jpinv23 = 1.*un ;
	jpinv31 = jp21./detjp ;
	jpinv32 = -jp11./detjp ;

	j11 = -jpinv12;
	j12 = jpinv12;
	j13 = -jpinv11.*c2-jpinv12.*s2;
	j14 = jpinv11.*s2-jpinv12.*c2;
	j15 = 0.*un;
	j16 = jpinv11.*c5+jpinv12.*s5;
	j17 = -jpinv11.*s5+jpinv12.*c5;
	j18 = 0.*un;

	j21 = -jpinv22;
	j22 = jpinv22;
	j23 = -jpinv21.*c2-jpinv22.*s2;
	j24 = jpinv21.*s2-jpinv22.*c2;
	j25 = -1.*un;
	j26 = jpinv21.*c5+jpinv22.*s5;
	j27 = -jpinv21.*s5+jpinv22.*c5;
	j28 = 1.*un;

	j31 = -jpinv32;
	j32 = jpinv32;
	j33 = -jpinv31.*c2-jpinv32.*s2;
	j34 = jpinv31.*s2-jpinv32.*c2;
	j35 = 0.*un;
	j36 = jpinv31.*c5+jpinv32.*s5;
	j37 = -jpinv31.*s5+jpinv32.*c5;
	j38 = 0.*un;

% Passive joint velocities
	qd2 = j11.*qd1 + j12.*qd4 + j13.*qed21 +...
		j14.*qed22 + j16.*qed51 + j17.*qed52;
	qd3 = j21.*qd1 + j22.*qd4 + j23.*qed21 +...
		j24.*qed22 + j25.*qed23 + j26.*qed51 +...
		j27.*qed52 + j28.*qed53;
	qd5 = j31.*qd1 + j32.*qd4 + j33.*qed21 +...
		j34.*qed22 + j36.*qed51 + j37.*qed52;

%% Second-order kinematic model
	jpd11 =  -lc2.*qd2;
	jpd13 = lc5.*qd5 ;
	jpd21 = -ls2.*qd2 ;
	jpd23 = ls5.*qd5 ;

	jad13 = ls2.*(qd2+qed23);
	jad14 = lc2.*(qd2+qed23);
	jad16 = -ls5.*(qd5+qed53);
	jad17 = -lc5.*(qd5+qed53);
	jad23 = -lc2.*(qd2+qed23);
	jad24 = ls2.*(qd2+qed23);
	jad26 = lc5.*(qd5+qed53);
	jad27 = -ls5.*(qd5+qed53);

	tintacc1 = -jpd11.*qd2 - jpd13.*qd5 +...
		jad13.*qed21 + jad14.*qed22 +...
		jad16.*qed51 + jad17.*qed52;
	tintacc2 = -jpd21.*qd2 - jpd23.*qd5 +...
		jad23.*qed21 + jad24.*qed22 +...
		jad26.*qed51 + jad27.*qed52;

	tacc1 = jpinv11.*tintacc1 + jpinv12.*tintacc2;
	tacc2 = jpinv21.*tintacc1 + jpinv22.*tintacc2;
	tacc3 = jpinv31.*tintacc1 + jpinv32.*tintacc2;

%% Dynamic model of the virtual tree structure
	s3=sin(q3);
	c3=cos(q3);
	ae1e2e2=-(qe23.*the23z);
	ae2e1e2=qe23.*the23z;
	looe12=de21x.*qe21;
	looe22=de22y.*qe22;
	a1e1e3=c3 + ae1e2e2.*s3;
	a1e2e3=ae1e2e2.*c3 - s3;
	a2e1e3=ae2e1e2.*c3 + s3;
	a2e2e3=c3 - ae2e1e2.*s3;
	loo13=l + looe12;
	qtri1e3e3=-(a2e1e3.*loo13) + a1e1e3.*looe22;
	qtri2e3e3=-(a2e2e3.*loo13) + a1e2e3.*looe22;
	we32=qed23.*the23z;
	ve12=de21x.*qed21;
	ve22=de22y.*qed22;
	vei13=a1e1e3.*ve12 + a2e1e3.*ve22;
	vei23=a1e2e3.*ve12 + a2e2e3.*ve22;
	wti33=qd2 + we32;
	w33=qd3 + wti33;
	matphi1e6e3=a1e1e3.*de21x;
	matphi1e7e3=a2e1e3.*de22y;
	matphi2e6e3=a1e2e3.*de21x;
	matphi2e7e3=a2e2e3.*de22y;
	jacvj1e1e3=a2e1e3.*c2 + a1e1e3.*s2;
	jacvj2e1e3=a2e2e3.*c2 + a1e2e3.*s2;
	ire3e3e2e1=2.*mre21xx;
	irepe3e3e2=ire3e3e2e1.*qe21;
	irede3e3e2=mre21xx.*qed21;
	iee3e3e2e2e2=mee222xx + mee222yy;
	iee3e3e2e2e3=mee223xx + mee223yy;
	iee3e3e2e3e2=mee232xx + mee232yy;
	iee3e3e2e3e3=mee233xx + mee233yy;
	ieep3e3e2e1=mee211xx.*qe21;
	ieep3e3e2e2=iee3e3e2e2e2.*qe22;
	ieep3e3e2e2=ieep3e3e2e2 + iee3e3e2e2e3.*qe23;
	ieep3e3e2e3=iee3e3e2e3e2.*qe22;
	ieep3e3e2e3=ieep3e3e2e3 + iee3e3e2e3e3.*qe23;
	ieeppe3e3e2=ieep3e3e2e1.*qe21 + ieep3e3e2e2.*qe22 +...
		ieep3e3e2e3.*qe23;
	ieepde3e3e2=ieep3e3e2e1.*qed21 + ieep3e3e2e2.*...
		qed22 + ieep3e3e2e3.*qed23;
	pv322=mre22xy - mre22yx;
	pvp1e3e2=-(mee221yx.*qe22) - mee231yx.*qe23;
	pvp2e3e2=mee221yx.*qe21;
	pvp3e3e2=mee231yx.*qe21;
	pvd1e3e2=-(mee221yx.*qed22) - mee231yx.*qed23;
	pvd2e3e2=mee221yx.*qed21;
	pvd3e3e2=mee231yx.*qed21;
	jje3e3e2=ieeppe3e3e2 + irepe3e3e2 + zz2;
	mse12=msde21x.*qe21;
	mse22=msde22y.*qe22 + msde23y.*qe23;
	msed12=msde21x.*qed21;
	msed22=msde22y.*qed22 + msde23y.*qed23;
	ms12=mse12 + mx2;
	ms22=mse22 + my2;
	jer622=pv322 + pvp2e3e2;
	jer623=mre23xy + pvp3e3e2;
	jee2e2e2=mee222xx + mee222yy;
	jee3e2e2=mee232xx + mee232yy;
	jee3e3e2=mee233xx + mee233yy;
	jinintj1e1e2=m2.*s2;
	jinintj1e2e2=c2.*m2;
	jinje1e1e2=c2.*jinintj1e2e2 + jinintj1e1e2.*s2;
	jinje2e1e2=c2.*ms12 - ms22.*s2;
	jinje6e1e2=msde21x.*s2;
	jinje7e1e2=c2.*msde22y;
	jinje8e1e2=c2.*msde23y;
	jinintj1e1e3=jacvj1e1e3.*m3;
	jinintj1e2e3=jacvj2e1e3.*m3;
	jinintj2e1e3=-my3 - m3.*qtri1e3e3;
	jinintj2e2e3=mx3 - m3.*qtri2e3e3;
	jinintj2e6e3=my3.*qtri1e3e3 - mx3.*qtri2e3e3 + zz3;
	jinintj6e1e3=m3.*matphi1e6e3;
	jinintj6e2e3=m3.*matphi2e6e3;
	jinintj6e6e3=matphi2e6e3.*mx3 - matphi1e6e3.*my3;
	jinintj7e1e3=m3.*matphi1e7e3;
	jinintj7e2e3=m3.*matphi2e7e3;
	jinintj7e6e3=matphi2e7e3.*mx3 - matphi1e7e3.*my3;
	jinintj8e1e3=-(my3.*the23z);
	jinintj8e2e3=mx3.*the23z;
	jinintj8e6e3=the23z.*zz3;
	jinje1e1e3=jacvj1e1e3.*jinintj1e1e3 +...
		jacvj2e1e3.*jinintj1e2e3;
	jinje2e1e3=jacvj1e1e3.*jinintj2e1e3 +...
		jacvj2e1e3.*jinintj2e2e3;
	jinje3e1e3=jacvj2e1e3.*mx3 - jacvj1e1e3.*my3;
	jinje6e1e3=jacvj1e1e3.*jinintj6e1e3 +...
		jacvj2e1e3.*jinintj6e2e3;
	jinje7e1e3=jacvj1e1e3.*jinintj7e1e3 +...
		jacvj2e1e3.*jinintj7e2e3;
	jinje8e1e3=jacvj1e1e3.*jinintj8e1e3 +...
		jacvj2e1e3.*jinintj8e2e3;
	jinje2e2e3=jinintj2e6e3 - jinintj2e1e3.*...
		qtri1e3e3 - jinintj2e2e3.*qtri2e3e3;
	jinje3e2e3=my3.*qtri1e3e3 - mx3.*qtri2e3e3 + zz3;
	jinje6e2e3=jinintj6e6e3 - jinintj6e1e3.*...
		qtri1e3e3 - jinintj6e2e3.*qtri2e3e3;
	jinje7e2e3=jinintj7e6e3 - jinintj7e1e3.*...
		qtri1e3e3 - jinintj7e2e3.*qtri2e3e3;
	jinje8e2e3=jinintj8e6e3 - jinintj8e1e3.*...
		qtri1e3e3 - jinintj8e2e3.*qtri2e3e3;
	jinje6e6e3=jinintj6e1e3.*matphi1e6e3 +...
		jinintj6e2e3.*matphi2e6e3;
	jinje7e6e3=jinintj7e1e3.*matphi1e6e3 +...
		jinintj7e2e3.*matphi2e6e3;
	jinje8e6e3=jinintj8e1e3.*matphi1e6e3 +...
		jinintj8e2e3.*matphi2e6e3;
	jinje7e7e3=jinintj7e1e3.*matphi1e7e3 +...
		jinintj7e2e3.*matphi2e7e3;
	jinje8e7e3=jinintj8e1e3.*matphi1e7e3 +...
		jinintj8e2e3.*matphi2e7e3;
	jinje8e8e3=jinintj8e6e3.*the23z;
	ire3e3e5e1=2.*mre51xx;
	irepe3e3e5=ire3e3e5e1.*qe51;
	irede3e3e5=mre51xx.*qed51;
	iee3e3e5e2e2=mee522xx + mee522yy;
	iee3e3e5e2e3=mee523xx + mee523yy;
	iee3e3e5e3e2=mee532xx + mee532yy;
	iee3e3e5e3e3=mee533xx + mee533yy;
	ieep3e3e5e1=mee511xx.*qe51;
	ieep3e3e5e2=iee3e3e5e2e2.*qe52;
	ieep3e3e5e2=ieep3e3e5e2 + iee3e3e5e2e3.*qe53;
	ieep3e3e5e3=iee3e3e5e3e2.*qe52;
	ieep3e3e5e3=ieep3e3e5e3 + iee3e3e5e3e3.*qe53;
	ieeppe3e3e5=ieep3e3e5e1.*qe51 + ieep3e3e5e2.*qe52 +...
		ieep3e3e5e3.*qe53;
	ieepde3e3e5=ieep3e3e5e1.*qed51 + ieep3e3e5e2.*...
		qed52 + ieep3e3e5e3.*qed53;
	pv352=mre52xy - mre52yx;
	pvp1e3e5=-(mee521yx.*qe52) - mee531yx.*qe53;
	pvp2e3e5=mee521yx.*qe51;
	pvp3e3e5=mee531yx.*qe51;
	pvd1e3e5=-(mee521yx.*qed52) - mee531yx.*qed53;
	pvd2e3e5=mee521yx.*qed51;
	pvd3e3e5=mee531yx.*qed51;
	jje3e3e5=ieeppe3e3e5 + irepe3e3e5 + zz5;
	mse15=msde51x.*qe51;
	mse25=msde52y.*qe52 + msde53y.*qe53;
	msed15=msde51x.*qed51;
	msed25=msde52y.*qed52 + msde53y.*qed53;
	ms15=mse15 + mx5;
	ms25=mse25 + my5;
	jer652=pv352 + pvp2e3e5;
	jer653=mre53xy + pvp3e3e5;
	jee2e2e5=mee522xx + mee522yy;
	jee3e2e5=mee532xx + mee532yy;
	jee3e3e5=mee533xx + mee533yy;
	jinintj4e1e5=m5.*s5;
	jinintj4e2e5=c5.*m5;
	jinje4e4e5=c5.*jinintj4e2e5 + jinintj4e1e5.*s5;
	jinje5e4e5=c5.*ms15 - ms25.*s5;
	jinje9e4e5=msde51x.*s5;
	jinje10e4e5=c5.*msde52y;
	jinje11e4e5=c5.*msde53y;
	corioj11=-(g3.*m1);
	dv332=-qd2.^2;
	clin12=dv332.*ms12 - 2.*msed22.*qd2;
	clin22=dv332.*ms22 + 2.*msed12.*qd2;
	cang32=2.*(ieepde3e3e2 + irede3e3e2).*qd2;
	cej21=2.*pvd1e3e2.*qd2 - (ieep3e3e2e1 +...
		mre21xx).*qd2.^2 + ke2xx.*qe21;
	cej22=2.*pvd2e3e2.*qd2 - ieep3e3e2e2.*qd2.^2 +...
		ke2yy.*qe22 + ke2yz.*qe23;
	cej23=2.*pvd3e3e2.*qd2 - ieep3e3e2e3.*qd2.^2 +...
		ke2zy.*qe22 + ke2zz.*qe23;
	gfield12=-(g3.*s2);
	gfield22=-(c2.*g3);
	corioj12=c2.*clin22 + gfield12.*jinintj1e1e2 +...
		gfield22.*jinintj1e2e2 + clin12.*s2;
	corioj22=cang32 + gfield22.*ms12 - gfield12.*ms22;
	corioj62=cej21 + gfield12.*msde21x;
	corioj72=cej22 + gfield22.*msde22y;
	corioj82=cej23 + gfield22.*msde23y;
	dv333=-w33.^2;
	clin13=dv333.*mx3;
	clin23=dv333.*my3;
	acc313=-(qd2.*vei23);
	acc323=qd2.*vei13;
	hlin13=2.*acc313 + a1e1e3.*dv332.*loo13 +...
		a2e1e3.*dv332.*looe22;
	hlin23=2.*acc323 + a1e2e3.*dv332.*loo13 +...
		a2e2e3.*dv332.*looe22;
	gfield13=a1e1e3.*gfield12 + a2e1e3.*gfield22;
	gfield23=a1e2e3.*gfield12 + a2e2e3.*gfield22;
	termacc13=gfield13 + hlin13;
	termacc23=gfield23 + hlin23;
	corioj13=clin13.*jacvj1e1e3 + clin23.*...
		jacvj2e1e3 + jinintj1e1e3.*termacc13 +...
		jinintj1e2e3.*termacc23;
	corioj23=-(clin13.*qtri1e3e3) - clin23.*...
		qtri2e3e3 + jinintj2e1e3.*termacc13 +...
		jinintj2e2e3.*termacc23;
	corioj33=-(my3.*termacc13) + mx3.*termacc23;
	corioj63=clin13.*matphi1e6e3 + clin23.*...
		matphi2e6e3 + jinintj6e1e3.*termacc13 +...
		jinintj6e2e3.*termacc23;
	corioj73=clin13.*matphi1e7e3 + clin23.*...
		matphi2e7e3 + jinintj7e1e3.*termacc13 +...
		jinintj7e2e3.*termacc23;
	corioj83=jinintj8e1e3.*termacc13 +...
		jinintj8e2e3.*termacc23;
	corioj44=-(g3.*m4);
	dv335=-qd5.^2;
	clin15=dv335.*ms15 - 2.*msed25.*qd5;
	clin25=dv335.*ms25 + 2.*msed15.*qd5;
	cang35=2.*(ieepde3e3e5 + irede3e3e5).*qd5;
	cej51=2.*pvd1e3e5.*qd5 - (ieep3e3e5e1 +...
		mre51xx).*qd5.^2 + ke5xx.*qe51;
	cej52=2.*pvd2e3e5.*qd5 - ieep3e3e5e2.*...
		qd5.^2 + ke5yy.*qe52 + ke5yz.*qe53;
	cej53=2.*pvd3e3e5.*qd5 - ieep3e3e5e3.*...
		qd5.^2 + ke5zy.*qe52 + ke5zz.*qe53;
	gfield15=-(g3.*s5);
	gfield25=-(c5.*g3);
	corioj45=c5.*clin25 + gfield15.*jinintj4e1e5 +...
		gfield25.*jinintj4e2e5 + clin15.*s5;
	corioj55=cang35 + gfield25.*ms15 - gfield15.*ms25;
	corioj95=cej51 + gfield15.*msde51x;
	corioj105=cej52 + gfield25.*msde52y;
	corioj115=cej53 + gfield25.*msde53y;
	jintote1e1=ia1 + jinje1e1e2 + jinje1e1e3 + m1;
	jintote2e1=jinje2e1e2 + jinje2e1e3;
	jintote3e1=jinje3e1e3;
	jintote4e1=0;
	jintote5e1=0;
	jintote6e1=jinje6e1e2 + jinje6e1e3;
	jintote7e1=jinje7e1e2 + jinje7e1e3;
	jintote8e1=jinje8e1e2 + jinje8e1e3;
	jintote9e1=0;
	jintote10e1=0;
	jintote11e1=0;
	jintote2e2=ia2 + jinje2e2e3 + jje3e3e2;
	jintote3e2=jinje3e2e3;
	jintote4e2=0;
	jintote5e2=0;
	jintote6e2=jinje6e2e3 + pvp1e3e2;
	jintote7e2=jer622 + jinje7e2e3;
	jintote8e2=jer623 + jinje8e2e3;
	jintote9e2=0;
	jintote10e2=0;
	jintote11e2=0;
	jintote3e3=ia3 + zz3;
	jintote4e3=0;
	jintote5e3=0;
	jintote6e3=jinintj6e6e3;
	jintote7e3=jinintj7e6e3;
	jintote8e3=jinintj8e6e3;
	jintote9e3=0;
	jintote10e3=0;
	jintote11e3=0;
	jintote4e4=ia4 + jinje4e4e5 + m4;
	jintote5e4=jinje5e4e5;
	jintote6e4=0;
	jintote7e4=0;
	jintote8e4=0;
	jintote9e4=jinje9e4e5;
	jintote10e4=jinje10e4e5;
	jintote11e4=jinje11e4e5;
	jintote5e5=ia5 + jje3e3e5;
	jintote6e5=0;
	jintote7e5=0;
	jintote8e5=0;
	jintote9e5=pvp1e3e5;
	jintote10e5=jer652;
	jintote11e5=jer653;
	jintote6e6=jinje6e6e3 + mee211xx;
	jintote7e6=jinje7e6e3;
	jintote8e6=jinje8e6e3;
	jintote9e6=0;
	jintote10e6=0;
	jintote11e6=0;
	jintote7e7=jee2e2e2 + jinje7e7e3;
	jintote8e7=jee3e2e2 + jinje8e7e3;
	jintote9e7=0;
	jintote10e7=0;
	jintote11e7=0;
	jintote8e8=jee3e3e2 + jinje8e8e3;
	jintote9e8=0;
	jintote10e8=0;
	jintote11e8=0;
	jintote9e9=mee511xx;
	jintote10e9=0;
	jintote11e9=0;
	jintote10e10=jee2e2e5;
	jintote11e10=jee3e2e5;
	jintote11e11=jee3e3e5;
	coriotot1=corioj11 + corioj12 + corioj13 +...
		fv1.*qd1 + fs1.*sign(qd1);
	coriotot2=corioj22 + corioj23 + fv2.*qd2 +...
		fs2.*sign(qd2);
	coriotot3=corioj33 + fv3.*qd3 + fs3.*sign(qd3);
	coriotot4=corioj44 + corioj45 + fv4.*qd4 +...
		fs4.*sign(qd4);
	coriotot5=corioj55 + fv5.*qd5 + fs5.*sign(qd5);
	coriotot6=corioj62 + corioj63;
	coriotot7=corioj72 + corioj73;
	coriotot8=corioj82 + corioj83;
	coriotot9=corioj95;
	coriotot10=corioj105;
	coriotot11=corioj115;
          
%% Inertia matrix of the real pkm
	matint1e1=jintote1e1+j11.*jintote2e1+...
		j21.*jintote3e1+j31.*jintote5e1 ;
	matint1e2=j12.*jintote2e1+j22.*jintote3e1+...
		jintote4e1+j32.*jintote5e1 ;
	matint1e3=j13.*jintote2e1+j23.*jintote3e1+...
		j33.*jintote5e1+jintote6e1 ;
	matint1e4=j14.*jintote2e1+j24.*jintote3e1+...
		j34.*jintote5e1+jintote7e1 ;
	matint1e5=j15.*jintote2e1+j25.*jintote3e1+...
		j35.*jintote5e1+jintote8e1 ;
	matint1e6=j16.*jintote2e1+j26.*jintote3e1+...
		j36.*jintote5e1+jintote9e1 ;
	matint1e7=j17.*jintote2e1+j37.*jintote3e1+...
		j27.*jintote5e1+jintote10e1 ;
	matint1e8=j18.*jintote2e1+j28.*jintote3e1+...
		j38.*jintote5e1+jintote11e1 ;

	matint2e1=jintote2e1+j11.*jintote2e2+...
		j21.*jintote3e2+j31.*jintote5e2 ;
	matint2e2=j12.*jintote2e2+j22.*jintote3e2+...
		jintote4e2+j32.*jintote5e2 ;
	matint2e3=j13.*jintote2e2+j23.*jintote3e2+...
		j33.*jintote5e2+jintote6e2 ;
	matint2e4=j14.*jintote2e2+j24.*jintote3e2+...
		j34.*jintote5e2+jintote7e2 ;
	matint2e5=j15.*jintote2e2+j25.*jintote3e2+...
		j35.*jintote5e2+jintote8e2 ;
	matint2e6=j16.*jintote2e2+j26.*jintote3e2+...
		j36.*jintote5e2+jintote9e2 ;
	matint2e7=j17.*jintote2e2+j37.*jintote3e2+...
		j27.*jintote5e2+jintote10e2 ;
	matint2e8=j18.*jintote2e2+j28.*jintote3e2+...
		j38.*jintote5e2+jintote11e2 ;

	matint3e1=jintote3e1+j11.*jintote3e2+...
		j21.*jintote3e3+j31.*jintote5e3 ;
	matint3e2=j12.*jintote3e2+j22.*jintote3e3+...
		jintote4e3+j32.*jintote5e3 ;
	matint3e3=j13.*jintote3e2+j23.*jintote3e3+...
		j33.*jintote5e3+jintote6e3 ;
	matint3e4=j14.*jintote3e2+j24.*jintote3e3+...
		j34.*jintote5e3+jintote7e3 ;
	matint3e5=j15.*jintote3e2+j25.*jintote3e3+...
		j35.*jintote5e3+jintote8e3 ;
	matint3e6=j16.*jintote3e2+j26.*jintote3e3+...
		j36.*jintote5e3+jintote9e3 ;
	matint3e7=j17.*jintote3e2+j37.*jintote3e3+...
		j27.*jintote5e3+jintote10e3 ;
	matint3e8=j18.*jintote3e2+j28.*jintote3e3+...
		j38.*jintote5e3+jintote11e3 ;

	matint4e1=jintote4e1+j11.*jintote4e2+...
		j21.*jintote4e3+j31.*jintote5e4 ;
	matint4e2=j12.*jintote4e2+j22.*jintote4e3+...
		jintote4e4+j32.*jintote5e4 ;
	matint4e3=j13.*jintote4e2+j23.*jintote4e3+...
		j33.*jintote5e4+jintote6e4 ;
	matint4e4=j14.*jintote4e2+j24.*jintote4e3+...
		j34.*jintote5e4+jintote7e4 ;
	matint4e5=j15.*jintote4e2+j25.*jintote4e3+...
		j35.*jintote5e4+jintote8e4 ;
	matint4e6=j16.*jintote4e2+j26.*jintote4e3+...
		j36.*jintote5e4+jintote9e4 ;
	matint4e7=j17.*jintote4e2+j37.*jintote4e3+...
		j27.*jintote5e4+jintote10e4 ;
	matint4e8=j18.*jintote4e2+j28.*jintote4e3+...
		j38.*jintote5e4+jintote11e4 ;

	matint5e1=jintote5e1+j11.*jintote5e2+...
		j21.*jintote5e3+j31.*jintote5e5 ;
	matint5e2=j12.*jintote5e2+j22.*jintote5e3+...
		jintote5e4+j32.*jintote5e5 ;
	matint5e3=j13.*jintote5e2+j23.*jintote5e3+...
		j33.*jintote5e5+jintote6e5 ;
	matint5e4=j14.*jintote5e2+j24.*jintote5e3+...
		j34.*jintote5e5+jintote7e5 ;
	matint5e5=j15.*jintote5e2+j25.*jintote5e3+...
		j35.*jintote5e5+jintote8e5 ;
	matint5e6=j16.*jintote5e2+j26.*jintote5e3+...
		j36.*jintote5e5+jintote9e5 ;
	matint5e7=j17.*jintote5e2+j37.*jintote5e3+...
		j27.*jintote5e5+jintote10e5 ;
	matint5e8=j18.*jintote5e2+j28.*jintote5e3+...
		j38.*jintote5e5+jintote11e5 ;

	matint6e1=jintote6e1+j11.*jintote6e2+...
		j21.*jintote6e3+j31.*jintote6e5 ;
	matint6e2=j12.*jintote6e2+j22.*jintote6e3+...
		jintote6e4+j32.*jintote6e5 ;
	matint6e3=j13.*jintote6e2+j23.*jintote6e3+...
		j33.*jintote6e5+jintote6e6 ;
	matint6e4=j14.*jintote6e2+j24.*jintote6e3+...
		j34.*jintote6e5+jintote7e6 ;
	matint6e5=j15.*jintote6e2+j25.*jintote6e3+...
		j35.*jintote6e5+jintote8e6 ;
	matint6e6=j16.*jintote6e2+j26.*jintote6e3+...
		j36.*jintote6e5+jintote9e6 ;
	matint6e7=j17.*jintote6e2+j37.*jintote6e3+...
		j27.*jintote6e5+jintote10e6 ;
	matint6e8=j18.*jintote6e2+j28.*jintote6e3+...
		j38.*jintote6e5+jintote11e6 ;

	matint7e1=jintote7e1+j11.*jintote7e2+...
		j21.*jintote7e3+j31.*jintote7e5 ;
	matint7e2=j12.*jintote7e2+j22.*jintote7e3+...
		jintote7e4+j32.*jintote7e5 ;
	matint7e3=j13.*jintote7e2+j23.*jintote7e3+...
		j33.*jintote7e5+jintote7e6 ;
	matint7e4=j14.*jintote7e2+j24.*jintote7e3+...
		j34.*jintote7e5+jintote7e7 ;
	matint7e5=j15.*jintote7e2+j25.*jintote7e3+...
		j35.*jintote7e5+jintote8e7 ;
	matint7e6=j16.*jintote7e2+j26.*jintote7e3+...
		j36.*jintote7e5+jintote9e7 ;
	matint7e7=j17.*jintote7e2+j37.*jintote7e3+...
		j27.*jintote7e5+jintote10e7 ;
	matint7e8=j18.*jintote7e2+j28.*jintote7e3+...
		j38.*jintote7e5+jintote11e7 ;

	matint8e1=jintote8e1+j11.*jintote8e2+...
		j21.*jintote8e3+j31.*jintote8e5 ;
	matint8e2=j12.*jintote8e2+j22.*jintote8e3+...
		jintote8e4+j32.*jintote8e5 ;
	matint8e3=j13.*jintote8e2+j23.*jintote8e3+...
		j33.*jintote8e5+jintote8e6 ;
	matint8e4=j14.*jintote8e2+j24.*jintote8e3+...
		j34.*jintote8e5+jintote8e7 ;
	matint8e5=j15.*jintote8e2+j25.*jintote8e3+...
		j35.*jintote8e5+jintote8e8 ;
	matint8e6=j16.*jintote8e2+j26.*jintote8e3+...
		j36.*jintote8e5+jintote9e8 ;
	matint8e7=j17.*jintote8e2+j37.*jintote8e3+...
		j27.*jintote8e5+jintote10e8 ;
	matint8e8=j18.*jintote8e2+j28.*jintote8e3+...
		j38.*jintote8e5+jintote11e8 ;

	matint9e1=jintote9e1+j11.*jintote9e2+...
		j21.*jintote9e3+j31.*jintote9e5 ;
	matint9e2=j12.*jintote9e2+j22.*jintote9e3+...
		jintote9e4+j32.*jintote9e5 ;
	matint9e3=j13.*jintote9e2+j23.*jintote9e3+...
		j33.*jintote9e5+jintote9e6 ;
	matint9e4=j14.*jintote9e2+j24.*jintote9e3+...
		j34.*jintote9e5+jintote9e7 ;
	matint9e5=j15.*jintote9e2+j25.*jintote9e3+...
		j35.*jintote9e5+jintote9e8 ;
	matint9e6=j16.*jintote9e2+j26.*jintote9e3+...
		j36.*jintote9e5+jintote9e9 ;
	matint9e7=j17.*jintote9e2+j37.*jintote9e3+...
		j27.*jintote9e5+jintote10e9 ;
	matint9e8=j18.*jintote9e2+j28.*jintote9e3+...
		j38.*jintote9e5+jintote11e9 ;

	matint10e1=jintote10e1+j11.*jintote10e2+...
		j21.*jintote10e3+j31.*jintote10e5 ;
	matint10e2=j12.*jintote10e2+j22.*jintote10e3+...
		jintote10e4+j32.*jintote10e5 ;
	matint10e3=j13.*jintote10e2+j23.*jintote10e3+...
		j33.*jintote10e5+jintote10e6 ;
	matint10e4=j14.*jintote10e2+j24.*jintote10e3+...
		j34.*jintote10e5+jintote10e7 ;
	matint10e5=j15.*jintote10e2+j25.*jintote10e3+...
		j35.*jintote10e5+jintote10e8 ;
	matint10e6=j16.*jintote10e2+j26.*jintote10e3+...
		j36.*jintote10e5+jintote10e9 ;
	matint10e7=j17.*jintote10e2+j37.*jintote10e3+...
		j27.*jintote10e5+jintote10e10 ;
	matint10e8=j18.*jintote10e2+j28.*jintote10e3+...
		j38.*jintote10e5+jintote11e10 ;

	matint11e1=jintote11e1+j11.*jintote11e2+...
		j21.*jintote11e3+j31.*jintote11e5 ;
	matint11e2=j12.*jintote11e2+j22.*jintote11e3+...
		jintote11e4+j32.*jintote11e5 ;
	matint11e3=j13.*jintote11e2+j23.*jintote11e3+...
		j33.*jintote11e5+jintote11e6 ;
	matint11e4=j14.*jintote11e2+j24.*jintote11e3+...
		j34.*jintote11e5+jintote11e7 ;
	matint11e5=j15.*jintote11e2+j25.*jintote11e3+...
		j35.*jintote11e5+jintote11e8 ;
	matint11e6=j16.*jintote11e2+j26.*jintote11e3+...
		j36.*jintote11e5+jintote11e9 ;
	matint11e7=j17.*jintote11e2+j37.*jintote11e3+...
		j27.*jintote11e5+jintote11e10 ;
	matint11e8=j18.*jintote11e2+j28.*jintote11e3+...
		j38.*jintote11e5+jintote11e11 ;

	matmass1e1=matint1e1+j11.*matint2e1+...
		j21.*matint3e1+j31.*matint5e1;
	matmass2e1=j12.*matint2e1+j22.*matint3e1+...
		matint4e1+j32.*matint5e1;
	matmass3e1=j13.*matint2e1+j23.*matint3e1+...
		j33.*matint5e1+matint6e1;
	matmass4e1=j14.*matint2e1+j24.*matint3e1+...
		j34.*matint5e1+matint7e1;
	matmass5e1=j15.*matint2e1+j25.*matint3e1+...
		j35.*matint5e1+matint8e1;
	matmass6e1=j16.*matint2e1+j26.*matint3e1+...
		j36.*matint5e1+matint9e1;
	matmass7e1=j17.*matint2e1+j37.*matint3e1+...
		j27.*matint5e1+matint10e1;
	matmass8e1=j18.*matint2e1+j28.*matint3e1+...
		j38.*matint5e1+matint11e1;

	matmass1e2=matmass2e1;
	matmass2e2=j12.*matint2e2+j22.*matint3e2+...
		matint4e2+j32.*matint5e2;
	matmass3e2=j13.*matint2e2+j23.*matint3e2+...
		j33.*matint5e2+matint6e2;
	matmass4e2=j14.*matint2e2+j24.*matint3e2+...
		j34.*matint5e2+matint7e2;
	matmass5e2=j15.*matint2e2+j25.*matint3e2+...
		j35.*matint5e2+matint8e2;
	matmass6e2=j16.*matint2e2+j26.*matint3e2+...
		j36.*matint5e2+matint9e2;
	matmass7e2=j17.*matint2e2+j37.*matint3e2+...
		j27.*matint5e2+matint10e2;
	matmass8e2=j18.*matint2e2+j28.*matint3e2+...
		j38.*matint5e2+matint11e2;

	matmass1e3=matmass3e1;
	matmass2e3=matmass3e2;
	matmass3e3=j13.*matint2e3+j23.*matint3e3+...
		j33.*matint5e3+matint6e3;
	matmass4e3=j14.*matint2e3+j24.*matint3e3+...
		j34.*matint5e3+matint7e3;
	matmass5e3=j15.*matint2e3+j25.*matint3e3+...
		j35.*matint5e3+matint8e3;
	matmass6e3=j16.*matint2e3+j26.*matint3e3+...
		j36.*matint5e3+matint9e3;
	matmass7e3=j17.*matint2e3+j37.*matint3e3+...
		j27.*matint5e3+matint10e3;
	matmass8e3=j18.*matint2e3+j28.*matint3e3+...
		j38.*matint5e3+matint11e3;

	matmass1e4=matmass4e1;
	matmass2e4=matmass4e2;
	matmass3e4=matmass4e3;
	matmass4e4=j14.*matint2e4+j24.*matint3e4+...
		j34.*matint5e4+matint7e4;
	matmass5e4=j15.*matint2e4+j25.*matint3e4+...
		j35.*matint5e4+matint8e4;
	matmass6e4=j16.*matint2e4+j26.*matint3e4+...
		j36.*matint5e4+matint9e4;
	matmass7e4=j17.*matint2e4+j37.*matint3e4+...
		j27.*matint5e4+matint10e4;
	matmass8e4=j18.*matint2e4+j28.*matint3e4+...
		j38.*matint5e4+matint11e4;

	matmass1e5=matmass5e1;
	matmass2e5=matmass5e2;
	matmass3e5=matmass5e3;
	matmass4e5=matmass5e4;
	matmass5e5=j15.*matint2e5+j25.*matint3e5+...
		j35.*matint5e5+matint8e5;
	matmass6e5=j16.*matint2e5+j26.*matint3e5+...
		j36.*matint5e5+matint9e5;
	matmass7e5=j17.*matint2e5+j37.*matint3e5+...
		j27.*matint5e5+matint10e5;
	matmass8e5=j18.*matint2e5+j28.*matint3e5+...
		j38.*matint5e5+matint11e5;

	matmass1e6=matmass6e1;
	matmass2e6=matmass6e2;
	matmass3e6=matmass6e3;
	matmass4e6=matmass6e4;
	matmass5e6=matmass6e5;
	matmass6e6=j16.*matint2e6+j26.*matint3e6+...
		j36.*matint5e6+matint9e6;
	matmass7e6=j17.*matint2e6+j37.*matint3e6+...
		j27.*matint5e6+matint10e6;
	matmass8e6=j18.*matint2e6+j28.*matint3e6+...
		j38.*matint5e6+matint11e6;

	matmass1e7=matmass7e1;
	matmass2e7=matmass7e2;
	matmass3e7=matmass7e3;
	matmass4e7=matmass7e4;
	matmass5e7=matmass7e5;
	matmass6e7=matmass7e6;
	matmass7e7=j17.*matint2e7+j37.*matint3e7+...
		j27.*matint5e7+matint10e7;
	matmass8e7=j18.*matint2e7+j28.*matint3e7+...
		j38.*matint5e7+matint11e7;

	matmass1e8=matmass8e1;
	matmass2e8=matmass8e2;
	matmass3e8=matmass8e3;
	matmass4e8=matmass8e4;
	matmass5e8=matmass8e5;
	matmass6e8=matmass8e6;
	matmass7e8=matmass8e6;
	matmass8e8=j18.*matint2e8+j28.*matint3e8+...
		j38.*matint5e8+matint11e8;

	matmass=[matmass1e1 matmass1e2 matmass1e3 ...
		matmass1e4 matmass1e5 matmass1e6 matmass1e7 ...
		matmass1e8 ;
					 matmass2e1 matmass2e2 matmass2e3 ...
		matmass2e4 matmass2e5 matmass2e6 matmass2e7 ...
		matmass2e8 ;
					 matmass3e1 matmass3e2 matmass3e3 ...
		matmass3e4 matmass3e5 matmass3e6 matmass3e7 ...
		matmass3e8 ; 
					 matmass4e1 matmass4e2 matmass4e3 ...
		matmass4e4 matmass4e5 matmass4e6 matmass4e7 ...
		matmass4e8 ;
					 matmass5e1 matmass5e2 matmass5e3 ...
		matmass5e4 matmass5e5 matmass5e6 matmass5e7 ...
		matmass5e8 ;
					 matmass6e1 matmass6e2 matmass6e3 ...
		matmass6e4 matmass6e5 matmass6e6 matmass6e7 ...
		matmass6e8 ;
					 matmass7e1 matmass7e2 matmass7e3 ...
		matmass7e4 matmass7e5 matmass7e6 matmass7e7 ...
		matmass7e8 ;
					 matmass8e1 matmass8e2 matmass8e3 ...
		matmass8e4 matmass8e5 matmass8e6 matmass8e7 ...
		matmass8e8 ];
	 
	mrr = matmass(1:2,1:2) ;
	mre = matmass(1:2,3:8) ;
	mer = mre' ;
	mee = matmass(3:8,3:8) ;

%% Vector of Coriolis / centrifugal / 
%% friction effects of the real pkm
	vcorioint1 = jintote2e1.*tacc1+jintote3e1.*...
		tacc2+jintote5e1.*tacc3+coriotot1;
	vcorioint2 = jintote2e2.*tacc1+jintote3e2.*...
		tacc2+jintote5e2.*tacc3+coriotot2;
	vcorioint3 = jintote3e2.*tacc1+jintote3e3.*...
		tacc2+jintote5e3.*tacc3+coriotot3;
	vcorioint4 = jintote4e2.*tacc1+jintote4e3.*...
		tacc2+jintote5e4.*tacc3+coriotot4;
	vcorioint5 = jintote5e2.*tacc1+jintote5e3.*...
		tacc2+jintote5e5.*tacc3+coriotot5;
	vcorioint6 = jintote6e2.*tacc1+jintote6e3.*...
		tacc2+jintote6e5.*tacc3+coriotot6;
	vcorioint7 = jintote7e2.*tacc1+jintote7e3.*...
		tacc2+jintote7e5.*tacc3+coriotot7;
	vcorioint8 = jintote8e2.*tacc1+jintote8e3.*...
		tacc2+jintote8e5.*tacc3+coriotot8;
	vcorioint9 = jintote9e2.*tacc1+jintote9e3.*...
		tacc2+jintote9e5.*tacc3+coriotot9;
	vcorioint10 = jintote10e2.*tacc1+jintote10e3.*...
		tacc2+jintote10e5.*tacc3+coriotot10;
	vcorioint11 = jintote11e2.*tacc1+jintote11e3.*...
		tacc2+jintote11e5.*tacc3+coriotot11;

	corio1 = vcorioint1+j11.*vcorioint2+...
		j21.*vcorioint3+j31.*vcorioint5;
	corio2 = j12.*vcorioint2+j22.*vcorioint3+...
		vcorioint4+j32.*vcorioint5;
	corio3 = j13.*vcorioint2+j23.*vcorioint3+...
		j33.*vcorioint5+vcorioint6;
	corio4 = j14.*vcorioint2+j24.*vcorioint3+...
		j34.*vcorioint5+vcorioint7;
	corio5 = j15.*vcorioint2+j25.*vcorioint3+...
		j35.*vcorioint5+vcorioint8;
	corio6 = j16.*vcorioint2+j26.*vcorioint3+...
		j36.*vcorioint5+vcorioint9;
	corio7 = j17.*vcorioint2+j37.*vcorioint3+...
		j27.*vcorioint5+vcorioint10;
	corio8 = j18.*vcorioint2+j28.*vcorioint3+...
		j38.*vcorioint5+vcorioint11;

	corio = [corio1;corio2;corio3;corio4;...
		corio5;corio6;corio7;corio8]; 

	cr = corio(1:2) ;
	ce = corio(3:8) ;

% Solving the system
% Elastic accelerations
	qedd = -pinv(mee)*(mer*[qdd1;qdd4] + ce) ;
	
% input efforts
	tau = mrr*[qdd1;qdd4] + mre*qedd + cr ;

	result = [tau;qedd] ;