m1  = 1;
l1  = 1;
me  = 2;
deltae = 30/180*pi;
I1  = 0.12;
lc1 = 0.5;
Ie  = 0.25;
lce = 0.6;



a1  = I1+m1*lc1^2+Ie+me*lce^2+me*l1^2;
a2  = Ie+me*lce^2;
a3  = me*l1*lce*cos(deltae);
a4  = me*l1*lce*sin(deltae);


%H11 = a1+2*a3*cos(q2)+2*a4*sin(q2);
%H12 = a2+a3*cos(q2)+a4*sin(q2);
%H21 = H12;
%H22 = a2;
%h   = a3*sin(q2)-a4*cos(q2);
Kd  = 100;
Kp  = 2000;
qd1 = 60/180*pi;
qd2 = 90/180*pi;