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

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-11-15?10:02??機(jī)械臂計(jì)算力矩控制\
?????目錄???????????0??2018-11-15?10:02??機(jī)械臂計(jì)算力矩控制\Tracking?Control\
?????文件???????20480??2017-12-08?17:45??機(jī)械臂計(jì)算力矩控制\Tracking?Control\Thumbs.db
?????文件????????1899??2017-12-27?10:24??機(jī)械臂計(jì)算力矩控制\Tracking?Control\initialcase2.m
?????文件?????????178??2010-02-16?08:32??機(jī)械臂計(jì)算力矩控制\Tracking?Control\invM.m
?????文件?????????819??2017-12-29?00:20??機(jī)械臂計(jì)算力矩控制\Tracking?Control\plotcase1.m
?????文件?????????880??2017-12-29?00:11??機(jī)械臂計(jì)算力矩控制\Tracking?Control\plotcase2.m
?????文件?????????723??2017-12-27?10:28??機(jī)械臂計(jì)算力矩控制\Tracking?Control\plotcase3.m
?????文件???????18229??2017-12-27?10:28??機(jī)械臂計(jì)算力矩控制\Tracking?Control\trackingcontrol.slx
?????目錄???????????0??2018-11-15?10:02??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\
?????文件???????20480??2017-12-08?17:45??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\Thumbs.db
?????文件????????1899??2017-12-27?10:30??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\initialcase2.m
?????文件?????????178??2010-02-16?08:32??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\invM.m
?????文件?????????819??2017-12-29?00:32??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\plotcase1.m
?????文件?????????880??2017-12-08?18:10??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\plotcase2.m
?????文件???????18464??2017-12-27?10:39??機(jī)械臂計(jì)算力矩控制\Tracking?Control?Sin（x）\trackingcontrolsin.slx