%author:????????????Wang?junhe
%time:???????????????20201013?10:55（completed）
%describe:?????????estimate?parameter?of?force?transducer
%algorithm?source:?《基于力控制的工業機器人精密裝配研究》
clear;
clc;
close;

%define?parameter
number?=11;
one?=?ones（number1）;
R3?=?zeros（number3）;
A1?=?zeros（number2）;
B1?=?zeros（number1）;
A2?=?zeros（number*24）;
B2?=?zeros（number*21）;
A3?=?zeros（number*36）;
B3?=?zeros（number*31）;

%robot?forward?kinematics（verified）
q?=?load（‘C:\Users\shumiaogeek\Documents\MATLAB\bianshi\data\q.txt‘）;
%???????theta????d???????????a????????alpha?????offset
SL0=link（[0??????0.3113-0.08946??0?????0????????0]‘standard‘）;
SL1=link（[0???????0.089459??????????0????pi/2????????0?????]‘standard‘）;?
SL2=link（[0???????0??????????-0.425?????0???????????0?????]‘standard‘）;
SL3=link（[0??????0??????????-0.39225???0????????0?????]‘standard‘）;
SL4=link（[0??????0.10915???????0????????pi/2????????0?????]‘standard‘）;
SL5=link（[0??????0.09465???????0???????-pi/2????????0?????]‘standard‘）;
SL6=link（[0??????0.0823?????????0????????0?????????????0?????]‘standard‘）;
starobot=Seriallink（[SL0?SL1?SL2?SL3?SL4?SL5?SL6]‘name‘‘standard‘）;

%record?data
filepath?=?‘C:\Users\shumiaogeek\Documents\MATLAB\bianshi\data\‘;
for?i?=?1:number
n?=?load（[filepath?num2str（i）?‘.txt‘]）;
fx?=?median（n（:3））/1000000;
fy?=?median（n（:4））/1000000;
fz?=?median（n（:5））/1000000;
tx?=?median（n（:6））/1000000;
ty?=?median（n（:7））/1000000;
tz?=?median（n（:8））/1000000;
qq?=?q（i:）*pi/180;
stat06=starobot.fkine（qq）;
R3（i1）?=?stat06.n（3?1）;
R3（i2）?=?stat06.o（3?1）;
R3（i3）?=?stat06.a（3?1）;
A1?=?[-R3（:3）?one?];
B1（i）?=fz;
B2（i*2-1:）?=?fx;
B2（i*2:）?=?fy;
B3（i*3-2:）=tx;
B3（i*3-1:）=ty;
B（i*3:）=tz;
end
%estimate?Gfzdrift
P1=A1‘?*?A1\?A1‘?*?B1;
G?=?P1（1）;
fzdrift?=?P1（2）;

%estimate?cosasinafxdriftfydrift
for?i?=?1:number
A2（i*2-1:）=[-G*R3（i1）-G*R3（i2）10];
A2（i*2:）?=[-G*R3（i2）G*R3（i1）01];
end
P2?=?A2‘?*?A2\A2‘?*?B2;
cosa?=?P2（1）;
sina?=?P2（2）;
fxdrift?=?P2（3）;
fydrift?=?P2（4）;

%estimate?pxpypztxdrifttydrifttzdrift
for?i?=?1:number
A3（i*3-2:）=[0-G*R3（i3）-G*sina*R3（i1）+G*cosa*R3（i2）100];
A3（i*3-1:）=[G*R3（i3）0-G*cosa*R3（i1）-G*sina*R3（i2）010];
A3（i*3:）=[G*sina*R3（i1）-G*cosa*R3（i2）G*cosa*R3（i1）+G*sina*R3（i2）0001];
end
P3?=?A3‘*A3\A3‘*B3;
px?=?P3（1）;
py?=?P3（2）;
pz?=?P3（3）;
txdrift?=?P3（4）;
tydrift?=?P3（5）;
tzdrift?=?P3（6）;

%display
value?=?{numberGfxdriftfydriftfzdrifttxdrifttydrifttzdriftcosasinapxpypz};
title?=?{‘number?of?simple‘‘G‘‘fxdrift‘‘fydrift‘‘fzdrift‘‘txdrift‘‘tydrift‘‘tzdrift‘‘cosa‘‘sina‘‘px‘‘py‘‘pz‘};
result=[title;value]‘;
xlswrite（‘Parameter?Estimation.xls‘result）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件?????1789996??2020-10-12?20:07??data\1.txt
?????文件?????1313296??2020-10-12?20:27??data\10.txt
?????文件?????1811696??2020-10-12?20:27??data\11.txt
?????文件?????2008068??2020-10-12?20:06??data\2.txt
?????文件?????2659604??2020-10-12?20:06??data\3.txt
?????文件?????1309096??2020-10-12?20:06??data\4.txt
?????文件?????2801496??2020-10-12?20:05??data\5.txt
?????文件?????1588289??2020-10-12?20:05??data\6.txt
?????文件??????802495??2020-10-12?20:05??data\7.txt
?????文件?????2105696??2020-10-12?20:04??data\8.txt
?????文件?????1714396??2020-10-12?20:26??data\9.txt
?????文件?????????467??2020-10-12?20:24??data\q.txt
?????文件????????2773??2020-10-20?02:36??sensorF_T_PE.m
?????文件?????????794??2020-10-20?02:36??verify.m