clear?all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Model?of?three-link?robot?（“Inverse?Control?of?a?two-link?manipulator“?2001）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l???=?[0.20.3];?%length?of?link?i?（each?colum?corresponds?to?one?robot）
lc??=?[0.10.15];?%distances?from?previous?joint?to?CM?of?link?i?（each?colum?corresponds?to?one?robot）
m???=?[11.5];?%mass?of?inertia?of?link?i
I???=?[0.0130.045];?%moment?of?inertia?of?link?i
%constant?part?of?parameters
Theta1?=?m（1）*lc（1）^2+m（2）*（l（1）^2+lc（2）^2）+I（1）+I（2）;?%?;
Theta2?=?m（2）*l（1）*lc（2）;??????????????????????????????%?;
Theta3?=?m（2）*lc（2）^2+I（2）;????????????????????????????%?;
Theta4?=?m（1）*lc（1）+m（2）*l（1）;
Theta5?=?m（2）*l（2）;
Theta??=?[Theta1;?Theta2;?Theta3;?Theta4;?Theta5];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%paremeters?of?the?graph?with?4?leaders?and?6?followers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%control?paremeter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_p=3;
k_v=2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%initial?data?of?six?two-link?robots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%qd=[0.3;0.1];????????????????????????%%the?reference?position
%vd0=[0.1;0.1]*0;?????????????????????%%initial?value?of?the?velocity?estimtor
q0=[0;0];?%intitial?positions?of?the?followers????
%?for?i=1:6
%?dq0（:i）?=?[0.2*i-0.5;?-0.1*i+0.5];?%intitial?velocities?of?the?followers
%?end
dq0=[0?;?0];
%s0=zeros（36）;
%zd=0;
g0=9.8;???????????????????????????????%%g?is?the?gravitational?acceleration