4 |
5 |
3 |
4 |
5 |
6 | Comparisons among Six Numerical Methods for Solving Repetitive Motion Planning of Redundant Robot Manipulators 7 |
8 | 9 | 10 |
11 |
12 | 
28 |
29 | If you find this work helpful, please kindly consider citing our paper in your publications.
30 |
31 | ```
32 | @inproceedings{kong2018six,
33 | title={Comparisons among Six Numerical Methods for Solving Repetitive Motion Planning of Redundant Robot Manipulators},
34 | author={Zhang, Zhijun and Kong, Lingdong and Yan, Ziyi and Chen, Ke and Li, Shuai and Qu, Xilong and Tan, Ning},
35 | booktitle={Proceedings of the IEEE International Conference on Robotics and Biomimetics},
36 | year={2018}
37 | }
38 | ```
39 |
40 | ## Results
41 |
42 | - Joint drifts
43 | 
44 |
45 | - Joint drifts
46 | 
47 |
48 | - Computation time
49 | 
50 |
51 | ## References
52 |
53 | * Z. Zhang, L. Zheng, J. Yu, Y. Li, and Z. Yu. “Three recurrent neural networks and three numerical methods for solving a repetitive motion planning scheme of redundant robot manipulators,” *IEEE/ASME Transactions on Mechatronics*, 2017.
54 | * L. Xiao and Y. Zhang. “Acceleration-level repetitive motion planning and its experimental verification on a six-link planar robot manipulator,” *IEEE Transactions on Control Systems Technology*, 2013.
55 | * Y. Zhang, Z. Tan, K. Chen, Z. Yang, and X. Lv, “Repetitive motion of redundant robots planned by three kinds of recurrent neural networks and illustrated with a four-link planar manipulators straight-line example,” *Robotics and Autonomous Systems*, 2009.
56 |
--------------------------------------------------------------------------------
/code/EXP2/CBHtaugcH.m:
--------------------------------------------------------------------------------
1 | %old message:
2 | %1.tau_gravity=ZYNrne(q,zeros(1,6),zeros(1,6),[0;0;9.81])
3 | %2.tau_coriolis=ZYNrne(q,dq,zeros(1,6),[0;0;0])
4 | %3.inertia=[ZYNrne(q,zeros(1,6),[1,0,0,0,0,0],[0;0;0]);...
5 | % ZYNrne(q,zeros(1,6),[0,1,0,0,0,0],[0;0;0]);...
6 | % ZYNrne(q,zeros(1,6),[0,0,1,0,0,0],[0;0;0]);...
7 | % ZYNrne(q,zeros(1,6),[0,0,0,1,0,0],[0;0;0]);...
8 | % ZYNrne(q,zeros(1,6),[0,0,0,0,1,0],[0;0;0]);...
9 | % ZYNrne(q,zeros(1,6),[0,0,0,0,0,1],[0;0;0])];
10 | %new usage below (VERSION of April 16th 2001):
11 | %[taug,tauc,inerH]=ZYNtaugcI(a2,a3,d3,d4,d6,q,dq)
12 | function inerH=CBHtaugcH(a2,a3,d3,d4,d6,q)%new add
13 | %common data below
14 | z0=[0;0;1];fext=zeros(6,1);%force/moments on end of arm
15 | alpha_r=[1.5707963267949,0,-1.5707963267949,1.5707963267949,-1.5707963267949,0];
16 | linkR=[zeros(1,3);-0.3638,0.006,0.2275;-0.0203,-0.0141,0.070;0,0.019,0;zeros(1,3);0,0,0.032];
17 | linkM=[0;17.4;4.8;0.82;0.34;0.09];
18 | linkI=[0,0.35,0,0,0,0;0.13,0.524,0.539,0,0,0;0.066,0.086,0.0125,0,0,0;...
19 | 1.8e-3,1.3e-3,1.8e-3,0,0,0;0.3e-3,0.4e-3,0.3e-3,0,0,0;0.15e-3,0.15e-3,0.04e-3,0,0,0];
20 | linkJm=[200e-6;200e-6;200e-6;33e-6;33e-6;33e-6];
21 | linkG=[-62.6111;107.815;-53.7063;76.0364;71.923;76.686];
22 | mys=sin(q);
23 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
24 | myc=cos(q);
25 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
26 | A1=[c1 0 s1 0;s1 0 -c1 0;0 1 0 0;0 0 0 1];
27 | A2=[c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 0;0 0 0 1];
28 | A3=[c3 0 -s3 c3*a3;s3 0 c3 s3*a3;0 -1 0 d3;0 0 0 1];
29 | A4=[c4 0 s4 0;s4 0 -c4 0;0 1 0 d4;0 0 0 1];
30 | A5=[c5 0 -s5 0;s5 0 c5 0;0 -1 0 0;0 0 0 1];
31 | A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1];%new add
32 | Rm{1}=[A1(1:3,1:3)];
33 | Rm{2}=[A2(1:3,1:3)];
34 | Rm{3}=[A3(1:3,1:3)];
35 | Rm{4}=[A4(1:3,1:3)];
36 | Rm{5}=[A5(1:3,1:3)];
37 | Rm{6}=[A6(1:3,1:3)];
38 | pstarm=[0,0,0;a2,0,0;a3,d3*sin(alpha_r(3)),d3*cos(alpha_r(3));...
39 | 0,d4*sin(alpha_r(4)),d4*cos(alpha_r(4));0,0,0;...
40 | 0,d6*sin(alpha_r(6)),d6*cos(alpha_r(6))]';%new add
41 |
42 | %calculate inertial matrix H
43 | for ji=1:6,
44 | grav=[0;0;0];qd=zeros(1,6);
45 | qdd=zeros(1,6);qdd(ji)=1;
46 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
47 | for j=1:6,%the forward recursion
48 | R=Rm{j}';
49 | pstar=pstarm(:,j);
50 | r=linkR(j,:)';
51 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
52 | w=R*(w+z0*qd(j));
53 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
54 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
55 | F=linkM(j)*vhat;
56 | LinkIj=diag(linkI(j,1:3));
57 | N=LinkIj*wd+cross(w,LinkIj*w);
58 | if j==1
59 | Fm=F;
60 | Nm=N;
61 | else
62 | Fm=[Fm F];
63 | Nm=[Nm N];
64 | end;
65 | end
66 | f=fext(1:3);nn=fext(4:6);
67 | for j=6:-1:1,%the backward recursion
68 | pstar=pstarm(:,j);
69 | if j==6,
70 | R=eye(3,3);
71 | else
72 | R=Rm{j+1};
73 | end
74 | r=linkR(j,:)';
75 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
76 | f=R*f+Fm(:,j);
77 | R=Rm{j};
78 | inerH(ji,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
79 | end
80 | end
81 |
--------------------------------------------------------------------------------
/code/EXP2/mvn_ZZJLJrebi1.m:
--------------------------------------------------------------------------------
1 | %VERSION of May 13, 2010,by phD Zhijun Zhang and MS Jun Lee
2 | clc;
3 | clear all;
4 | format long;
5 | global alpha r q0 T lambda sigma aa alpha_i a_i d_i D sa ca s2a c2a pen_long k tt errorr;
6 |
7 | %circle data
8 | T=8.0; %????????????
9 | r=0.8;
10 | alpha=pi/6;
11 | sigma=pi/1;
12 | aa=0.035;
13 | tt=0; errorr=0;
14 | %%%p560 kenimatic data: with a long tool
15 | %a2=0.0;a3=0.0;d3=0.0;d4=0.0;d6=0.0; %P560????????
16 |
17 | %Jaco??????????????
18 | %????????
19 | % D = [0.2755, 0.4100, 0.2073, 0.07433, 0.07433, 0.1687, 0.0098]; %D1~D6;E2
20 | %????????
21 | D = [0.2755, 0.4100, 0.2073, 0.07433, 0.07433, 0.13, 0.0098]; %D1~D6;E2
22 | pen_long=0.075; %????
23 | aaa = 11.0*pi/72;
24 | ca = cos(aaa);
25 | sa = sin(aaa);
26 | c2a = cos(2*aaa);
27 | s2a = sin(2*aaa);
28 | d4b = D(3)+sa/s2a*D(4);
29 | d5b = sa/s2a*D(4)+sa/s2a*D(5);
30 | d6b = sa/s2a*D(5)+D(6);
31 | alpha_i = [pi/2, pi, pi/2, 2*aaa, 2*aaa, pi];
32 | a_i = [0, D(2), 0, 0, 0, 0];
33 | d_i = [D(1), 0, -D(7), -d4b, -d5b, -d6b];
34 |
35 | %selection of an initial state q0
36 | qa=[0;0;0;0;0;0]; %????????????????
37 | qb=[0;-pi/4;0;pi/2;-pi/4;0];
38 | qc=[0;+pi/4;0;pi/2;-pi/4;0];
39 | qd=[0;-pi/4;0;2*pi/3;-pi/4;0];
40 | qe=[0;+pi/4;0;2*pi/3;-pi/4;0];
41 | qf=[0;-pi/4;0;pi/2;-pi/4;0];
42 | qg=[0;-pi/4;0;pi/2;-pi/4;0];
43 |
44 | qh=[1.675;1.343;-3.716;4.187;-1.71;1.308]; %jaco??????????????
45 | qi=[1.675;2.843;-3.216;4.187;-1.71;-2.29]; %??????????????
46 | qj=[1.670;2.845;-3.218;4.182;-1.715;-2.655]; %????????????????????
47 | q0=qj;%an initial state q0
48 |
49 | global mu_p mu_v qP qM qDp qDm qDDp qDDm myInf n m Pinfty Minfty gamma nu beta Time;
50 | n=6; %??????????????6??????
51 | m=3; %??????????????????????????????3??????????
52 | nu=2500;
53 | gamma=500; %????????????DNN????alpha 50
54 | beta=4;%may need adjust
55 | lambda=4;
56 | myInf=1e10;
57 | mu_p=20;
58 | qP=[10000;223;71;10000;10000;10000]; %????????????????
59 | qM=[-10000;-137;-289;-10000;-10000;-10000]; %????????????????
60 | rad=180/pi; %????????
61 | qP=qP/rad; %????????
62 | qM=qM/rad; %????????
63 | qDp=ones(n,1)*1.5;qDm=-qDp;%not real, just a test %??????????
64 | % qDDp=ones(6,1)*4;qDDm=-qDDp;%not real, just a test
65 | Pinfty=myInf*ones(m,1);
66 | Minfty=-Pinfty;
67 |
68 | Time=0; %??????????????????
69 |
70 | dq0=zeros(n,1); %???????????????????????????? Theta dot
71 | u0=[dq0;zeros(m,1)]; %????????????????????u
72 | % u0=(rand(n+m,1)-0.5)*10
73 | init=[q0;u0];%acutally, abitrary initial y0
74 | options=odeset('Mass', @mvn_ZNN_matrixALL, 'MaxStep',0.05,'RelTol',1e-6,'AbsTol',1e-8*ones(n+n+m,1)); %????ODE????????????????????????????????????????????????
75 | tic; %????????
76 | N=1;
77 | [t,y]=ode15s(@mvn_ZZJLJrebi1_net,[0,N*T],init,options);%ode15s much better than ode45 %ODE????
78 | cpu_time=toc %????????
79 | size(t)
80 | size(y)
81 | %save cpu_time to "txt" file
82 | fid=fopen('mvn_cost_time.txt','a'); %????????????????
83 | fprintf(fid,'Computing cost time: %g\n',cpu_time);
84 | fclose(fid); %????????
85 |
86 | %??????????????
87 | save mvn_SYSdata u0 aa beta sigma r alpha lambda alpha_i a_i d_i D sa ca s2a c2a pen_long q0 T mu_p mu_v qP qM qDp qDm qDDp qDDm m Pinfty Minfty gamma n myInf Time k;
88 | save mvn_INITdata t y;
89 |
--------------------------------------------------------------------------------
/code/EXP1/mvn_ZZJLJrebi1.m:
--------------------------------------------------------------------------------
1 | %VERSION of May 13, 2010,by phD Zhijun Zhang and MS Jun Lee
2 | clc;
3 |
4 | format long;
5 | global alpha r q0 T lambda sigma aa alpha_i a_i d_i D sa ca s2a c2a pen_long k tt errorr K;
6 |
7 | %circle data
8 | T=8.0; %????????????
9 | r=0.8;
10 | alpha=pi/6;
11 | sigma=pi/1;
12 | aa=0.08;
13 | tt=0; errorr=0;
14 |
15 | %%%p560 kenimatic data: with a long tool
16 | %a2=0.0;a3=0.0;d3=0.0;d4=0.0;d6=0.0; %P560????????
17 |
18 | %Jaco??????????????
19 | %????????
20 | % D = [0.2755, 0.4100, 0.2073, 0.07433, 0.07433, 0.1687, 0.0098]; %D1~D6;E2
21 | %????????
22 | % D = [0.2755, 0.4100, 0.2073, 0.07433, 0.07433, 0.13, 0.0098]; %D1~D6;E2
23 | D= [0.2755, 0.4100, 0.2073, 0.0741, 0.0741, 0.1425, 0.0098];
24 | pen_long=0.075; %????
25 | aaa = 12.0*pi/72;
26 | ca = cos(aaa);
27 | sa = sin(aaa);
28 | c2a = cos(2*aaa);
29 | s2a = sin(2*aaa);
30 | d4b = D(3)+sa/s2a*D(4);
31 | d5b = sa/s2a*D(4)+sa/s2a*D(5);
32 | d6b = sa/s2a*D(5)+D(6);
33 | alpha_i = [pi/2, pi, pi/2, 2*aaa, 2*aaa, pi];
34 | a_i = [0, D(2), 0, 0, 0, 0];
35 | d_i = [D(1), 0, -D(7), -d4b, -d5b, -d6b];
36 |
37 | %selection of an initial state q0
38 | qa=[0;0;0;0;0;0]; %????????????????
39 | qb=[0;-pi/4;0;pi/2;-pi/4;0];
40 | qc=[0;+pi/4;0;pi/2;-pi/4;0];
41 | qd=[0;-pi/4;0;2*pi/3;-pi/4;0];
42 | qe=[0;+pi/4;0;2*pi/3;-pi/4;0];
43 | qf=[0;-pi/4;0;pi/2;-pi/4;0];
44 | qg=[0;-pi/4;0;pi/2;-pi/4;0];
45 |
46 | qh=[1.675;1.343;-3.716;4.187;-1.71;1.308]; %jaco??????????????
47 | qi=[1.675;2.843;-3.216;4.187;-1.71;-2.29]; %??????????????
48 | qj=[1.675;2.843;-3.216;4.187;-1.71;-2.65]; %????????????????????
49 | q0=qj;%an initial state q0
50 |
51 | global mu_p mu_v qP qM qDp qDm qDDp qDDm myInf n m Pinfty Minfty gamma beta Time nu;
52 | n=6; %??????????????6??????
53 | m=3;%??????????????????????????????3??????????
54 | nu=2500;
55 | gamma=500;
56 | K=1*eye(m,m);
57 | beta=4;%may need adjust
58 | lambda=4;
59 | myInf=1e10;
60 | mu_p=20;
61 | qP=[10000;223;71;10000;10000;10000]; %????????????????
62 | qM=[-10000;-137;-289;-10000;-10000;-10000]; %????????????????
63 | rad=180/pi; %????????
64 | qP=qP/rad; %????????
65 | qM=qM/rad; %????????
66 | qDp=ones(n,1)*1.5;qDm=-qDp;%not real, just a test %??????????
67 | % qDDp=ones(6,1)*4;qDDm=-qDDp;%not real, just a test
68 | Pinfty=myInf*ones(m,1);
69 | Minfty=-Pinfty;
70 |
71 | Time=0; %??????????????????
72 |
73 | dq0=zeros(n,1); %???????????????????????????? Theta dot
74 | u0=[dq0;zeros(m,1)]; %????????????????????u
75 | % u0=(rand(n+m,1)-0.5)*10
76 | init=[q0;u0];%acutally, abitrary initial y0
77 | options=odeset('Mass', @mvn_ZNN_matrixALL, 'MaxStep',0.05,'RelTol',1e-6,'AbsTol',1e-8*ones(n+n+m,1)); %????ODE????????????????????????????????????????????????
78 | tic; %????????
79 | N=1;
80 | [t,y]=ode15s(@mvn_ZZJLJrebi1_net,[0,N*T],init,options);%ode15s much better than ode45 %ODE????
81 | cpu_time=toc %????????
82 | size(t)
83 | size(y)
84 | %save cpu_time to "txt" file
85 | fid=fopen('mvn_cost_time.txt','a'); %????????????????
86 | fprintf(fid,'Computing cost time: %g\n',cpu_time);
87 | fclose(fid); %????????
88 |
89 | %??????????????
90 | save mvn_SYSdata u0 aa beta sigma r alpha lambda alpha_i a_i d_i D sa ca s2a c2a pen_long q0 T mu_p mu_v qP qM qDp qDm qDDp qDDm m Pinfty Minfty gamma n myInf Time k K;
91 | save mvn_INITdata t y;
92 |
--------------------------------------------------------------------------------
/code/EXP1/mvn_ZZJLJrebi1_net.m:
--------------------------------------------------------------------------------
1 | function doty=mvn_ZZJLJrebi1_net(t,y)
2 |
3 | global aa alpha_i a_i d_i D sa ca s2a c2a pen_long q0 T m gamma nu n Time znn_W k tt errorr K;
4 |
5 | persistent ix0 iy0 iz0 ix1 iy1 iz1 t_last;
6 |
7 |
8 | q=y(1:n); %??????????
9 | dq=y(n+1:n+n); %????Theta dot??????????????Theta dot
10 | yy=y(n+1:n+n+m);
11 |
12 |
13 | if t==0
14 | [x,y,z]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q0); %????????????
15 |
16 | ix0=x(7);iy0=y(7);iz0=z(7);
17 | end
18 |
19 | t_time=t-Time*T; %??????????????????????????????????????????????????????????????????????????????????
20 |
21 | if t_time>T;
22 | Time=Time+1;
23 | t_time=t_time-T;
24 | end
25 |
26 |
27 |
28 | num=4; %4??????4+1=5????
29 | phi_sin=2*pi*sin(0.5*pi*t_time/T);
30 | phi=phi_sin*sin(0.5*pi*t_time/T);
31 | phiDot=phi_sin*pi*cos(0.5*pi*t_time/T)/T;
32 |
33 | %??????????????1????????r
34 | rx=aa*(cos(phi)+cos(num*phi)/num-1-1/num); %????????????????????
35 | ry=aa*(sin(phi)-sin(num*phi)/num);
36 | rz=0;
37 | %??????????????2????????r_dot
38 | drx=-aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T + (2*pi^2*cos((pi*t_time)/(2*T))*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T)))/T);
39 | dry=aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T - (2*pi^2*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T)))/T);
40 | drz=0;
41 | %??????????????????r_dotdot
42 | ddrx=-aa*((pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 + (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2)/T^2);
43 | ddry=aa*((pi^3*cos((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2)/T^2 + (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 - (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2);
44 | ddrz=0;
45 |
46 | dr=[drx;dry;drz]; %????????????????r_dot
47 | ddr=[ddrx;ddry;ddrz];
48 | znn_d=dr;
49 | [J,DJ]=ZYNjdj(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,dq);
50 |
51 |
52 | znn_W=eye(n,n);
53 | znn_Q=[znn_W, J'; J, zeros(m,m)];
54 | znn_dotQ=[zeros(n,n), DJ'; DJ, zeros(m,m)];
55 |
56 |
57 | % C=[J,zeros(m,1)];
58 | k=6; %????????????c????????gamma????????0????????????????????????
59 | % zz=k*(q-q0);
60 | znn_q=k*(q-q0); %????????????c
61 | znn_u=[-znn_q; znn_d];
62 | znn_dotu=[zeros(n,1); ddr];
63 |
64 |
65 | %%%%%%%%%%%%%%%%%
66 | %VP-CDNN??????????????
67 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFMlinear(znn_Q*yy-znn_u)+znn_dotu;
68 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFpower(znn_Q*yy-znn_u)+znn_dotu;
69 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFsigmoid(znn_Q*yy-znn_u)+znn_dotu;
70 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFsinh(znn_Q*yy-znn_u)+znn_dotu;
71 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu;
72 |
73 | %%%%%%%%%%%%%%%%%
74 | %FP-CDNN??????????????
75 | % dotyy=-znn_dotQ*yy-gamma*AFMlinear(znn_Q*yy-znn_u)+znn_dotu;
76 | % dotyy=-znn_dotQ*yy-gamma*AFpower(znn_Q*yy-znn_u)+znn_dotu;
77 | % dotyy=-znn_dotQ*yy-gamma*AFsigmoid(znn_Q*yy-znn_u)+znn_dotu;
78 | % dotyy=-znn_dotQ*yy-gamma*AFsinh(znn_Q*yy-znn_u)+znn_dotu;
79 | % dotyy=-znn_dotQ*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu;
80 |
81 | %%%%%%%%%%%%%%%%%????
82 | delt_D=1.0*[sin(t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(3*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
83 | cos(2*t/T*pi) cos(2*t/T*pi) -cos(4*t/T*pi) sin(t/T*pi) -sin(2*t/T*pi) -cos(3*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
84 | sin(3*t/T*pi) -sin(4*t/T*pi) cos(5*t/T*pi) -cos(3*t/T*pi) cos(4*t/T*pi) -sin((5*t/T*pi)) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
85 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
86 | sin(t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
87 | -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
88 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(2*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
89 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
90 | -sin(2*t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(2*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi)];
91 | delt_s=1.5*[2*sin(t/T*pi);4*cos(2*t/T*pi);-5*sin(3*t/T*pi); cos(3*t/T*pi); -sin(3*t/T*pi); 3*cos(t/T*pi);-sin(t/T*pi);-sin(2*t/T*pi);cos(t/T*pi)];
92 |
93 | % delt_D=0.01*rand(n+m,n+m);
94 | % delt_s=t*ones(9,1);
95 | % delt_s=[1;2;3;4;5;6;7;8;9];
96 | % delt_s=0.1*rand(n+m,1);
97 |
98 |
99 |
100 | %IE-RNN
101 | dotyy=-(znn_dotQ)*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu-nu*(znn_Q*yy-znn_u)*t+delt_s;
102 |
103 | %Z-RNN
104 | %dotyy=-(znn_dotQ)*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu+delt_s;
105 |
106 |
107 | t
108 |
109 | doty=[dq;dotyy];
110 |
--------------------------------------------------------------------------------
/code/EXP1/ZYNtaugcH_cbh.m:
--------------------------------------------------------------------------------
1 | %old message:
2 | %1.tau_gravity=ZYNrne(q,zeros(1,6),zeros(1,6),[0;0;9.81])
3 | %2.tau_coriolis=ZYNrne(q,dq,zeros(1,6),[0;0;0])
4 | %3.inertia=[ZYNrne(q,zeros(1,6),[1,0,0,0,0,0],[0;0;0]);...
5 | % ZYNrne(q,zeros(1,6),[0,1,0,0,0,0],[0;0;0]);...
6 | % ZYNrne(q,zeros(1,6),[0,0,1,0,0,0],[0;0;0]);...
7 | % ZYNrne(q,zeros(1,6),[0,0,0,1,0,0],[0;0;0]);...
8 | % ZYNrne(q,zeros(1,6),[0,0,0,0,1,0],[0;0;0]);...
9 | % ZYNrne(q,zeros(1,6),[0,0,0,0,0,1],[0;0;0])];
10 | %new usage below (VERSION of April 16th 2001):
11 | %[taug,tauc,inerH]=ZYNtaugcI(a2,a3,d3,d4,d6,q,dq)
12 | function inerH=ZYNtaugcH_cbh(a2,a3,d3,d4,d6,q,qd)%new add
13 | %common data below
14 | keepqd=qd;
15 | z0=[0;0;1];fext=zeros(6,1);%force/moments on end of arm
16 | alpha_r=[1.5707963267949,0,-1.5707963267949,1.5707963267949,-1.5707963267949,0];
17 | linkR=[zeros(1,3);-0.3638,0.006,0.2275;-0.0203,-0.0141,0.070;0,0.019,0;zeros(1,3);0,0,0.032];
18 | linkM=[0;17.4;4.8;0.82;0.34;0.09];
19 | linkI=[0,0.35,0,0,0,0;0.13,0.524,0.539,0,0,0;0.066,0.086,0.0125,0,0,0;...
20 | 1.8e-3,1.3e-3,1.8e-3,0,0,0;0.3e-3,0.4e-3,0.3e-3,0,0,0;0.15e-3,0.15e-3,0.04e-3,0,0,0];
21 | linkJm=[200e-6;200e-6;200e-6;33e-6;33e-6;33e-6];
22 | linkG=[-62.6111;107.815;-53.7063;76.0364;71.923;76.686];
23 | mys=sin(q);
24 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
25 | myc=cos(q);
26 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
27 | A1=[c1 0 s1 0;s1 0 -c1 0;0 1 0 0;0 0 0 1];
28 | A2=[c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 0;0 0 0 1];
29 | A3=[c3 0 -s3 c3*a3;s3 0 c3 s3*a3;0 -1 0 d3;0 0 0 1];
30 | A4=[c4 0 s4 0;s4 0 -c4 0;0 1 0 d4;0 0 0 1];
31 | A5=[c5 0 -s5 0;s5 0 c5 0;0 -1 0 0;0 0 0 1];
32 | A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1];%new add
33 | Rm{1}=[A1(1:3,1:3)];
34 | Rm{2}=[A2(1:3,1:3)];
35 | Rm{3}=[A3(1:3,1:3)];
36 | Rm{4}=[A4(1:3,1:3)];
37 | Rm{5}=[A5(1:3,1:3)];
38 | Rm{6}=[A6(1:3,1:3)];
39 | pstarm=[0,0,0;a2,0,0;a3,d3*sin(alpha_r(3)),d3*cos(alpha_r(3));...
40 | 0,d4*sin(alpha_r(4)),d4*cos(alpha_r(4));0,0,0;...
41 | 0,d6*sin(alpha_r(6)),d6*cos(alpha_r(6))]';%new add
42 |
43 | %calculate inertial matrix H
44 | for ji=1:6,
45 | grav=[0;0;0];qd=zeros(1,6);
46 | qdd=zeros(1,6);qdd(ji)=1;
47 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
48 | for j=1:6,%the forward recursion
49 | R=Rm{j}';
50 | pstar=pstarm(:,j);
51 | r=linkR(j,:)';
52 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
53 | w=R*(w+z0*qd(j));
54 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
55 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
56 | F=linkM(j)*vhat;
57 | LinkIj=diag(linkI(j,1:3));
58 | N=LinkIj*wd+cross(w,LinkIj*w);
59 | if j==1
60 | Fm=F;
61 | Nm=N;
62 | else
63 | Fm=[Fm F];
64 | Nm=[Nm N];
65 | end;
66 | end
67 | f=fext(1:3);nn=fext(4:6);
68 | for j=6:-1:1,%the backward recursion
69 | pstar=pstarm(:,j);
70 | if j==6,
71 | R=eye(3,3);
72 | else
73 | R=Rm{j+1};
74 | end
75 | r=linkR(j,:)';
76 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
77 | f=R*f+Fm(:,j);
78 | R=Rm{j};
79 | inerH(ji,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
80 | end
81 | end
82 |
83 | %%%%%%edited by cbh%%%%%the following codes is not need for mke%%%%%%
84 | if(0)
85 | %calculate taug
86 | grav=[0;0;9.81];qd=zeros(1,6);qdd=zeros(1,6);
87 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
88 | for j=1:6,%the forward recursion
89 | R=Rm{j}';
90 | pstar=pstarm(:,j);
91 | r=linkR(j,:)';
92 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
93 | w=R*(w+z0*qd(j));
94 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
95 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
96 | F=linkM(j)*vhat;
97 | LinkIj=diag(linkI(j,1:3));
98 | N=LinkIj*wd+cross(w,LinkIj*w);
99 | if j==1
100 | Fm=F;
101 | Nm=N;
102 | else
103 | Fm=[Fm F];
104 | Nm=[Nm N];
105 | end;
106 | end
107 | f=fext(1:3);nn=fext(4:6);%the backward recursion
108 | for j=6:-1:1,
109 | pstar=pstarm(:,j);
110 | if j==6,
111 | R=eye(3,3);
112 | else
113 | R=Rm{j+1};
114 | end
115 | r=linkR(j,:)';
116 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
117 | f=R*f+Fm(:,j);
118 | R=Rm{j};
119 | taug(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
120 | end
121 |
122 | %calculate tauc
123 | grav=[0;0;0];qd=keepqd;qdd=zeros(1,6);
124 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
125 | for j=1:6,%the forward recursion
126 | R=Rm{j}';
127 | pstar=pstarm(:,j);
128 | r=linkR(j,:)';
129 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
130 | w=R*(w+z0*qd(j));
131 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
132 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
133 | F=linkM(j)*vhat;
134 | LinkIj=diag(linkI(j,1:3));
135 | N=LinkIj*wd+cross(w,LinkIj*w);
136 | if j==1
137 | Fm=F;
138 | Nm=N;
139 | else
140 | Fm=[Fm F];
141 | Nm=[Nm N];
142 | end;
143 | end
144 | f=fext(1:3);nn=fext(4:6);%the backward recursion
145 | for j=6:-1:1,
146 | pstar=pstarm(:,j);
147 | if j==6,
148 | R=eye(3,3);
149 | else
150 | R=Rm{j+1};
151 | end
152 | r=linkR(j,:)';
153 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
154 | f=R*f+Fm(:,j);
155 | R=Rm{j};
156 | tauc(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
157 | end
158 | end
159 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
--------------------------------------------------------------------------------
/code/EXP2/ZYNtaugcH_cbh.m:
--------------------------------------------------------------------------------
1 | %old message:
2 | %1.tau_gravity=ZYNrne(q,zeros(1,6),zeros(1,6),[0;0;9.81])
3 | %2.tau_coriolis=ZYNrne(q,dq,zeros(1,6),[0;0;0])
4 | %3.inertia=[ZYNrne(q,zeros(1,6),[1,0,0,0,0,0],[0;0;0]);...
5 | % ZYNrne(q,zeros(1,6),[0,1,0,0,0,0],[0;0;0]);...
6 | % ZYNrne(q,zeros(1,6),[0,0,1,0,0,0],[0;0;0]);...
7 | % ZYNrne(q,zeros(1,6),[0,0,0,1,0,0],[0;0;0]);...
8 | % ZYNrne(q,zeros(1,6),[0,0,0,0,1,0],[0;0;0]);...
9 | % ZYNrne(q,zeros(1,6),[0,0,0,0,0,1],[0;0;0])];
10 | %new usage below (VERSION of April 16th 2001):
11 | %[taug,tauc,inerH]=ZYNtaugcI(a2,a3,d3,d4,d6,q,dq)
12 | function inerH=ZYNtaugcH_cbh(a2,a3,d3,d4,d6,q,qd)%new add
13 | %common data below
14 | keepqd=qd;
15 | z0=[0;0;1];fext=zeros(6,1);%force/moments on end of arm
16 | alpha_r=[1.5707963267949,0,-1.5707963267949,1.5707963267949,-1.5707963267949,0];
17 | linkR=[zeros(1,3);-0.3638,0.006,0.2275;-0.0203,-0.0141,0.070;0,0.019,0;zeros(1,3);0,0,0.032];
18 | linkM=[0;17.4;4.8;0.82;0.34;0.09];
19 | linkI=[0,0.35,0,0,0,0;0.13,0.524,0.539,0,0,0;0.066,0.086,0.0125,0,0,0;...
20 | 1.8e-3,1.3e-3,1.8e-3,0,0,0;0.3e-3,0.4e-3,0.3e-3,0,0,0;0.15e-3,0.15e-3,0.04e-3,0,0,0];
21 | linkJm=[200e-6;200e-6;200e-6;33e-6;33e-6;33e-6];
22 | linkG=[-62.6111;107.815;-53.7063;76.0364;71.923;76.686];
23 | mys=sin(q);
24 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
25 | myc=cos(q);
26 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
27 | A1=[c1 0 s1 0;s1 0 -c1 0;0 1 0 0;0 0 0 1];
28 | A2=[c2 -s2 0 a2*c2;s2 c2 0 a2*s2;0 0 1 0;0 0 0 1];
29 | A3=[c3 0 -s3 c3*a3;s3 0 c3 s3*a3;0 -1 0 d3;0 0 0 1];
30 | A4=[c4 0 s4 0;s4 0 -c4 0;0 1 0 d4;0 0 0 1];
31 | A5=[c5 0 -s5 0;s5 0 c5 0;0 -1 0 0;0 0 0 1];
32 | A6=[c6 -s6 0 0;s6 c6 0 0;0 0 1 d6;0 0 0 1];%new add
33 | Rm{1}=[A1(1:3,1:3)];
34 | Rm{2}=[A2(1:3,1:3)];
35 | Rm{3}=[A3(1:3,1:3)];
36 | Rm{4}=[A4(1:3,1:3)];
37 | Rm{5}=[A5(1:3,1:3)];
38 | Rm{6}=[A6(1:3,1:3)];
39 | pstarm=[0,0,0;a2,0,0;a3,d3*sin(alpha_r(3)),d3*cos(alpha_r(3));...
40 | 0,d4*sin(alpha_r(4)),d4*cos(alpha_r(4));0,0,0;...
41 | 0,d6*sin(alpha_r(6)),d6*cos(alpha_r(6))]';%new add
42 |
43 | %calculate inertial matrix H
44 | for ji=1:6,
45 | grav=[0;0;0];qd=zeros(1,6);
46 | qdd=zeros(1,6);qdd(ji)=1;
47 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
48 | for j=1:6,%the forward recursion
49 | R=Rm{j}';
50 | pstar=pstarm(:,j);
51 | r=linkR(j,:)';
52 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
53 | w=R*(w+z0*qd(j));
54 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
55 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
56 | F=linkM(j)*vhat;
57 | LinkIj=diag(linkI(j,1:3));
58 | N=LinkIj*wd+cross(w,LinkIj*w);
59 | if j==1
60 | Fm=F;
61 | Nm=N;
62 | else
63 | Fm=[Fm F];
64 | Nm=[Nm N];
65 | end;
66 | end
67 | f=fext(1:3);nn=fext(4:6);
68 | for j=6:-1:1,%the backward recursion
69 | pstar=pstarm(:,j);
70 | if j==6,
71 | R=eye(3,3);
72 | else
73 | R=Rm{j+1};
74 | end
75 | r=linkR(j,:)';
76 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
77 | f=R*f+Fm(:,j);
78 | R=Rm{j};
79 | inerH(ji,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
80 | end
81 | end
82 |
83 | %%%%%%edited by cbh%%%%%the following codes is not need for mke%%%%%%
84 | if(0)
85 | %calculate taug
86 | grav=[0;0;9.81];qd=zeros(1,6);qdd=zeros(1,6);
87 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
88 | for j=1:6,%the forward recursion
89 | R=Rm{j}';
90 | pstar=pstarm(:,j);
91 | r=linkR(j,:)';
92 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
93 | w=R*(w+z0*qd(j));
94 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
95 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
96 | F=linkM(j)*vhat;
97 | LinkIj=diag(linkI(j,1:3));
98 | N=LinkIj*wd+cross(w,LinkIj*w);
99 | if j==1
100 | Fm=F;
101 | Nm=N;
102 | else
103 | Fm=[Fm F];
104 | Nm=[Nm N];
105 | end;
106 | end
107 | f=fext(1:3);nn=fext(4:6);%the backward recursion
108 | for j=6:-1:1,
109 | pstar=pstarm(:,j);
110 | if j==6,
111 | R=eye(3,3);
112 | else
113 | R=Rm{j+1};
114 | end
115 | r=linkR(j,:)';
116 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
117 | f=R*f+Fm(:,j);
118 | R=Rm{j};
119 | taug(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
120 | end
121 |
122 | %calculate tauc
123 | grav=[0;0;0];qd=keepqd;qdd=zeros(1,6);
124 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
125 | for j=1:6,%the forward recursion
126 | R=Rm{j}';
127 | pstar=pstarm(:,j);
128 | r=linkR(j,:)';
129 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
130 | w=R*(w+z0*qd(j));
131 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
132 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
133 | F=linkM(j)*vhat;
134 | LinkIj=diag(linkI(j,1:3));
135 | N=LinkIj*wd+cross(w,LinkIj*w);
136 | if j==1
137 | Fm=F;
138 | Nm=N;
139 | else
140 | Fm=[Fm F];
141 | Nm=[Nm N];
142 | end;
143 | end
144 | f=fext(1:3);nn=fext(4:6);%the backward recursion
145 | for j=6:-1:1,
146 | pstar=pstarm(:,j);
147 | if j==6,
148 | R=eye(3,3);
149 | else
150 | R=Rm{j+1};
151 | end
152 | r=linkR(j,:)';
153 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
154 | f=R*f+Fm(:,j);
155 | R=Rm{j};
156 | tauc(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
157 | end
158 | end
159 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
--------------------------------------------------------------------------------
/code/EXP1/ZYNtaugcI.m:
--------------------------------------------------------------------------------
1 | %old message:
2 | %1.tau_gravity=ZYNrne(q,zeros(1,6),zeros(1,6),[0;0;9.81])
3 | %2.tau_coriolis=ZYNrne(q,dq,zeros(1,6),[0;0;0])
4 | %3.inertia=[ZYNrne(q,zeros(1,6),[1,0,0,0,0,0],[0;0;0]);...
5 | % ZYNrne(q,zeros(1,6),[0,1,0,0,0,0],[0;0;0]);...
6 | % ZYNrne(q,zeros(1,6),[0,0,1,0,0,0],[0;0;0]);...
7 | % ZYNrne(q,zeros(1,6),[0,0,0,1,0,0],[0;0;0]);...
8 | % ZYNrne(q,zeros(1,6),[0,0,0,0,1,0],[0;0;0]);...
9 | % ZYNrne(q,zeros(1,6),[0,0,0,0,0,1],[0;0;0])];
10 | %new usage below (VERSION of April 16th 2001):
11 | %[taug,tauc,inerH]=ZYNtaugcI(a2,a3,d3,d4,d6,q,dq)
12 | function [taug,tauc,inerH]=ZYNtaugcI(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,qd)%new add
13 | %common data below
14 | keepqd=qd;
15 | z0=[0;0;1];fext=zeros(6,1);%force/moments on end of arm
16 | alpha_r=[1.5707963267949,0,-1.5707963267949,1.5707963267949,-1.5707963267949,0];
17 | linkR=[zeros(1,3);-0.3638,0.006,0.2275;-0.0203,-0.0141,0.070;0,0.019,0;zeros(1,3);0,0,0.032];
18 | linkM=[0;17.4;4.8;0.82;0.34;0.09];
19 | linkI=[0,0.35,0,0,0,0;0.13,0.524,0.539,0,0,0;0.066,0.086,0.0125,0,0,0;...
20 | 1.8e-3,1.3e-3,1.8e-3,0,0,0;0.3e-3,0.4e-3,0.3e-3,0,0,0;0.15e-3,0.15e-3,0.04e-3,0,0,0];
21 | linkJm=[200e-6;200e-6;200e-6;33e-6;33e-6;33e-6];
22 | linkG=[-62.6111;107.815;-53.7063;76.0364;71.923;76.686];
23 | mys=sin(q);
24 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
25 | myc=cos(q);
26 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
27 |
28 | alpha_4 = alpha_i(4);alpha_5 = alpha_i(5);
29 | d_1 = d_i(1);d_2 = d_i(2);d_3 = d_i(3);d_4 = d_i(4);
30 | d_5 = d_i(5);d_6 = d_i(6);
31 | % sp = sin(phi);
32 | % cp = cos(phi);
33 | sa4 = sin(alpha_4);ca4 = cos(alpha_4);
34 | sa5 = sin(alpha_5);ca5 = cos(alpha_5);
35 |
36 | sp1=sin(0.02);cp1=cos(0.02);
37 | sp2=sin(0.05);cp2=cos(0.05);
38 |
39 | AW1 = [1, 0, 0, 0;...
40 | 0, cp1, sp1, 0;...
41 | 0, -sp1, cp1, 0;...
42 | 0, 0, 0, 1];
43 |
44 | AW2 = [cp2, 0, -sp2, 0;...
45 | 0, 1, 0, 0;...
46 | sp2, 0, cp2, 0;...
47 | 0, 0, 0, 1];
48 |
49 | AW1=AW1*AW2;
50 |
51 | A1 = [c1, 0, s1, 0;s1, 0, -c1, 0;0, 1, 0, d_1;0, 0, 0, 1];
52 | A2 = [c2, s2, 0, D(2)*c2;s2, -c2, 0, D(2)*s2;0, 0, -1, 0;0, 0, 0, 1];
53 | A3 = [c3, 0, s3, 0;s3, 0, -c3, 0;0, 1, 0, d_3;0, 0, 0, 1];
54 | A4 = [c4, -ca4*s4, sa4*s4, 0;s4, ca4*c4, -sa4*c4, 0;0, sa4, ca4, d_4;0, 0, 0, 1];
55 | A5 = [c5, -ca5*s5, sa5*s5, 0;s5, ca5*c5, -sa5*c5, 0;0, sa5, ca5, d_5;0, 0, 0, 1];
56 | A6 = [c6, s6, 0, 0;s6, -c6, 0, 0;0, 0, -1, d_6;0, 0, 0, 1];%new add
57 | Rm{1}=[A1(1:3,1:3)];
58 | Rm{2}=[A2(1:3,1:3)];
59 | Rm{3}=[A3(1:3,1:3)];
60 | Rm{4}=[A4(1:3,1:3)];
61 | Rm{5}=[A5(1:3,1:3)];
62 | Rm{6}=[A6(1:3,1:3)];
63 | pstarm=[0,0,0;0,0,0;0,0*sin(alpha_r(3)),0*cos(alpha_r(3));...
64 | 0,0*sin(alpha_r(4)),0*cos(alpha_r(4));0,0,0;...
65 | 0,0*sin(alpha_r(6)),0*cos(alpha_r(6))]';%new add
66 |
67 | %calculate inertial matrix H
68 | for ji=1:6,
69 | grav=[0;0;0];qd=zeros(1,6);
70 | qdd=zeros(1,6);qdd(ji)=1;
71 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
72 | for j=1:6,%the forward recursion
73 | R=Rm{j}';
74 | pstar=pstarm(:,j);
75 | r=linkR(j,:)';
76 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
77 | w=R*(w+z0*qd(j));
78 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
79 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
80 | F=linkM(j)*vhat;
81 | LinkIj=diag(linkI(j,1:3));
82 | N=LinkIj*wd+cross(w,LinkIj*w);
83 | if j==1
84 | Fm=F;
85 | Nm=N;
86 | else
87 | Fm=[Fm F];
88 | Nm=[Nm N];
89 | end;
90 | end
91 | f=fext(1:3);nn=fext(4:6);
92 | for j=6:-1:1,%the backward recursion
93 | pstar=pstarm(:,j);
94 | if j==6,
95 | R=eye(3,3);
96 | else
97 | R=Rm{j+1};
98 | end
99 | r=linkR(j,:)';
100 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
101 | f=R*f+Fm(:,j);
102 | R=Rm{j};
103 | inerH(ji,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
104 | end
105 | end
106 |
107 | %calculate taug
108 | grav=[0;0;9.81];qd=zeros(1,6);qdd=zeros(1,6);
109 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
110 | for j=1:6,%the forward recursion
111 | R=Rm{j}';
112 | pstar=pstarm(:,j);
113 | r=linkR(j,:)';
114 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
115 | w=R*(w+z0*qd(j));
116 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
117 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
118 | F=linkM(j)*vhat;
119 | LinkIj=diag(linkI(j,1:3));
120 | N=LinkIj*wd+cross(w,LinkIj*w);
121 | if j==1
122 | Fm=F;
123 | Nm=N;
124 | else
125 | Fm=[Fm F];
126 | Nm=[Nm N];
127 | end;
128 | end
129 | f=fext(1:3);nn=fext(4:6);%the backward recursion
130 | for j=6:-1:1,
131 | pstar=pstarm(:,j);
132 | if j==6,
133 | R=eye(3,3);
134 | else
135 | R=Rm{j+1};
136 | end
137 | r=linkR(j,:)';
138 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
139 | f=R*f+Fm(:,j);
140 | R=Rm{j};
141 | taug(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
142 | end
143 |
144 | %calculate tauc
145 | grav=[0;0;0];qd=keepqd;qdd=zeros(1,6);
146 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
147 | for j=1:6,%the forward recursion
148 | R=Rm{j}';
149 | pstar=pstarm(:,j);
150 | r=linkR(j,:)';
151 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
152 | w=R*(w+z0*qd(j));
153 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
154 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
155 | F=linkM(j)*vhat;
156 | LinkIj=diag(linkI(j,1:3));
157 | N=LinkIj*wd+cross(w,LinkIj*w);
158 | if j==1
159 | Fm=F;
160 | Nm=N;
161 | else
162 | Fm=[Fm F];
163 | Nm=[Nm N];
164 | end;
165 | end
166 | f=fext(1:3);nn=fext(4:6);%the backward recursion
167 | for j=6:-1:1,
168 | pstar=pstarm(:,j);
169 | if j==6,
170 | R=eye(3,3);
171 | else
172 | R=Rm{j+1};
173 | end
174 | r=linkR(j,:)';
175 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
176 | f=R*f+Fm(:,j);
177 | R=Rm{j};
178 | tauc(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
179 | end
--------------------------------------------------------------------------------
/code/EXP2/ZYNtaugcI.m:
--------------------------------------------------------------------------------
1 | %old message:
2 | %1.tau_gravity=ZYNrne(q,zeros(1,6),zeros(1,6),[0;0;9.81])
3 | %2.tau_coriolis=ZYNrne(q,dq,zeros(1,6),[0;0;0])
4 | %3.inertia=[ZYNrne(q,zeros(1,6),[1,0,0,0,0,0],[0;0;0]);...
5 | % ZYNrne(q,zeros(1,6),[0,1,0,0,0,0],[0;0;0]);...
6 | % ZYNrne(q,zeros(1,6),[0,0,1,0,0,0],[0;0;0]);...
7 | % ZYNrne(q,zeros(1,6),[0,0,0,1,0,0],[0;0;0]);...
8 | % ZYNrne(q,zeros(1,6),[0,0,0,0,1,0],[0;0;0]);...
9 | % ZYNrne(q,zeros(1,6),[0,0,0,0,0,1],[0;0;0])];
10 | %new usage below (VERSION of April 16th 2001):
11 | %[taug,tauc,inerH]=ZYNtaugcI(a2,a3,d3,d4,d6,q,dq)
12 | function [taug,tauc,inerH]=ZYNtaugcI(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,qd)%new add
13 | %common data below
14 | keepqd=qd;
15 | z0=[0;0;1];fext=zeros(6,1);%force/moments on end of arm
16 | alpha_r=[1.5707963267949,0,-1.5707963267949,1.5707963267949,-1.5707963267949,0];
17 | linkR=[zeros(1,3);-0.3638,0.006,0.2275;-0.0203,-0.0141,0.070;0,0.019,0;zeros(1,3);0,0,0.032];
18 | linkM=[0;17.4;4.8;0.82;0.34;0.09];
19 | linkI=[0,0.35,0,0,0,0;0.13,0.524,0.539,0,0,0;0.066,0.086,0.0125,0,0,0;...
20 | 1.8e-3,1.3e-3,1.8e-3,0,0,0;0.3e-3,0.4e-3,0.3e-3,0,0,0;0.15e-3,0.15e-3,0.04e-3,0,0,0];
21 | linkJm=[200e-6;200e-6;200e-6;33e-6;33e-6;33e-6];
22 | linkG=[-62.6111;107.815;-53.7063;76.0364;71.923;76.686];
23 | mys=sin(q);
24 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
25 | myc=cos(q);
26 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
27 |
28 | alpha_4 = alpha_i(4);alpha_5 = alpha_i(5);
29 | d_1 = d_i(1);d_2 = d_i(2);d_3 = d_i(3);d_4 = d_i(4);
30 | d_5 = d_i(5);d_6 = d_i(6);
31 | % sp = sin(phi);
32 | % cp = cos(phi);
33 | sa4 = sin(alpha_4);ca4 = cos(alpha_4);
34 | sa5 = sin(alpha_5);ca5 = cos(alpha_5);
35 |
36 | sp1=sin(0.02);cp1=cos(0.02);
37 | sp2=sin(0.05);cp2=cos(0.05);
38 |
39 | AW1 = [1, 0, 0, 0;...
40 | 0, cp1, sp1, 0;...
41 | 0, -sp1, cp1, 0;...
42 | 0, 0, 0, 1];
43 |
44 | AW2 = [cp2, 0, -sp2, 0;...
45 | 0, 1, 0, 0;...
46 | sp2, 0, cp2, 0;...
47 | 0, 0, 0, 1];
48 |
49 | AW1=AW1*AW2;
50 |
51 | A1 = [c1, 0, s1, 0;s1, 0, -c1, 0;0, 1, 0, d_1;0, 0, 0, 1];
52 | A2 = [c2, s2, 0, D(2)*c2;s2, -c2, 0, D(2)*s2;0, 0, -1, 0;0, 0, 0, 1];
53 | A3 = [c3, 0, s3, 0;s3, 0, -c3, 0;0, 1, 0, d_3;0, 0, 0, 1];
54 | A4 = [c4, -ca4*s4, sa4*s4, 0;s4, ca4*c4, -sa4*c4, 0;0, sa4, ca4, d_4;0, 0, 0, 1];
55 | A5 = [c5, -ca5*s5, sa5*s5, 0;s5, ca5*c5, -sa5*c5, 0;0, sa5, ca5, d_5;0, 0, 0, 1];
56 | A6 = [c6, s6, 0, 0;s6, -c6, 0, 0;0, 0, -1, d_6;0, 0, 0, 1];%new add
57 | Rm{1}=[A1(1:3,1:3)];
58 | Rm{2}=[A2(1:3,1:3)];
59 | Rm{3}=[A3(1:3,1:3)];
60 | Rm{4}=[A4(1:3,1:3)];
61 | Rm{5}=[A5(1:3,1:3)];
62 | Rm{6}=[A6(1:3,1:3)];
63 | pstarm=[0,0,0;0,0,0;0,0*sin(alpha_r(3)),0*cos(alpha_r(3));...
64 | 0,0*sin(alpha_r(4)),0*cos(alpha_r(4));0,0,0;...
65 | 0,0*sin(alpha_r(6)),0*cos(alpha_r(6))]';%new add
66 |
67 | %calculate inertial matrix H
68 | for ji=1:6,
69 | grav=[0;0;0];qd=zeros(1,6);
70 | qdd=zeros(1,6);qdd(ji)=1;
71 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
72 | for j=1:6,%the forward recursion
73 | R=Rm{j}';
74 | pstar=pstarm(:,j);
75 | r=linkR(j,:)';
76 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
77 | w=R*(w+z0*qd(j));
78 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
79 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
80 | F=linkM(j)*vhat;
81 | LinkIj=diag(linkI(j,1:3));
82 | N=LinkIj*wd+cross(w,LinkIj*w);
83 | if j==1
84 | Fm=F;
85 | Nm=N;
86 | else
87 | Fm=[Fm F];
88 | Nm=[Nm N];
89 | end;
90 | end
91 | f=fext(1:3);nn=fext(4:6);
92 | for j=6:-1:1,%the backward recursion
93 | pstar=pstarm(:,j);
94 | if j==6,
95 | R=eye(3,3);
96 | else
97 | R=Rm{j+1};
98 | end
99 | r=linkR(j,:)';
100 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
101 | f=R*f+Fm(:,j);
102 | R=Rm{j};
103 | inerH(ji,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
104 | end
105 | end
106 |
107 | %calculate taug
108 | grav=[0;0;9.81];qd=zeros(1,6);qdd=zeros(1,6);
109 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
110 | for j=1:6,%the forward recursion
111 | R=Rm{j}';
112 | pstar=pstarm(:,j);
113 | r=linkR(j,:)';
114 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
115 | w=R*(w+z0*qd(j));
116 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
117 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
118 | F=linkM(j)*vhat;
119 | LinkIj=diag(linkI(j,1:3));
120 | N=LinkIj*wd+cross(w,LinkIj*w);
121 | if j==1
122 | Fm=F;
123 | Nm=N;
124 | else
125 | Fm=[Fm F];
126 | Nm=[Nm N];
127 | end;
128 | end
129 | f=fext(1:3);nn=fext(4:6);%the backward recursion
130 | for j=6:-1:1,
131 | pstar=pstarm(:,j);
132 | if j==6,
133 | R=eye(3,3);
134 | else
135 | R=Rm{j+1};
136 | end
137 | r=linkR(j,:)';
138 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
139 | f=R*f+Fm(:,j);
140 | R=Rm{j};
141 | taug(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
142 | end
143 |
144 | %calculate tauc
145 | grav=[0;0;0];qd=keepqd;qdd=zeros(1,6);
146 | w=zeros(3,1);wd=zeros(3,1);vd=grav;v=zeros(3,1);%seemingly v useless
147 | for j=1:6,%the forward recursion
148 | R=Rm{j}';
149 | pstar=pstarm(:,j);
150 | r=linkR(j,:)';
151 | wd=R*(wd+z0*qdd(j)+cross(w,z0*qd(j)));
152 | w=R*(w+z0*qd(j));
153 | vd=cross(wd,pstar)+cross(w,cross(w,pstar))+R*vd;
154 | vhat=cross(wd,r)+cross(w,cross(w,r))+vd;
155 | F=linkM(j)*vhat;
156 | LinkIj=diag(linkI(j,1:3));
157 | N=LinkIj*wd+cross(w,LinkIj*w);
158 | if j==1
159 | Fm=F;
160 | Nm=N;
161 | else
162 | Fm=[Fm F];
163 | Nm=[Nm N];
164 | end;
165 | end
166 | f=fext(1:3);nn=fext(4:6);%the backward recursion
167 | for j=6:-1:1,
168 | pstar=pstarm(:,j);
169 | if j==6,
170 | R=eye(3,3);
171 | else
172 | R=Rm{j+1};
173 | end
174 | r=linkR(j,:)';
175 | nn=R*(nn+cross(R'*pstar,f))+cross(pstar+r,Fm(:,j))+Nm(:,j);
176 | f=R*f+Fm(:,j);
177 | R=Rm{j};
178 | tauc(1,j)=nn'*(R'*z0)+linkJm(j)*qdd(j)*(linkG(j)^2)+0;
179 | end
--------------------------------------------------------------------------------
/code/EXP1/mvn_ZZJLJrebi4.m:
--------------------------------------------------------------------------------
1 | clear;
2 | format long;
3 | load mvn_SYSdata;
4 | load mvn_SRDdata1;
5 | load mvn_SRDdata2;
6 | load mvn_SRDdata3;
7 |
8 | %==plotting angles==
9 | figure;
10 | plot(t,qAll(:,1),'m--',t,qAll(:,2),'g.-',t,qAll(:,3),'k.',t,qAll(:,4),'r:',t,qAll(:,5),'k--',t,qAll(:,6),'b-','linewidth',1.5);hold on;%axis([0 10 -2.5 2.5]);
11 | % plot(t,0.0349,t,-1.7453);
12 | title('Simulated theta');
13 | xlabel('Time (Second)');
14 | legend('q1','q2','q3','q4','q5','q6');
15 | %%%%%%%%%%%%%%%%%%%%%%%%
16 | % qplim1=qP(1)*ones(size(t));
17 | % qplim2=qP(2)*ones(size(t));
18 | % qplim3=qP(3)*ones(size(t));
19 | % qplim4=qP(4)*ones(size(t));
20 | % qplim5=qP(5)*ones(size(t));
21 | % qplim6=qP(6)*ones(size(t));
22 | %
23 | % qmlim1=qM(1)*ones(size(t));
24 | % qmlim2=qM(2)*ones(size(t));
25 | % qmlim3=qM(3)*ones(size(t));
26 | % qmlim4=qM(4)*ones(size(t));
27 | % qmlim5=qM(5)*ones(size(t));
28 | % qmlim6=qM(6)*ones(size(t));
29 | % %%%%%%%%%%%%%%%%%%%%
30 | %
31 | % figure;
32 | % plot(t,qAll(:,1),'linewidth',1.5);hold on;
33 | % plot(t,qplim1,'k-',t,qmlim1,'k-','linewidth',1.5);
34 | % ylim([-4 4]);
35 | % xlim([0 15]);
36 | % title('Simulated theta q1');
37 | % xlabel('Time (Second)');
38 | %
39 | % figure;
40 | % plot(t,qAll(:,2),'linewidth',1.5);hold on;
41 | % plot(t,qplim2,'k-',t,qmlim2,'k-','linewidth',1.5);
42 | % ylim([-4 2]);
43 | % xlim([0 15]);
44 | % title('Simulated theta q2');
45 | % xlabel('Time (Second)');
46 | %
47 | % figure;
48 | % plot(t,qAll(:,3),'linewidth',1.5);hold on;
49 | % plot(t,qplim3,'k-',t,qmlim3,'k-','linewidth',1.5);
50 | % ylim([-2 4]);
51 | % xlim([0 15]);
52 | % title('Simulated theta q3');
53 | % xlabel('Time (Second)');
54 | %
55 | % figure;
56 | % plot(t,qAll(:,4),'linewidth',1.5);hold on;
57 | % plot(t,qplim4,'k-',t,qmlim4,'k-','linewidth',1.5);
58 | % ylim([-2.5 4]);
59 | % xlim([0 15]);
60 | % title('Simulated theta q4');
61 | % xlabel('Time (Second)');
62 | %
63 | % figure;
64 | % plot(t,qAll(:,5),'linewidth',1.5);hold on;
65 | % plot(t,qplim5,'k-',t,qmlim5,'k-','linewidth',1.5);
66 | % ylim([-2 0.5]);
67 | % xlim([0 15]);
68 | % title('Simulated theta q5');
69 | % xlabel('Time (Second)');
70 | %
71 | % figure;
72 | % plot(t,qAll(:,6),'linewidth',1.5);hold on;
73 | % plot(t,qplim6,'k-',t,qmlim6,'k-','linewidth',1.5);
74 | % ylim([-4 4]);
75 | % xlim([0 15]);
76 | % title('Simulated theta q6');
77 | % xlabel('Time (Second)');
78 | %
79 | %
80 | %
81 | %
82 | %
83 | %
84 | %
85 | %
86 | figure;
87 | plot(t,dqAll(:,1),'m--',t,dqAll(:,2),'g.-',t,dqAll(:,3),'k.',t,dqAll(:,4),'r:',t,dqAll(:,5),'k--',t,dqAll(:,6),'b-','linewidth',1.5);hold on;
88 | title('Simulated dotTheta');
89 | xlabel('Time (Second)');
90 | legend('dq1','dq2','dq3','dq4','dq5','dq6');
91 |
92 | figure;
93 | plot(t,ddqAll(:,1),'m--',t,ddqAll(:,2),'g.-',t,ddqAll(:,3),'k.',t,ddqAll(:,4),'r:',t,ddqAll(:,5),'k--',t,ddqAll(:,6),'b-','linewidth',1.5);hold on;
94 | title('Simulated dotdotTheta');
95 | xlabel('Time (Second)');
96 | legend('ddq1','ddq2','ddq3','ddq4','ddq5','ddq6');
97 |
98 | % figure
99 | % plot(t,uAll(:,1:7));hold on;
100 | % xlabel('Time (Second)');
101 | % title('compare last figure: the same or not?');
102 |
103 | % figure;
104 | % plot(t,tauAll);hold on;
105 | % title('Simulated Torque');
106 | % xlabel('Time (Second)');
107 | % legend('\tau_1','\tau_2','\tau_3','\tau_4','\tau_5','\tau_6');
108 |
109 | %==plotting 3d trajectories of joints==
110 | figure;
111 | grid on;
112 | plot3(j2px,j2py,j2pz);hold on;
113 | plot3(j3px,j3py,j3pz);hold on;
114 | plot3(j4px,j4py,j4pz);hold on;
115 | plot3(j5px,j5py,j5pz);hold on;
116 | plot3(posx,posy,posz);hold on;
117 | xlabel('Time (Second)');
118 | title('joints Position');
119 | legend('X','Y','Z');
120 |
121 | %==plotting end effector==
122 | figure;
123 | plot(t,posx,'m--',t,posy,'c.-',t,posz,'k-');hold on;
124 | %axis([0 10 -0.5 1]);
125 | xlabel('Time (Second)');
126 | title('End Effector Position');
127 | legend('X','Y','Z');
128 |
129 | figure;
130 | plot(t,dposx,t,dposy,t,dposz);
131 | %axis([0 10 -0.2 0.2]);
132 | xlabel('Time (Second)');
133 | title('End Effector Velocity');
134 | legend('dX','dY','dZ');
135 |
136 | figure;
137 | plot(t,ddposx,t,ddposy,t,ddposz);
138 | %axis([0 10 -0.2 0.2]);
139 | xlabel('Time (Second)');
140 | title('End Effector Acceleration');
141 | legend('ddX','ddY','ddZ');
142 |
143 | figure;
144 | plot(t,erposx,'m--',t,erposy,'b-',t,erposz,'k-.','linewidth',2.5);
145 | title('Position Error');
146 | xlabel('Time (Second)');
147 | legend('ex','ey','ez');
148 |
149 | figure;
150 |
151 | plot(t,erdposx,t,erdposy,t,erdposz);
152 | title('Velocity Error');
153 | xlabel('Time (Second)');
154 | legend('edx','edy','edz');
155 |
156 | figure;
157 | plot(t,erddposx,t,erddposy,t,erddposz);
158 | %axis([0 10 -15*10^-5 15*10^-5]);
159 | title('Acceleration Error');
160 | legend('eddx','eddy','eddz');
161 | xlabel('Time (Second)');
162 |
163 | figure;
164 | plot3(rx,ry,rz,'k--','linewidth',2);
165 | hold on;
166 | for jj=1:8:length(t)
167 | plot3(posx(jj),posy(jj),posz(jj),'md-','linewidth',2);
168 | grid on;
169 | hold on;
170 | end
171 | legend('Expected Path','Actual Trajectories');
172 | xlabel('x');
173 | ylabel('y');
174 | zlabel('z');
175 |
176 | u_initial=u0;
177 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qAll(1,:));
178 | pos_initial=[Ppx Ppy Ppz]
179 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qAll(length(t),:));
180 | pos_final=[Ppx Ppy Ppz]
181 | EndeffectorPe=pos_final(6,:)-pos_initial(6,:);
182 | %save position status information to "txt" file
183 | fid=fopen('joint_position_information.txt','a');
184 | fprintf(fid,'u_initial: %g\n',u_initial);
185 | % fprintf(fid,'initial_joint_position: %g\n',pos_initial);
186 | % fprintf(fid,'final_joint_position: %g\n',pos_final);
187 | fprintf(fid,'End-effector_position_error: %g\n',EndeffectorPe);
188 | fclose(fid);
189 | %save join drift information to "txt" file
190 | joindrift=qAll(length(t),:)-qAll(1,:)
191 | joindriftnorm=norm(joindrift)
192 | fid=fopen('join_angle_drift_information.txt','a');
193 | fprintf(fid,'u_initial: %g\n',u_initial);
194 | fprintf(fid,'joindrift: %g\n',joindrift);
195 | fprintf(fid,'joindriftnorm: %g\n',joindriftnorm);
196 | fclose(fid);
197 |
198 | % figure;
199 | % plot(t,qAll(:,1),'m--','linewidth',1.5);hold on;
200 | % legend('q1');
201 | % figure;
202 | % plot(t,qAll(:,2),'g.-','linewidth',1.5);hold on;
203 | % legend('q2');
204 | % figure;
205 | % plot(t,qAll(:,3),'k.','linewidth',1.5);hold on;
206 | % legend('q3');
207 | % figure;
208 | % plot(t,qAll(:,4),'r:','linewidth',1.5);hold on;
209 | % legend('q4');
210 | % figure;
211 | % plot(t,qAll(:,5),'k--','linewidth',1.5);hold on;
212 | % legend('q5');
213 | % figure;
214 | % plot(t,qAll(:,6),'b-','linewidth',1.5);hold on;
215 | % legend('q6');
216 |
217 | %%????????????
218 | figure(30);
219 | plot(t,error,'linewidth',2.5);
220 | title('function error')
221 | xlabel('Time(Second)');
222 | ylabel('error');
223 | hold on;
--------------------------------------------------------------------------------
/code/EXP2/mvn_ZZJLJrebi4.m:
--------------------------------------------------------------------------------
1 | clear;
2 | format long;
3 | load mvn_SYSdata;
4 | load mvn_SRDdata1;
5 | load mvn_SRDdata2;
6 | load mvn_SRDdata3;
7 |
8 | %==plotting angles==
9 | figure;
10 | plot(t,qAll(:,1),'m--',t,qAll(:,2),'g.-',t,qAll(:,3),'k.',t,qAll(:,4),'r:',t,qAll(:,5),'k--',t,qAll(:,6),'b-','linewidth',1.5);hold on;%axis([0 10 -2.5 2.5]);
11 | % plot(t,0.0349,t,-1.7453);
12 | title('Simulated theta');
13 | xlabel('Time (Second)');
14 | legend('q1','q2','q3','q4','q5','q6');
15 | %%%%%%%%%%%%%%%%%%%%%%%%
16 | % qplim1=qP(1)*ones(size(t));
17 | % qplim2=qP(2)*ones(size(t));
18 | % qplim3=qP(3)*ones(size(t));
19 | % qplim4=qP(4)*ones(size(t));
20 | % qplim5=qP(5)*ones(size(t));
21 | % qplim6=qP(6)*ones(size(t));
22 | %
23 | % qmlim1=qM(1)*ones(size(t));
24 | % qmlim2=qM(2)*ones(size(t));
25 | % qmlim3=qM(3)*ones(size(t));
26 | % qmlim4=qM(4)*ones(size(t));
27 | % qmlim5=qM(5)*ones(size(t));
28 | % qmlim6=qM(6)*ones(size(t));
29 | % %%%%%%%%%%%%%%%%%%%%
30 | %
31 | % figure;
32 | % plot(t,qAll(:,1),'linewidth',1.5);hold on;
33 | % plot(t,qplim1,'k-',t,qmlim1,'k-','linewidth',1.5);
34 | % ylim([-4 4]);
35 | % xlim([0 15]);
36 | % title('Simulated theta q1');
37 | % xlabel('Time (Second)');
38 | %
39 | % figure;
40 | % plot(t,qAll(:,2),'linewidth',1.5);hold on;
41 | % plot(t,qplim2,'k-',t,qmlim2,'k-','linewidth',1.5);
42 | % ylim([-4 2]);
43 | % xlim([0 15]);
44 | % title('Simulated theta q2');
45 | % xlabel('Time (Second)');
46 | %
47 | % figure;
48 | % plot(t,qAll(:,3),'linewidth',1.5);hold on;
49 | % plot(t,qplim3,'k-',t,qmlim3,'k-','linewidth',1.5);
50 | % ylim([-2 4]);
51 | % xlim([0 15]);
52 | % title('Simulated theta q3');
53 | % xlabel('Time (Second)');
54 | %
55 | % figure;
56 | % plot(t,qAll(:,4),'linewidth',1.5);hold on;
57 | % plot(t,qplim4,'k-',t,qmlim4,'k-','linewidth',1.5);
58 | % ylim([-2.5 4]);
59 | % xlim([0 15]);
60 | % title('Simulated theta q4');
61 | % xlabel('Time (Second)');
62 | %
63 | % figure;
64 | % plot(t,qAll(:,5),'linewidth',1.5);hold on;
65 | % plot(t,qplim5,'k-',t,qmlim5,'k-','linewidth',1.5);
66 | % ylim([-2 0.5]);
67 | % xlim([0 15]);
68 | % title('Simulated theta q5');
69 | % xlabel('Time (Second)');
70 | %
71 | % figure;
72 | % plot(t,qAll(:,6),'linewidth',1.5);hold on;
73 | % plot(t,qplim6,'k-',t,qmlim6,'k-','linewidth',1.5);
74 | % ylim([-4 4]);
75 | % xlim([0 15]);
76 | % title('Simulated theta q6');
77 | % xlabel('Time (Second)');
78 | %
79 | %
80 | %
81 | %
82 | %
83 | %
84 | %
85 | %
86 | figure;
87 | plot(t,dqAll(:,1),'m--',t,dqAll(:,2),'g.-',t,dqAll(:,3),'k.',t,dqAll(:,4),'r:',t,dqAll(:,5),'k--',t,dqAll(:,6),'b-','linewidth',1.5);hold on;
88 | title('Simulated dotTheta');
89 | xlabel('Time (Second)');
90 | legend('dq1','dq2','dq3','dq4','dq5','dq6');
91 |
92 | figure;
93 | plot(t,ddqAll(:,1),'m--',t,ddqAll(:,2),'g.-',t,ddqAll(:,3),'k.',t,ddqAll(:,4),'r:',t,ddqAll(:,5),'k--',t,ddqAll(:,6),'b-','linewidth',1.5);hold on;
94 | title('Simulated dotdotTheta');
95 | xlabel('Time (Second)');
96 | legend('ddq1','ddq2','ddq3','ddq4','ddq5','ddq6');
97 |
98 | % figure
99 | % plot(t,uAll(:,1:7));hold on;
100 | % xlabel('Time (Second)');
101 | % title('compare last figure: the same or not?');
102 |
103 | % figure;
104 | % plot(t,tauAll);hold on;
105 | % title('Simulated Torque');
106 | % xlabel('Time (Second)');
107 | % legend('\tau_1','\tau_2','\tau_3','\tau_4','\tau_5','\tau_6');
108 |
109 | %==plotting 3d trajectories of joints==
110 | figure;
111 | grid on;
112 | plot3(j2px,j2py,j2pz);hold on;
113 | plot3(j3px,j3py,j3pz);hold on;
114 | plot3(j4px,j4py,j4pz);hold on;
115 | plot3(j5px,j5py,j5pz);hold on;
116 | plot3(posx,posy,posz);hold on;
117 | xlabel('Time (Second)');
118 | title('joints Position');
119 | legend('X','Y','Z');
120 |
121 | %==plotting end effector==
122 | figure;
123 | plot(t,posx,'m--',t,posy,'c.-',t,posz,'k-');hold on;
124 | %axis([0 10 -0.5 1]);
125 | xlabel('Time (Second)');
126 | title('End Effector Position');
127 | legend('X','Y','Z');
128 |
129 | figure;
130 | plot(t,dposx,t,dposy,t,dposz);
131 | %axis([0 10 -0.2 0.2]);
132 | xlabel('Time (Second)');
133 | title('End Effector Velocity');
134 | legend('dX','dY','dZ');
135 |
136 | figure;
137 | plot(t,ddposx,t,ddposy,t,ddposz);
138 | %axis([0 10 -0.2 0.2]);
139 | xlabel('Time (Second)');
140 | title('End Effector Acceleration');
141 | legend('ddX','ddY','ddZ');
142 |
143 | figure;
144 | plot(t,erposx,'m--',t,erposy,'b-',t,erposz,'k-.','linewidth',2.5);
145 | title('Position Error');
146 | xlabel('Time (Second)');
147 | legend('ex','ey','ez');
148 |
149 | figure;
150 |
151 | plot(t,erdposx,t,erdposy,t,erdposz);
152 | title('Velocity Error');
153 | xlabel('Time (Second)');
154 | legend('edx','edy','edz');
155 |
156 | figure;
157 | plot(t,erddposx,t,erddposy,t,erddposz);
158 | %axis([0 10 -15*10^-5 15*10^-5]);
159 | title('Acceleration Error');
160 | legend('eddx','eddy','eddz');
161 | xlabel('Time (Second)');
162 |
163 | figure;
164 | plot3(rx,ry,rz,'k--','linewidth',2);
165 | hold on;
166 | for jj=1:8:length(t)
167 | plot3(posx(jj),posy(jj),posz(jj),'md-','linewidth',2);
168 | grid on;
169 | hold on;
170 | end
171 | legend('Expected Path','Actual Trajectories');
172 | xlabel('x');
173 | ylabel('y');
174 | zlabel('z');
175 |
176 |
177 | u_initial=u0;
178 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qAll(1,:));
179 | pos_initial=[Ppx Ppy Ppz]
180 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qAll(length(t),:));
181 | pos_final=[Ppx Ppy Ppz]
182 | EndeffectorPe=pos_final(6,:)-pos_initial(6,:);
183 | %save position status information to "txt" file
184 | fid=fopen('joint_position_information.txt','a');
185 | fprintf(fid,'u_initial: %g\n',u_initial);
186 | % fprintf(fid,'initial_joint_position: %g\n',pos_initial);
187 | % fprintf(fid,'final_joint_position: %g\n',pos_final);
188 | fprintf(fid,'End-effector_position_error: %g\n',EndeffectorPe);
189 | fclose(fid);
190 | %save join drift information to "txt" file
191 | joindrift=qAll(length(t),:)-qAll(1,:)
192 | joindriftnorm=norm(joindrift)
193 | fid=fopen('join_angle_drift_information.txt','a');
194 | fprintf(fid,'u_initial: %g\n',u_initial);
195 | fprintf(fid,'joindrift: %g\n',joindrift);
196 | fprintf(fid,'joindriftnorm: %g\n',joindriftnorm);
197 | fclose(fid);
198 |
199 | % figure;
200 | % plot(t,qAll(:,1),'m--','linewidth',1.5);hold on;
201 | % legend('q1');
202 | % figure;
203 | % plot(t,qAll(:,2),'g.-','linewidth',1.5);hold on;
204 | % legend('q2');
205 | % figure;
206 | % plot(t,qAll(:,3),'k.','linewidth',1.5);hold on;
207 | % legend('q3');
208 | % figure;
209 | % plot(t,qAll(:,4),'r:','linewidth',1.5);hold on;
210 | % legend('q4');
211 | % figure;
212 | % plot(t,qAll(:,5),'k--','linewidth',1.5);hold on;
213 | % legend('q5');
214 | % figure;
215 | % plot(t,qAll(:,6),'b-','linewidth',1.5);hold on;
216 | % legend('q6');
217 |
218 | %%????????????
219 | figure(30);
220 | plot(t,error,'linewidth',2.5);
221 | title('function error')
222 | xlabel('Time(Second)');
223 | ylabel('error');
224 | hold on;
--------------------------------------------------------------------------------
/code/EXP1/ZYNjdj.m:
--------------------------------------------------------------------------------
1 | function [Jacob0,dotJacob0]=ZYNjdj(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,dq)
2 | %VERSION of April 15th, 2001
3 | mys=sin(q);
4 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
5 | myc=cos(q);
6 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
7 |
8 | alpha_4 = alpha_i(4);alpha_5 = alpha_i(5);
9 | d_1 = d_i(1);d_2 = d_i(2);d_3 = d_i(3);d_4 = d_i(4);
10 | d_5 = d_i(5);d_6 = d_i(6);
11 | % sp = sin(phi);
12 | % cp = cos(phi);
13 | sa4 = sin(alpha_4);ca4 = cos(alpha_4);
14 | sa5 = sin(alpha_5);ca5 = cos(alpha_5);
15 |
16 | sp1=sin(0.02);cp1=cos(0.02);
17 | sp2=sin(0.05);cp2=cos(0.05);
18 |
19 | AW1 = [1, 0, 0, 0;...
20 | 0, cp1, sp1, 0;...
21 | 0, -sp1, cp1, 0;...
22 | 0, 0, 0, 1];
23 |
24 | AW2 = [cp2, 0, -sp2, 0;...
25 | 0, 1, 0, 0;...
26 | sp2, 0, cp2, 0;...
27 | 0, 0, 0, 1];
28 |
29 | AW1=AW1*AW2;
30 |
31 | A1 = [c1, 0, s1, 0;...
32 | s1, 0, -c1, 0;...
33 | 0, 1, 0, d_1;...
34 | 0, 0, 0, 1];
35 | DA1 = [-s1, 0, c1, 0;...
36 | c1, 0, s1, 0;...
37 | 0, 0, 0, 0;...
38 | 0, 0, 0, 0];
39 | DDA1 = [-c1, 0, -s1, 0;...
40 | -s1, 0, c1, 0;...
41 | 0, 0, 0, 0;...
42 | 0, 0, 0, 0];
43 |
44 | A2 = [c2, s2, 0, D(2)*c2;...
45 | s2, -c2, 0, D(2)*s2;...
46 | 0, 0, -1, 0;...
47 | 0, 0, 0, 1];
48 | DA2 = [-s2, c2, 0, D(2)*-s2;...
49 | c2, s2, 0, D(2)*c2;...
50 | 0, 0, 0, 0;...
51 | 0, 0, 0, 0];
52 | DDA2 = [-c2, -s2, 0, D(2)*-c2;...
53 | -s2, c2, 0, D(2)*-s2;...
54 | 0, 0, 0, 0;...
55 | 0, 0, 0, 0];
56 |
57 | A3 = [c3, 0, s3, 0;...
58 | s3, 0, -c3, 0;...
59 | 0, 1, 0, d_3;...
60 | 0, 0, 0, 1];
61 | DA3 = [-s3, 0, c3, 0;...
62 | c3, 0, s3, 0;...
63 | 0, 0, 0, 0;...
64 | 0, 0, 0, 0];
65 | DDA3 = [-c3, 0, -s3, 0;...
66 | -s3, 0, c3, 0;...
67 | 0, 0, 0, 0;...
68 | 0, 0, 0, 0];
69 |
70 | A4 = [c4, -ca4*s4, sa4*s4, 0;...
71 | s4, ca4*c4, -sa4*c4, 0;...
72 | 0, sa4, ca4, d_4;...
73 | 0, 0, 0, 1];
74 | DA4 = [-s4, -ca4*c4, sa4*c4, 0;...
75 | c4, ca4*-s4, sa4*s4, 0;...
76 | 0, 0, 0, 0;...
77 | 0, 0, 0, 0];
78 | DDA4 = [-c4, ca4*s4, sa4*-s4, 0;...
79 | -s4, ca4*-c4, sa4*c4, 0;...
80 | 0, 0, 0, 0;...
81 | 0, 0, 0, 0];
82 |
83 | A5 = [c5, -ca5*s5, sa5*s5, 0;...
84 | s5, ca5*c5, -sa5*c5, 0;...
85 | 0, sa5, ca5, d_5;...
86 | 0, 0, 0, 1];
87 | DA5 = [-s5, -ca5*c5, sa5*c5, 0;...
88 | c5, ca5*-s5, sa5*s5, 0;...
89 | 0, 0, 0, 0;...
90 | 0, 0, 0, 0];
91 | DDA5 = [-c5, ca5*s5, sa5*-s5, 0;...
92 | -s5, ca5*-c5, sa5*c5, 0;...
93 | 0, 0, 0, 0;...
94 | 0, 0, 0, 0];
95 |
96 | A6 = [c6, s6, 0, 0;...
97 | s6, -c6, 0, 0;...
98 | 0, 0, -1, d_6;...
99 | 0, 0, 0, 1];
100 | DA6 = [-s6, c6, 0, 0;...
101 | c6, s6, 0, 0;...
102 | 0, 0, 0, 0;...
103 | 0, 0, 0, 0];
104 | DDA6 = [-c6, -s6, 0, 0;...
105 | -s6, c6, 0, 0;...
106 | 0, 0, 0, 0;...
107 | 0, 0, 0, 0];
108 |
109 | Pen=[0.0;-pen_long;0.0;1.0];
110 |
111 | A6v=A6*Pen;
112 | A56v=A5*A6v;
113 | A456v=A4*A56v;
114 | A3456v=A3*A456v;
115 | A23456v=A2*A3456v;
116 | Je1=DA1*A23456v;
117 | D2A3456v=DA2*A3456v;
118 | Je2=A1*D2A3456v;
119 | D3A456v=DA3*A456v;
120 | A2D3A456v=A2*D3A456v;
121 | Je3=A1*A2D3A456v;
122 | D4A56v=DA4*A56v;
123 | A3D4A56v=A3*D4A56v;
124 | A23D4A56v=A2*A3D4A56v;
125 | Je4=A1*A23D4A56v;
126 | D5A6v=DA5*A6v;
127 | A4D5A6v=A4*D5A6v;
128 | A34D5A6v=A3*A4D5A6v;
129 | A234D5A6v=A2*A34D5A6v;
130 | Je5=A1*A234D5A6v;
131 | D6v=DA6*Pen;
132 | A5D6v=A5*D6v;
133 | A45D6v=A4*A5D6v;
134 | A345D6v=A3*A45D6v;
135 | A2345D6v=A2*A345D6v;
136 | Je6=A1*A2345D6v;
137 |
138 | Je1=AW1*Je1;
139 | Je2=AW1*Je2;
140 | Je3=AW1*Je3;
141 | Je4=AW1*Je4;
142 | Je5=AW1*Je5;
143 | Je6=AW1*Je6;
144 | Jacob0(:,1)=Je1(1:3,1);
145 | Jacob0(:,2)=Je2(1:3,1);
146 | Jacob0(:,3)=Je3(1:3,1);
147 | Jacob0(:,4)=Je4(1:3,1);
148 | Jacob0(:,5)=Je5(1:3,1);
149 | Jacob0(:,6)=Je6(1:3,1);
150 |
151 | DJe11=DDA1*A23456v*dq(1);%column 1
152 | D12A3456v=DA1*D2A3456v;
153 | DJe12=D12A3456v*dq(2);
154 | D1A2D3A456v=DA1*A2D3A456v;
155 | DJe13=D1A2D3A456v*dq(3);
156 | D1A23D4A56v=DA1*A23D4A56v;
157 | DJe14=D1A23D4A56v*dq(4);
158 | D1A234D5A6v=DA1*A234D5A6v;
159 | DJe15=D1A234D5A6v*dq(5);
160 | D1A2345D6v=DA1*A2345D6v;
161 | DJe16=D1A2345D6v*dq(6);
162 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
163 | DJtemp=AW1*DJtemp;
164 | dotJacob0(:,1)=DJtemp(1:3,1);
165 | DJe11=D12A3456v*dq(1);%column 2
166 | DJe12=A1*(DDA2*A3456v)*dq(2);
167 | A1D23A456v=A1*(DA2*D3A456v);
168 | DJe13=A1D23A456v*dq(3);
169 | A1D2A3D4A56v=A1*(DA2*A3D4A56v);
170 | DJe14=A1D2A3D4A56v*dq(4);
171 | A1D2A34D5A6v=A1*(DA2*A34D5A6v);
172 | DJe15=A1D2A34D5A6v*dq(5);
173 | A1D2A345D6v=A1*(DA2*A345D6v);
174 | DJe16=A1D2A345D6v*dq(6);
175 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
176 | DJtemp=AW1*DJtemp;
177 | dotJacob0(:,2)=DJtemp(1:3,1);
178 | DJe11=D1A2D3A456v*dq(1);%column 3
179 | DJe12=A1D23A456v*dq(2);
180 | Je6=A2*(DDA3*A456v);%Je6 as a temp
181 | DJe13=A1*Je6*dq(3);
182 | Je6=A2*(DA3*D4A56v);%Je6 as a temp
183 | A12D34A56v=A1*Je6;%Je6 as a temp
184 | DJe14=A12D34A56v*dq(4);
185 | Je6=A2*(DA3*A4D5A6v);%Je6 as a temp
186 | A12D3A4D5A6v=A1*Je6;%Je6 as a temp
187 | DJe15=A12D3A4D5A6v*dq(5);
188 | Je6=A2*(DA3*A45D6v);%Je6 as a temp
189 | A12D3A45D6v=A1*Je6;%Je6 as a temp
190 | DJe16=A12D3A45D6v*dq(6);
191 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
192 | DJtemp=AW1*DJtemp;
193 | dotJacob0(:,3)=DJtemp(1:3,1);
194 | DJe11=D1A23D4A56v*dq(1);%column 4
195 | DJe12=A1D2A3D4A56v*dq(2);
196 | DJe13=A12D34A56v*dq(3);
197 | Je6=A3*(DDA4*A56v);%Je6 as a temp
198 | Je6=A1*(A2*Je6);%Je6 as a temp
199 | DJe14=Je6*dq(4);
200 | Je6=A3*(DA4*D5A6v);%Je6 as a temp
201 | A123D45A6v=A1*(A2*Je6);
202 | DJe15=A123D45A6v*dq(5);
203 | Je6=A3*(DA4*A5D6v);%Je6 as a temp
204 | A123D4A5D6v=A1*(A2*Je6);
205 | DJe16=A123D4A5D6v*dq(6);
206 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
207 | DJtemp=AW1*DJtemp;
208 | dotJacob0(:,4)=DJtemp(1:3,1);
209 | DJe11=D1A234D5A6v*dq(1);%column 5
210 | DJe12=A1D2A34D5A6v*dq(2);
211 | DJe13=A12D3A4D5A6v*dq(3);
212 | DJe14=A123D45A6v*dq(4);
213 | Je6=A4*(DDA5*A6v);%Je6 as a temp
214 | Je6=A2*(A3*Je6);
215 | DJe15=A1*Je6*dq(5);
216 | Je6=A4*(DA5*D6v);%Je6 as a temp
217 | Je6=A2*(A3*Je6);
218 | A1234D56v=A1*Je6;
219 | DJe16=A1234D56v*dq(6);
220 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
221 | DJtemp=AW1*DJtemp;
222 | dotJacob0(:,5)=DJtemp(1:3,1);
223 | DJe11=D1A2345D6v*dq(1);%column 6
224 | DJe12=A1D2A345D6v*dq(2);
225 | DJe13=A12D3A45D6v*dq(3);
226 | DJe14=A123D4A5D6v*dq(4);
227 | DJe15=A1234D56v*dq(5);
228 | Je6=A5*(DDA6*Pen);%Je6 as a temp
229 | Je6=A3*(A4*Je6);
230 | Je6=A1*(A2*Je6);
231 | DJe16=Je6*dq(6);
232 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
233 | DJtemp=AW1*DJtemp;
234 | dotJacob0(:,6)=DJtemp(1:3,1);
--------------------------------------------------------------------------------
/code/EXP2/ZYNjdj.m:
--------------------------------------------------------------------------------
1 | function [Jacob0,dotJacob0]=ZYNjdj(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,dq)
2 | %VERSION of April 15th, 2001
3 | mys=sin(q);
4 | s1=mys(1);s2=mys(2);s3=mys(3);s4=mys(4);s5=mys(5);s6=mys(6);
5 | myc=cos(q);
6 | c1=myc(1);c2=myc(2);c3=myc(3);c4=myc(4);c5=myc(5);c6=myc(6);
7 |
8 | alpha_4 = alpha_i(4);alpha_5 = alpha_i(5);
9 | d_1 = d_i(1);d_2 = d_i(2);d_3 = d_i(3);d_4 = d_i(4);
10 | d_5 = d_i(5);d_6 = d_i(6);
11 | % sp = sin(phi);
12 | % cp = cos(phi);
13 | sa4 = sin(alpha_4);ca4 = cos(alpha_4);
14 | sa5 = sin(alpha_5);ca5 = cos(alpha_5);
15 |
16 | sp1=sin(0.02);cp1=cos(0.02);
17 | sp2=sin(0.05);cp2=cos(0.05);
18 |
19 | AW1 = [1, 0, 0, 0;...
20 | 0, cp1, sp1, 0;...
21 | 0, -sp1, cp1, 0;...
22 | 0, 0, 0, 1];
23 |
24 | AW2 = [cp2, 0, -sp2, 0;...
25 | 0, 1, 0, 0;...
26 | sp2, 0, cp2, 0;...
27 | 0, 0, 0, 1];
28 |
29 | AW1=AW1*AW2;
30 |
31 | A1 = [c1, 0, s1, 0;...
32 | s1, 0, -c1, 0;...
33 | 0, 1, 0, d_1;...
34 | 0, 0, 0, 1];
35 | DA1 = [-s1, 0, c1, 0;...
36 | c1, 0, s1, 0;...
37 | 0, 0, 0, 0;...
38 | 0, 0, 0, 0];
39 | DDA1 = [-c1, 0, -s1, 0;...
40 | -s1, 0, c1, 0;...
41 | 0, 0, 0, 0;...
42 | 0, 0, 0, 0];
43 |
44 | A2 = [c2, s2, 0, D(2)*c2;...
45 | s2, -c2, 0, D(2)*s2;...
46 | 0, 0, -1, 0;...
47 | 0, 0, 0, 1];
48 | DA2 = [-s2, c2, 0, D(2)*-s2;...
49 | c2, s2, 0, D(2)*c2;...
50 | 0, 0, 0, 0;...
51 | 0, 0, 0, 0];
52 | DDA2 = [-c2, -s2, 0, D(2)*-c2;...
53 | -s2, c2, 0, D(2)*-s2;...
54 | 0, 0, 0, 0;...
55 | 0, 0, 0, 0];
56 |
57 | A3 = [c3, 0, s3, 0;...
58 | s3, 0, -c3, 0;...
59 | 0, 1, 0, d_3;...
60 | 0, 0, 0, 1];
61 | DA3 = [-s3, 0, c3, 0;...
62 | c3, 0, s3, 0;...
63 | 0, 0, 0, 0;...
64 | 0, 0, 0, 0];
65 | DDA3 = [-c3, 0, -s3, 0;...
66 | -s3, 0, c3, 0;...
67 | 0, 0, 0, 0;...
68 | 0, 0, 0, 0];
69 |
70 | A4 = [c4, -ca4*s4, sa4*s4, 0;...
71 | s4, ca4*c4, -sa4*c4, 0;...
72 | 0, sa4, ca4, d_4;...
73 | 0, 0, 0, 1];
74 | DA4 = [-s4, -ca4*c4, sa4*c4, 0;...
75 | c4, ca4*-s4, sa4*s4, 0;...
76 | 0, 0, 0, 0;...
77 | 0, 0, 0, 0];
78 | DDA4 = [-c4, ca4*s4, sa4*-s4, 0;...
79 | -s4, ca4*-c4, sa4*c4, 0;...
80 | 0, 0, 0, 0;...
81 | 0, 0, 0, 0];
82 |
83 | A5 = [c5, -ca5*s5, sa5*s5, 0;...
84 | s5, ca5*c5, -sa5*c5, 0;...
85 | 0, sa5, ca5, d_5;...
86 | 0, 0, 0, 1];
87 | DA5 = [-s5, -ca5*c5, sa5*c5, 0;...
88 | c5, ca5*-s5, sa5*s5, 0;...
89 | 0, 0, 0, 0;...
90 | 0, 0, 0, 0];
91 | DDA5 = [-c5, ca5*s5, sa5*-s5, 0;...
92 | -s5, ca5*-c5, sa5*c5, 0;...
93 | 0, 0, 0, 0;...
94 | 0, 0, 0, 0];
95 |
96 | A6 = [c6, s6, 0, 0;...
97 | s6, -c6, 0, 0;...
98 | 0, 0, -1, d_6;...
99 | 0, 0, 0, 1];
100 | DA6 = [-s6, c6, 0, 0;...
101 | c6, s6, 0, 0;...
102 | 0, 0, 0, 0;...
103 | 0, 0, 0, 0];
104 | DDA6 = [-c6, -s6, 0, 0;...
105 | -s6, c6, 0, 0;...
106 | 0, 0, 0, 0;...
107 | 0, 0, 0, 0];
108 |
109 | Pen=[0.0;-pen_long;0.0;1.0];
110 |
111 | A6v=A6*Pen;
112 | A56v=A5*A6v;
113 | A456v=A4*A56v;
114 | A3456v=A3*A456v;
115 | A23456v=A2*A3456v;
116 | Je1=DA1*A23456v;
117 | D2A3456v=DA2*A3456v;
118 | Je2=A1*D2A3456v;
119 | D3A456v=DA3*A456v;
120 | A2D3A456v=A2*D3A456v;
121 | Je3=A1*A2D3A456v;
122 | D4A56v=DA4*A56v;
123 | A3D4A56v=A3*D4A56v;
124 | A23D4A56v=A2*A3D4A56v;
125 | Je4=A1*A23D4A56v;
126 | D5A6v=DA5*A6v;
127 | A4D5A6v=A4*D5A6v;
128 | A34D5A6v=A3*A4D5A6v;
129 | A234D5A6v=A2*A34D5A6v;
130 | Je5=A1*A234D5A6v;
131 | D6v=DA6*Pen;
132 | A5D6v=A5*D6v;
133 | A45D6v=A4*A5D6v;
134 | A345D6v=A3*A45D6v;
135 | A2345D6v=A2*A345D6v;
136 | Je6=A1*A2345D6v;
137 |
138 | Je1=AW1*Je1;
139 | Je2=AW1*Je2;
140 | Je3=AW1*Je3;
141 | Je4=AW1*Je4;
142 | Je5=AW1*Je5;
143 | Je6=AW1*Je6;
144 | Jacob0(:,1)=Je1(1:3,1);
145 | Jacob0(:,2)=Je2(1:3,1);
146 | Jacob0(:,3)=Je3(1:3,1);
147 | Jacob0(:,4)=Je4(1:3,1);
148 | Jacob0(:,5)=Je5(1:3,1);
149 | Jacob0(:,6)=Je6(1:3,1);
150 |
151 | DJe11=DDA1*A23456v*dq(1);%column 1
152 | D12A3456v=DA1*D2A3456v;
153 | DJe12=D12A3456v*dq(2);
154 | D1A2D3A456v=DA1*A2D3A456v;
155 | DJe13=D1A2D3A456v*dq(3);
156 | D1A23D4A56v=DA1*A23D4A56v;
157 | DJe14=D1A23D4A56v*dq(4);
158 | D1A234D5A6v=DA1*A234D5A6v;
159 | DJe15=D1A234D5A6v*dq(5);
160 | D1A2345D6v=DA1*A2345D6v;
161 | DJe16=D1A2345D6v*dq(6);
162 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
163 | DJtemp=AW1*DJtemp;
164 | dotJacob0(:,1)=DJtemp(1:3,1);
165 | DJe11=D12A3456v*dq(1);%column 2
166 | DJe12=A1*(DDA2*A3456v)*dq(2);
167 | A1D23A456v=A1*(DA2*D3A456v);
168 | DJe13=A1D23A456v*dq(3);
169 | A1D2A3D4A56v=A1*(DA2*A3D4A56v);
170 | DJe14=A1D2A3D4A56v*dq(4);
171 | A1D2A34D5A6v=A1*(DA2*A34D5A6v);
172 | DJe15=A1D2A34D5A6v*dq(5);
173 | A1D2A345D6v=A1*(DA2*A345D6v);
174 | DJe16=A1D2A345D6v*dq(6);
175 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
176 | DJtemp=AW1*DJtemp;
177 | dotJacob0(:,2)=DJtemp(1:3,1);
178 | DJe11=D1A2D3A456v*dq(1);%column 3
179 | DJe12=A1D23A456v*dq(2);
180 | Je6=A2*(DDA3*A456v);%Je6 as a temp
181 | DJe13=A1*Je6*dq(3);
182 | Je6=A2*(DA3*D4A56v);%Je6 as a temp
183 | A12D34A56v=A1*Je6;%Je6 as a temp
184 | DJe14=A12D34A56v*dq(4);
185 | Je6=A2*(DA3*A4D5A6v);%Je6 as a temp
186 | A12D3A4D5A6v=A1*Je6;%Je6 as a temp
187 | DJe15=A12D3A4D5A6v*dq(5);
188 | Je6=A2*(DA3*A45D6v);%Je6 as a temp
189 | A12D3A45D6v=A1*Je6;%Je6 as a temp
190 | DJe16=A12D3A45D6v*dq(6);
191 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
192 | DJtemp=AW1*DJtemp;
193 | dotJacob0(:,3)=DJtemp(1:3,1);
194 | DJe11=D1A23D4A56v*dq(1);%column 4
195 | DJe12=A1D2A3D4A56v*dq(2);
196 | DJe13=A12D34A56v*dq(3);
197 | Je6=A3*(DDA4*A56v);%Je6 as a temp
198 | Je6=A1*(A2*Je6);%Je6 as a temp
199 | DJe14=Je6*dq(4);
200 | Je6=A3*(DA4*D5A6v);%Je6 as a temp
201 | A123D45A6v=A1*(A2*Je6);
202 | DJe15=A123D45A6v*dq(5);
203 | Je6=A3*(DA4*A5D6v);%Je6 as a temp
204 | A123D4A5D6v=A1*(A2*Je6);
205 | DJe16=A123D4A5D6v*dq(6);
206 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
207 | DJtemp=AW1*DJtemp;
208 | dotJacob0(:,4)=DJtemp(1:3,1);
209 | DJe11=D1A234D5A6v*dq(1);%column 5
210 | DJe12=A1D2A34D5A6v*dq(2);
211 | DJe13=A12D3A4D5A6v*dq(3);
212 | DJe14=A123D45A6v*dq(4);
213 | Je6=A4*(DDA5*A6v);%Je6 as a temp
214 | Je6=A2*(A3*Je6);
215 | DJe15=A1*Je6*dq(5);
216 | Je6=A4*(DA5*D6v);%Je6 as a temp
217 | Je6=A2*(A3*Je6);
218 | A1234D56v=A1*Je6;
219 | DJe16=A1234D56v*dq(6);
220 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
221 | DJtemp=AW1*DJtemp;
222 | dotJacob0(:,5)=DJtemp(1:3,1);
223 | DJe11=D1A2345D6v*dq(1);%column 6
224 | DJe12=A1D2A345D6v*dq(2);
225 | DJe13=A12D3A45D6v*dq(3);
226 | DJe14=A123D4A5D6v*dq(4);
227 | DJe15=A1234D56v*dq(5);
228 | Je6=A5*(DDA6*Pen);%Je6 as a temp
229 | Je6=A3*(A4*Je6);
230 | Je6=A1*(A2*Je6);
231 | DJe16=Je6*dq(6);
232 | DJtemp=DJe11+DJe12+DJe13+DJe14+DJe15+DJe16;
233 | DJtemp=AW1*DJtemp;
234 | dotJacob0(:,6)=DJtemp(1:3,1);
--------------------------------------------------------------------------------
/code/EXP1/mvn_ZZJLJrebi3.m:
--------------------------------------------------------------------------------
1 | %clear all;
2 | format long;
3 |
4 | global aa alpha_i a_i d_i D sa ca s2a c2a pen_long q0 T mu_p mu_v qP qM qDp qDm qDDp qDDm m Pinfty Minfty gamma m n znn_W k;
5 | load mvn_SYSdata;
6 | load mvn_SNDdata;
7 |
8 | Time=0; %Time????
9 | t_last=0;
10 | t_time=0;
11 | [z,n]=size(qAll);
12 | q_Time=zeros(z,1);
13 |
14 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q0); %??????????????
15 | ix0=Ppx(7);iy0=Ppy(7);iz0=Ppz(7);
16 | for jj=1:length(t),
17 | qjj=qAll(jj,:)';%simulated end effector position
18 | [Ppx,Ppy,Ppz]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qjj);
19 | j0px(jj,1)=Ppx(1);
20 | j1px(jj,1)=Ppx(2);
21 | j2px(jj,1)=Ppx(3);
22 | j3px(jj,1)=Ppx(4);
23 | j4px(jj,1)=Ppx(5);
24 | j5px(jj,1)=Ppx(6);
25 | posx(jj,1)=Ppx(7);%joint6
26 | j0py(jj,1)=Ppy(1);
27 | j1py(jj,1)=Ppy(2);
28 | j2py(jj,1)=Ppy(3);
29 | j3py(jj,1)=Ppy(4);
30 | j4py(jj,1)=Ppy(5);
31 | j5py(jj,1)=Ppy(6);
32 | posy(jj,1)=Ppy(7);%joint6
33 | j0pz(jj,1)=Ppz(1);
34 | j1pz(jj,1)=Ppz(2);
35 | j2pz(jj,1)=Ppz(3);
36 | j3pz(jj,1)=Ppz(4);
37 | j4pz(jj,1)=Ppz(5);
38 | j5pz(jj,1)=Ppz(6);
39 | posz(jj,1)=Ppz(7);%joint6
40 | %-------------------------------------------------
41 | dqjj=dqAll(jj,:)';%simulated end effector velocity
42 | [J,DJ]=ZYNjdj(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,qjj,dqjj);
43 | %-----------------
44 | dpos=J*dqjj;
45 | dposx(jj,1)=dpos(1);
46 | dposy(jj,1)=dpos(2);
47 | dposz(jj,1)=dpos(3);
48 |
49 | ddqjj=ddqAll(jj,:)';
50 | ddpos=J*ddqjj+DJ*dqjj;
51 | ddposx(jj,1)=ddpos(1);
52 | ddposy(jj,1)=ddpos(2);%simulated end-effector acceleration
53 | ddposz(jj,1)=ddpos(3);
54 |
55 | t_time=t(jj)-Time*T; %??????????????????????????????????????????????????????????????????????????????????
56 | if t_time>T;
57 | q_Time(jj,1)=1;
58 | Time=Time+1;
59 | t_time=t_time-T;
60 | end
61 |
62 | % phi_sin=2*pi*sin(0.5*pi*t_time/T);
63 | % phi=phi_sin*sin(0.5*pi*t_time/T);
64 | % phiDot=phi_sin*pi*cos(0.5*pi*t_time/T)/T;
65 | % phiDotDot=pi^3*cos(pi*t_time/T)/T^2;
66 | % num=4;
67 | % rx(jj,1)=aa*(cos(phi)+cos(num*phi)/num-1-1/num)+ix0; %????????????????????
68 | % ry(jj,1)=aa*(sin(phi)-sin(num*phi)/num)+iy0;
69 | % rz(jj,1)=0+iz0;
70 | % drx(jj,1)=-aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T + (2*pi^2*cos((pi*t_time)/(2*T))*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T)))/T);
71 | % dry(jj,1)=aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T - (2*pi^2*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T)))/T);
72 | % drz(jj,1)=0;
73 | % ddrx(jj,1)=-aa*((pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 + (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2)/T^2);
74 | % ddry(jj,1)=aa*((pi^3*cos((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2)/T^2 + (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 - (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2);
75 | % ddrz(jj,1)=0;
76 |
77 | phi_sin=2*pi*sin(0.5*pi*t_time/T);
78 | phi=phi_sin*sin(0.5*pi*t_time/T);
79 | phiDot=phi_sin*pi*cos(0.5*pi*t_time/T)/T;
80 | phiDotDot=pi^3*cos(pi*t_time/T)/T^2;
81 | num=4;
82 | rx(jj,1)=aa*(cos(phi)+cos(num*phi)/num-1-1/num)+ix0; %????????????????????
83 | ry(jj,1)=aa*(sin(phi)-sin(num*phi)/num)+iy0;
84 | rz(jj,1)=0+iz0;
85 | drx(jj,1)=-aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T + (2*pi^2*cos((pi*t_time)/(2*T))*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T)))/T);
86 | dry(jj,1)=aa*((2*pi^2*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T))*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T - (2*pi^2*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))*sin((pi*t_time)/(2*T)))/T);
87 | drz(jj,1)=0;
88 | ddrx(jj,1)=-aa*((pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (pi^3*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 + (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2)/T^2);
89 | ddry(jj,1)=aa*((pi^3*cos((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*sin((pi*t_time)/(2*T))^2*cos(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 - (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*cos((pi*t_time)/(2*T))^2)/T^2 + (pi^3*cos(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2 - (4*pi^4*cos((pi*t_time)/(2*T))^2*sin((pi*t_time)/(2*T))^2*sin(2*pi*sin((pi*t_time)/(2*T))^2))/T^2 + (4*pi^4*num*cos((pi*t_time)/(2*T))^2*sin(2*pi*num*sin((pi*t_time)/(2*T))^2)*sin((pi*t_time)/(2*T))^2)/T^2);
90 | ddrz(jj,1)=0;
91 |
92 | Q=[eye(n,n), J'; J, zeros(m,m)];
93 | znn_d=[drx(jj,1);dry(jj,1);drz(jj,1)];
94 | znn_u=[-k*(qjj-q0); znn_d];
95 | error_function=Q*uAll(jj,:)'-znn_u;
96 | error(jj)=norm(error_function,2);
97 |
98 |
99 | t(jj)
100 |
101 | end
102 | %--Errors-
103 | erposx=rx-posx;
104 | erposy=ry-posy;
105 | erposz=rz-posz;
106 | averposx=mean(erposx);
107 | averposy=mean(erposy);
108 | averposz=mean(erposz);
109 | erdposx=drx-dposx;
110 | erdposy=dry-dposy;
111 | erdposz=drz-dposz;
112 | erddposx=ddrx-ddposx;
113 | erddposy=ddry-ddposy;
114 | erddposz=ddrz-ddposz;
115 | %??????
116 | rmse_erposx=rms(erposx);
117 | rmse_erposy=rms(erposy);
118 | rmse_erposz=rms(erposz);
119 | rmse_erpos=rms(sqrt(erposx.^2+erposy.^2+erposz.^2))
120 |
121 | save mvn_SRDdata1 t qAll dqAll ddqAll uAll posx posy posz dposx dposy dposz ddposx ddposy ddposz;
122 | save mvn_SRDdata2 erposx erposy erposz erdposx erdposy erdposz erddposx erddposy erddposz error averposx averposy averposz;
123 | save mvn_SRDdata3 j0px j1px j2px j3px j4px j5px j0py j1py j2py j3py j4py j5py j0pz j1pz j2pz j3pz j4pz j5pz rx ry rz;
124 |
125 | %????????????????????
126 | %??????????????
127 | [m,n]=size(qAll);
128 | step_long=0.000005;
129 | jjj=1;
130 | k=1;
131 | q_step(1,:)=qAll(1,:);
132 | for iii=2:m
133 | deta=(posx(iii)-posx(k))*(posx(iii)-posx(k))+(posy(iii)-posy(k))*(posy(iii)-posy(k))+(posz(iii)-posz(k))*(posz(iii)-posz(k));
134 | if deta>step_long||q_Time(iii)==1
135 | jjj=jjj+1;
136 | k=iii;
137 | q_step(jjj,:)=qAll(iii,:);
138 | t_step(jjj)=t(iii);
139 | q_deta(jjj,1)=deta;
140 | q_finish(jjj,1)=q_Time(iii);
141 | end
142 | end
143 | q_deta(1,1)=step_long;
144 | %??????????????Jaco????????????Jaco??????0??????????????????????????????????????????????????????????????????????????
145 | q_tran=[2*pi*ones(jjj,1),(1/2*pi)*ones(jjj,1),(3/2*pi)*ones(jjj,1),0*ones(jjj,1),(pi)*ones(jjj,1),0*ones(jjj,1),0*ones(jjj,1),0*ones(jjj,1)];
146 | q_Real=[-q_step(:,1),q_step(:,2),q_step(:,3),q_step(:,4),q_step(:,5),-q_step(:,6), q_deta, q_finish];
147 | q_Real=q_Real+q_tran;
148 |
149 | save mvn_Datarun q_Real;
150 |
151 |
152 |
--------------------------------------------------------------------------------
/code/EXP2/mvn_ZZJLJrebi1_net.m:
--------------------------------------------------------------------------------
1 | function doty=mvn_ZZJLJrebi1_net(t,y)
2 |
3 | global aa alpha_i a_i d_i D sa ca s2a c2a pen_long q0 T m gamma nu n Time znn_W k tt errorr;
4 |
5 | persistent ix0 iy0 iz0 ix1 iy1 iz1 t_last;
6 |
7 |
8 | q=y(1:n); %??????????
9 | dq=y(n+1:n+n); %????Theta dot??????????????Theta dot
10 | yy=y(n+1:n+n+m);
11 |
12 |
13 | if t==0
14 | [x,y,z]=ZYNposition(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q0); %????????????
15 |
16 | ix0=x(7);iy0=y(7);iz0=z(7);
17 | end
18 |
19 | t_time=t-Time*T; %??????????????????????????????????????????????????????????????????????????????????
20 |
21 | if t_time>T;
22 | Time=Time+1;
23 | t_time=t_time-T;
24 | end
25 |
26 |
27 |
28 | phi_sin=2*pi*sin(0.5*pi*t/T);
29 | phi=phi_sin*sin(0.5*pi*t/T);
30 | phiDot=phi_sin*pi*cos(0.5*pi*t/T)/T;
31 | fac=exp(cos(phi))-2*cos(4*phi)-sin(phi/12)^5;%factor
32 |
33 | rx=aa*sin(phi)*fac;%*T/2/pi;
34 | ry=aa*(cos(phi)*fac);%T/2/pi;
35 | rz=0;
36 | drx=-aa*sin(2*pi*sin((pi*t)/(2*T))^2)*((5*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(6*T) - (16*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(8*pi*sin((pi*t)/(2*T))^2))/T + (2*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T) - (2*pi^2*aa*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T;
37 | dry=(2*pi^2*aa*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T - aa*cos(2*pi*sin((pi*t)/(2*T))^2)*((5*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(6*T) - (16*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(8*pi*sin((pi*t)/(2*T))^2))/T + (2*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T);
38 | drz=0;
39 | ddrx=aa*sin(2*pi*sin((pi*t)/(2*T))^2)*((8*pi^3*cos((pi*t)/(2*T))^2*sin(8*pi*sin((pi*t)/(2*T))^2))/T^2 - (8*pi^3*sin((pi*t)/(2*T))^2*sin(8*pi*sin((pi*t)/(2*T))^2))/T^2 + (5*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*sin((pi*sin((pi*t)/(2*T))^2)/6)^5)/(36*T^2) - (5*pi^3*cos((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(12*T^2) - (pi^3*cos((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T^2 + (5*pi^3*sin((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(12*T^2) + (pi^3*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T^2 + (128*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*cos(8*pi*sin((pi*t)/(2*T))^2))/T^2 - (5*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)^2*sin((pi*sin((pi*t)/(2*T))^2)/6)^3)/(9*T^2) + (4*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2)^2)/T^2 - (4*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*cos(2*pi*sin((pi*t)/(2*T))^2))/T^2) - (pi^3*aa*cos((pi*t)/(2*T))^2*cos(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2 + (pi^3*aa*sin((pi*t)/(2*T))^2*cos(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2 - (4*pi^2*aa*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos(2*pi*sin((pi*t)/(2*T))^2)*((5*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(6*T) - (16*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(8*pi*sin((pi*t)/(2*T))^2))/T + (2*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T))/T + (4*pi^4*aa*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*sin(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2;
40 | ddry=aa*cos(2*pi*sin((pi*t)/(2*T))^2)*((8*pi^3*cos((pi*t)/(2*T))^2*sin(8*pi*sin((pi*t)/(2*T))^2))/T^2 - (8*pi^3*sin((pi*t)/(2*T))^2*sin(8*pi*sin((pi*t)/(2*T))^2))/T^2 + (5*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*sin((pi*sin((pi*t)/(2*T))^2)/6)^5)/(36*T^2) - (5*pi^3*cos((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(12*T^2) - (pi^3*cos((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T^2 + (5*pi^3*sin((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(12*T^2) + (pi^3*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T^2 + (128*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*cos(8*pi*sin((pi*t)/(2*T))^2))/T^2 - (5*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*cos((pi*sin((pi*t)/(2*T))^2)/6)^2*sin((pi*sin((pi*t)/(2*T))^2)/6)^3)/(9*T^2) + (4*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2)^2)/T^2 - (4*pi^4*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*cos(2*pi*sin((pi*t)/(2*T))^2))/T^2) + (pi^3*aa*cos((pi*t)/(2*T))^2*sin(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2 - (pi^3*aa*sin((pi*t)/(2*T))^2*sin(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2 + (4*pi^4*aa*cos((pi*t)/(2*T))^2*sin((pi*t)/(2*T))^2*cos(2*pi*sin((pi*t)/(2*T))^2)*(sin((pi*sin((pi*t)/(2*T))^2)/6)^5 - exp(cos(2*pi*sin(1/2/T*pi*t)^2)) + 2*cos(8*pi*sin((pi*t)/(2*T))^2)))/T^2 + (4*pi^2*aa*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(2*pi*sin((pi*t)/(2*T))^2)*((5*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*cos((pi*sin((pi*t)/(2*T))^2)/6)*sin((pi*sin((pi*t)/(2*T))^2)/6)^4)/(6*T) - (16*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*sin(8*pi*sin((pi*t)/(2*T))^2))/T + (2*pi^2*cos((pi*t)/(2*T))*sin((pi*t)/(2*T))*exp(cos(2*pi*sin((pi*t)/(2*T))^2))*sin(2*pi*sin((pi*t)/(2*T))^2))/T))/T;
41 | ddrz=0;
42 |
43 |
44 |
45 |
46 | dr=[drx;dry;drz]; %????????????????r_dot
47 | ddr=[ddrx;ddry;ddrz];
48 | znn_d=dr;
49 | [J,DJ]=ZYNjdj(alpha_i,a_i,d_i,D,sa,ca,s2a,c2a,pen_long,q,dq);
50 |
51 |
52 | znn_W=eye(n,n);
53 | znn_Q=[znn_W, J'; J, zeros(m,m)];
54 | znn_dotQ=[zeros(n,n), DJ'; DJ, zeros(m,m)];
55 |
56 |
57 | % C=[J,zeros(m,1)];
58 | k=5; %????????????c????????gamma????????0????????????????????????
59 | % zz=k*(q-q0);
60 | znn_q=k*(q-q0); %????????????c
61 | znn_u=[-znn_q; znn_d];
62 | % znn_dotu=[zeros(n,1); ddr];
63 | znn_dotu=[-k*dq; ddr];
64 |
65 |
66 | %%%%%%%%%%%%%%%%%
67 | %VP-CDNN??????????????
68 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFMlinear(znn_Q*yy-znn_u)+znn_dotu;
69 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFpower(znn_Q*yy-znn_u)+znn_dotu;
70 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFsigmoid(znn_Q*yy-znn_u)+znn_dotu;
71 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFsinh(znn_Q*yy-znn_u)+znn_dotu;
72 | % dotyy=-znn_dotQ*yy-gamma*exp(t)*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu;
73 |
74 | %%%%%%%%%%%%%%%%%
75 | %FP-CDNN??????????????
76 | % dotyy=-znn_dotQ*yy-gamma*AFMlinear(znn_Q*yy-znn_u)+znn_dotu;
77 | % dotyy=-znn_dotQ*yy-gamma*AFpower(znn_Q*yy-znn_u)+znn_dotu;
78 | % dotyy=-znn_dotQ*yy-gamma*AFsigmoid(znn_Q*yy-znn_u)+znn_dotu;
79 | % dotyy=-znn_dotQ*yy-gamma*AFsinh(znn_Q*yy-znn_u)+znn_dotu;
80 | % dotyy=-znn_dotQ*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu;
81 |
82 | %%%%%%%%%%%%%%%%%????
83 | delt_D=0.5*[sin(t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(3*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
84 | cos(2*t/T*pi) cos(2*t/T*pi) -cos(4*t/T*pi) sin(t/T*pi) -sin(2*t/T*pi) -cos(3*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
85 | sin(3*t/T*pi) -sin(4*t/T*pi) cos(5*t/T*pi) -cos(3*t/T*pi) cos(4*t/T*pi) -sin((5*t/T*pi)) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
86 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
87 | sin(t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
88 | -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
89 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(2*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi);...
90 | sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(t/T*pi);...
91 | -sin(2*t/T*pi) cos(2*t/T*pi) -sin(t/T*pi) cos(t/T*pi) -sin(t/T*pi) cos(2*t/T*pi) -sin(2*t/T*pi) cos(t/T*pi) -sin(t/T*pi)];
92 | delt_s=1.5*[2*sin(t/T*pi);4*cos(2*t/T*pi);-5*sin(3*t/T*pi); cos(3*t/T*pi); -sin(3*t/T*pi); 3*cos(t/T*pi);-sin(t/T*pi);-sin(2*t/T*pi);cos(t/T*pi)];
93 |
94 | % delt_D=0.01*rand(n+m,n+m);
95 | % delt_s=t*ones(9,1);
96 | % delt_s=[1;2;3;4;5;6;7;8;9];
97 | % delt_s=0.1*rand(n+m,1);
98 | errorr=errorr+(znn_Q*yy-znn_u)*(t-tt);
99 |
100 |
101 | %IE-RNN
102 | dotyy=-(znn_dotQ)*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu-nu*(znn_Q*yy-znn_u)*t+delt_s;
103 |
104 | %Z-RNN
105 | %dotyy=-(znn_dotQ)*yy-gamma*AFMpowersigmoid(znn_Q*yy-znn_u)+znn_dotu+delt_s;
106 |
107 |
108 | t
109 | tt=t;
110 | doty=[dq;dotyy];
111 |
--------------------------------------------------------------------------------
/code/EXP2/mvn_cost_time.txt:
--------------------------------------------------------------------------------
1 | Computing cost time: 2.58228
2 | Computing cost time: 2.81188
3 | Computing cost time: 2.81924
4 | Computing cost time: 1.40161
5 | Computing cost time: 0.976611
6 | Computing cost time: 0.902834
7 | Computing cost time: 0.918307
8 | Computing cost time: 2.51113
9 | Computing cost time: 1.73114
10 | Computing cost time: 2.85684
11 | Computing cost time: 2.27624
12 | Computing cost time: 37.9044
13 | Computing cost time: 1.2007
14 | Computing cost time: 1.22408
15 | Computing cost time: 1.10245
16 | Computing cost time: 1.25779
17 | Computing cost time: 2.09408
18 | Computing cost time: 1.09975
19 | Computing cost time: 1.13851
20 | Computing cost time: 13.1514
21 | Computing cost time: 1.85594
22 | Computing cost time: 1.64796
23 | Computing cost time: 2.63425
24 | Computing cost time: 1.72125
25 | Computing cost time: 0.847284
26 | Computing cost time: 8.65201
27 | Computing cost time: 1.59183
28 | Computing cost time: 1.58087
29 | Computing cost time: 70.3988
30 | Computing cost time: 12.5728
31 | Computing cost time: 20.5479
32 | Computing cost time: 44.1257
33 | Computing cost time: 8.92964
34 | Computing cost time: 26.9609
35 | Computing cost time: 26.8267
36 | Computing cost time: 26.9331
37 | Computing cost time: 26.6534
38 | Computing cost time: 26.8126
39 | Computing cost time: 2.49771
40 | Computing cost time: 2.49152
41 | Computing cost time: 8.07907
42 | Computing cost time: 6.60717
43 | Computing cost time: 27.7292
44 | Computing cost time: 6.70176
45 | Computing cost time: 4.42095
46 | Computing cost time: 13.584
47 | Computing cost time: 22.9602
48 | Computing cost time: 28.2724
49 | Computing cost time: 55.9527
50 | Computing cost time: 43.2806
51 | Computing cost time: 56.1877
52 | Computing cost time: 39.3574
53 | Computing cost time: 10.7696
54 | Computing cost time: 10.0695
55 | Computing cost time: 21.9045
56 | Computing cost time: 9.23245
57 | Computing cost time: 9.39975
58 | Computing cost time: 9.41842
59 | Computing cost time: 9.23738
60 | Computing cost time: 10.4469
61 | Computing cost time: 10.4849
62 | Computing cost time: 10.618
63 | Computing cost time: 11.6035
64 | Computing cost time: 15.0017
65 | Computing cost time: 10.8325
66 | Computing cost time: 31.2564
67 | Computing cost time: 31.0847
68 | Computing cost time: 1.68496
69 | Computing cost time: 9.98491
70 | Computing cost time: 10.0415
71 | Computing cost time: 9.88633
72 | Computing cost time: 7.15985
73 | Computing cost time: 6.95357
74 | Computing cost time: 6.98266
75 | Computing cost time: 7.00731
76 | Computing cost time: 3.76609
77 | Computing cost time: 70.9162
78 | Computing cost time: 7.05737
79 | Computing cost time: 7.01821
80 | Computing cost time: 10.0411
81 | Computing cost time: 7.29782
82 | Computing cost time: 85.9678
83 | Computing cost time: 0.624511
84 | Computing cost time: 0.967238
85 | Computing cost time: 10.7657
86 | Computing cost time: 7.29784
87 | Computing cost time: 11.1091
88 | Computing cost time: 52.5246
89 | Computing cost time: 39.6712
90 | Computing cost time: 17.8789
91 | Computing cost time: 9.19516
92 | Computing cost time: 91.8417
93 | Computing cost time: 24.9205
94 | Computing cost time: 41.6391
95 | Computing cost time: 22.758
96 | Computing cost time: 114.421
97 | Computing cost time: 119.154
98 | Computing cost time: 9.4585
99 | Computing cost time: 52.2214
100 | Computing cost time: 16.1012
101 | Computing cost time: 0.821882
102 | Computing cost time: 0.814161
103 | Computing cost time: 0.80573
104 | Computing cost time: 11.6522
105 | Computing cost time: 1.1778
106 | Computing cost time: 7.12427
107 | Computing cost time: 1.18192
108 | Computing cost time: 1.84064
109 | Computing cost time: 1.83257
110 | Computing cost time: 2.03264
111 | Computing cost time: 1.91256
112 | Computing cost time: 0.727554
113 | Computing cost time: 0.752649
114 | Computing cost time: 1.95893
115 | Computing cost time: 1.89752
116 | Computing cost time: 1.90335
117 | Computing cost time: 2.00704
118 | Computing cost time: 1.96622
119 | Computing cost time: 1.92284
120 | Computing cost time: 1.94461
121 | Computing cost time: 1.9228
122 | Computing cost time: 1.90419
123 | Computing cost time: 1.93737
124 | Computing cost time: 1.92585
125 | Computing cost time: 1.97815
126 | Computing cost time: 0.68005
127 | Computing cost time: 0.649394
128 | Computing cost time: 0.605997
129 | Computing cost time: 0.609774
130 | Computing cost time: 0.665649
131 | Computing cost time: 0.631864
132 | Computing cost time: 0.632492
133 | Computing cost time: 0.635033
134 | Computing cost time: 0.659713
135 | Computing cost time: 0.654461
136 | Computing cost time: 0.615932
137 | Computing cost time: 0.611713
138 | Computing cost time: 0.643992
139 | Computing cost time: 0.657247
140 | Computing cost time: 0.588786
141 | Computing cost time: 0.64172
142 | Computing cost time: 0.649459
143 | Computing cost time: 0.658916
144 | Computing cost time: 0.645283
145 | Computing cost time: 0.648696
146 | Computing cost time: 0.656122
147 | Computing cost time: 0.66415
148 | Computing cost time: 0.662933
149 | Computing cost time: 0.662782
150 | Computing cost time: 0.645593
151 | Computing cost time: 0.660692
152 | Computing cost time: 0.643391
153 | Computing cost time: 0.600247
154 | Computing cost time: 0.603497
155 | Computing cost time: 0.649571
156 | Computing cost time: 0.635468
157 | Computing cost time: 0.673725
158 | Computing cost time: 0.66173
159 | Computing cost time: 0.633327
160 | Computing cost time: 0.728062
161 | Computing cost time: 0.610644
162 | Computing cost time: 0.640305
163 | Computing cost time: 0.621738
164 | Computing cost time: 0.634941
165 | Computing cost time: 0.630007
166 | Computing cost time: 0.637567
167 | Computing cost time: 0.643947
168 | Computing cost time: 0.677306
169 | Computing cost time: 0.764923
170 | Computing cost time: 0.659554
171 | Computing cost time: 0.65137
172 | Computing cost time: 0.669122
173 | Computing cost time: 0.651588
174 | Computing cost time: 0.654613
175 | Computing cost time: 0.652267
176 | Computing cost time: 0.651714
177 | Computing cost time: 0.659696
178 | Computing cost time: 0.658092
179 | Computing cost time: 0.654217
180 | Computing cost time: 0.657328
181 | Computing cost time: 0.652367
182 | Computing cost time: 0.658433
183 | Computing cost time: 0.654309
184 | Computing cost time: 0.670464
185 | Computing cost time: 0.664676
186 | Computing cost time: 0.679763
187 | Computing cost time: 0.672839
188 | Computing cost time: 0.678591
189 | Computing cost time: 0.665606
190 | Computing cost time: 0.656932
191 | Computing cost time: 0.62119
192 | Computing cost time: 0.632732
193 | Computing cost time: 0.610067
194 | Computing cost time: 0.674547
195 | Computing cost time: 0.643745
196 | Computing cost time: 0.639548
197 | Computing cost time: 0.612356
198 | Computing cost time: 0.645648
199 | Computing cost time: 0.620496
200 | Computing cost time: 0.663735
201 | Computing cost time: 0.657216
202 | Computing cost time: 0.66961
203 | Computing cost time: 0.676619
204 | Computing cost time: 0.726365
205 | Computing cost time: 0.71429
206 | Computing cost time: 0.654975
207 | Computing cost time: 0.702527
208 | Computing cost time: 0.653137
209 | Computing cost time: 0.647041
210 | Computing cost time: 0.635178
211 | Computing cost time: 0.65246
212 | Computing cost time: 0.66099
213 | Computing cost time: 0.660628
214 | Computing cost time: 0.68507
215 | Computing cost time: 0.655551
216 | Computing cost time: 0.669636
217 | Computing cost time: 0.687303
218 | Computing cost time: 0.673025
219 | Computing cost time: 0.6606
220 | Computing cost time: 0.679998
221 | Computing cost time: 2.56998
222 | Computing cost time: 13.1686
223 | Computing cost time: 175.191
224 | Computing cost time: 148.519
225 | Computing cost time: 43.1882
226 | Computing cost time: 14.395
227 | Computing cost time: 14.2782
228 | Computing cost time: 13.6366
229 | Computing cost time: 14.5063
230 | Computing cost time: 14.1306
231 | Computing cost time: 18.5557
232 | Computing cost time: 17.9537
233 | Computing cost time: 15.0285
234 | Computing cost time: 15.2195
235 | Computing cost time: 12.9319
236 | Computing cost time: 2.52258
237 | Computing cost time: 2.61241
238 | Computing cost time: 2.74965
239 | Computing cost time: 2.92806
240 | Computing cost time: 2.74063
241 | Computing cost time: 2.75658
242 | Computing cost time: 2.9506
243 | Computing cost time: 2.1843
244 | Computing cost time: 1.8828
245 | Computing cost time: 1.8199
246 | Computing cost time: 2.04072
247 | Computing cost time: 4.23822
248 | Computing cost time: 3.72794
249 | Computing cost time: 24.7193
250 | Computing cost time: 25.6057
251 | Computing cost time: 25.9175
252 | Computing cost time: 22.625
253 | Computing cost time: 19.2882
254 | Computing cost time: 17.7776
255 | Computing cost time: 1.8436
256 | Computing cost time: 1.99432
257 | Computing cost time: 1.22539
258 | Computing cost time: 0.701608
259 | Computing cost time: 1.21095
260 | Computing cost time: 0.757847
261 | Computing cost time: 0.751795
262 | Computing cost time: 0.74226
263 | Computing cost time: 0.704712
264 | Computing cost time: 0.689728
265 | Computing cost time: 0.690065
266 | Computing cost time: 0.739245
267 | Computing cost time: 0.718325
268 | Computing cost time: 0.72618
269 | Computing cost time: 0.701551
270 | Computing cost time: 0.692618
271 | Computing cost time: 0.706898
272 | Computing cost time: 0.70705
273 | Computing cost time: 0.716234
274 | Computing cost time: 0.746034
275 | Computing cost time: 0.688102
276 | Computing cost time: 0.687605
277 | Computing cost time: 0.770117
278 | Computing cost time: 0.725358
279 | Computing cost time: 0.703824
280 | Computing cost time: 0.707067
281 | Computing cost time: 0.758633
282 | Computing cost time: 0.754899
283 | Computing cost time: 0.699512
284 | Computing cost time: 0.71031
285 | Computing cost time: 0.703154
286 | Computing cost time: 0.715391
287 | Computing cost time: 0.866563
288 | Computing cost time: 0.794432
289 | Computing cost time: 0.835624
290 | Computing cost time: 1.02833
291 | Computing cost time: 0.977465
292 | Computing cost time: 0.975445
293 | Computing cost time: 1.09031
294 | Computing cost time: 0.936657
295 | Computing cost time: 0.893604
296 | Computing cost time: 0.958792
297 | Computing cost time: 0.853112
298 | Computing cost time: 0.847164
299 | Computing cost time: 0.85082
300 | Computing cost time: 1.01815
301 | Computing cost time: 1.03509
302 | Computing cost time: 0.958303
303 | Computing cost time: 0.911518
304 | Computing cost time: 0.885078
305 | Computing cost time: 0.845492
306 | Computing cost time: 0.836777
307 | Computing cost time: 0.937811
308 | Computing cost time: 0.908964
309 | Computing cost time: 0.95247
310 | Computing cost time: 0.912741
311 | Computing cost time: 0.940221
312 | Computing cost time: 1.14204
313 | Computing cost time: 1.16231
314 | Computing cost time: 1.12708
315 | Computing cost time: 1.12811
316 | Computing cost time: 1.14726
317 | Computing cost time: 1.12596
318 | Computing cost time: 1.12202
319 | Computing cost time: 1.12013
320 | Computing cost time: 1.13587
321 | Computing cost time: 1.10603
322 | Computing cost time: 1.09562
323 | Computing cost time: 1.13073
324 | Computing cost time: 1.15336
325 | Computing cost time: 1.16272
326 | Computing cost time: 1.16141
327 | Computing cost time: 1.19084
328 | Computing cost time: 1.10999
329 | Computing cost time: 1.21761
330 | Computing cost time: 1.19825
331 | Computing cost time: 1.11408
332 | Computing cost time: 1.15836
333 | Computing cost time: 1.51403
334 | Computing cost time: 1.41573
335 | Computing cost time: 1.20384
336 | Computing cost time: 1.24708
337 | Computing cost time: 1.24785
338 | Computing cost time: 1.62472
339 | Computing cost time: 1.20798
340 | Computing cost time: 1.19829
341 | Computing cost time: 1.6087
342 | Computing cost time: 1.32916
343 | Computing cost time: 1.18103
344 | Computing cost time: 1.20512
345 | Computing cost time: 1.23696
346 | Computing cost time: 1.13881
347 | Computing cost time: 1.21254
348 | Computing cost time: 1.21538
349 | Computing cost time: 1.19739
350 | Computing cost time: 1.12583
351 | Computing cost time: 1.12227
352 | Computing cost time: 1.19752
353 | Computing cost time: 1.12552
354 | Computing cost time: 1.23983
355 | Computing cost time: 1.13452
356 | Computing cost time: 1.23057
357 | Computing cost time: 1.14012
358 | Computing cost time: 0.555746
359 | Computing cost time: 0.839524
360 | Computing cost time: 1.22315
361 | Computing cost time: 2.22931
362 | Computing cost time: 14.0946
363 | Computing cost time: 0.944567
364 | Computing cost time: 0.478174
365 | Computing cost time: 0.451262
366 | Computing cost time: 1.39
367 | Computing cost time: 5.89359
368 | Computing cost time: 1.20718
369 | Computing cost time: 1.16846
370 | Computing cost time: 1.16544
371 | Computing cost time: 1.16734
372 | Computing cost time: 1.25472
373 | Computing cost time: 1.12547
374 | Computing cost time: 1.17023
375 | Computing cost time: 1.18333
376 | Computing cost time: 1.0947
377 | Computing cost time: 1.09725
378 | Computing cost time: 1.15871
379 | Computing cost time: 1.09895
380 | Computing cost time: 1.10257
381 | Computing cost time: 1.23545
382 | Computing cost time: 1.22882
383 | Computing cost time: 1.12615
384 | Computing cost time: 1.2021
385 | Computing cost time: 1.7466
386 | Computing cost time: 1.17294
387 | Computing cost time: 2.43876
388 | Computing cost time: 7.20839
389 | Computing cost time: 9.29293
390 | Computing cost time: 1.21764
391 | Computing cost time: 1.22919
392 | Computing cost time: 1.23834
393 | Computing cost time: 1.24854
394 | Computing cost time: 1.24247
395 | Computing cost time: 1.69337
396 | Computing cost time: 1.14644
397 | Computing cost time: 1.61525
398 | Computing cost time: 1.21008
399 | Computing cost time: 1.27499
400 | Computing cost time: 1.29611
401 | Computing cost time: 1.17515
402 | Computing cost time: 1.40772
403 | Computing cost time: 1.48659
404 | Computing cost time: 1.60326
405 | Computing cost time: 1.73925
406 | Computing cost time: 2.92124
407 | Computing cost time: 1.27779
408 | Computing cost time: 1.51543
409 | Computing cost time: 1.62108
410 | Computing cost time: 25.4271
411 | Computing cost time: 22.023
412 | Computing cost time: 1.57961
413 | Computing cost time: 1.54225
414 | Computing cost time: 1.58396
415 | Computing cost time: 1.24947
416 | Computing cost time: 2.60142
417 | Computing cost time: 2.99727
418 | Computing cost time: 3.25447
419 | Computing cost time: 3.92029
420 | Computing cost time: 4.35636
421 | Computing cost time: 7.76471
422 | Computing cost time: 6.20182
423 | Computing cost time: 5.43813
424 | Computing cost time: 4.90486
425 | Computing cost time: 4.3807
426 | Computing cost time: 4.37728
427 | Computing cost time: 3.48404
428 | Computing cost time: 3.45316
429 | Computing cost time: 1.5387
430 | Computing cost time: 1.87387
431 | Computing cost time: 1.58962
432 | Computing cost time: 7.98971
433 | Computing cost time: 6.96039
434 | Computing cost time: 1.46354
435 | Computing cost time: 1.49272
436 | Computing cost time: 1.157
437 | Computing cost time: 1.16304
438 | Computing cost time: 1.54362
439 | Computing cost time: 1.54733
440 | Computing cost time: 1.50641
441 | Computing cost time: 2.86265
442 | Computing cost time: 1.51028
443 | Computing cost time: 1.66141
444 | Computing cost time: 2.06665
445 | Computing cost time: 1.57461
446 | Computing cost time: 1.57875
447 | Computing cost time: 1.65644
448 | Computing cost time: 1.53191
449 | Computing cost time: 1.50223
450 | Computing cost time: 1.53297
451 | Computing cost time: 1.72676
452 | Computing cost time: 1.62496
453 | Computing cost time: 1.45113
454 | Computing cost time: 1.46155
455 | Computing cost time: 1.57479
456 | Computing cost time: 1.75886
457 | Computing cost time: 1.59179
458 | Computing cost time: 1.55869
459 | Computing cost time: 1.22902
460 | Computing cost time: 1.30329
461 | Computing cost time: 1.58717
462 | Computing cost time: 1.60134
463 | Computing cost time: 1.66098
464 | Computing cost time: 1.34885
465 | Computing cost time: 1.52454
466 | Computing cost time: 1.53998
467 | Computing cost time: 1.1665
468 | Computing cost time: 1.17336
469 | Computing cost time: 1.17159
470 | Computing cost time: 1.66702
471 | Computing cost time: 1.95719
472 | Computing cost time: 1.10196
473 | Computing cost time: 1.44593
474 | Computing cost time: 1.30717
475 | Computing cost time: 1.44858
476 | Computing cost time: 1.55246
477 | Computing cost time: 1.1339
478 | Computing cost time: 1.16142
479 | Computing cost time: 1.57428
480 | Computing cost time: 1.43465
481 | Computing cost time: 1.46381
482 | Computing cost time: 2.89581
483 | Computing cost time: 1.45766
484 | Computing cost time: 1.49321
485 | Computing cost time: 1.4755
486 | Computing cost time: 1.96035
487 | Computing cost time: 1.42875
488 | Computing cost time: 1.48527
489 | Computing cost time: 1.14531
490 | Computing cost time: 1.11154
491 | Computing cost time: 1.14125
492 | Computing cost time: 1.44816
493 | Computing cost time: 1.44652
494 | Computing cost time: 1.21946
495 | Computing cost time: 1.48297
496 | Computing cost time: 60.485
497 | Computing cost time: 67.092
498 | Computing cost time: 63.4648
499 | Computing cost time: 8.59781
500 | Computing cost time: 7.48871
501 | Computing cost time: 11.0572
502 | Computing cost time: 8.32469
503 | Computing cost time: 9.64022
504 | Computing cost time: 9.44165
505 | Computing cost time: 8.81665
506 | Computing cost time: 9.26553
507 | Computing cost time: 9.03461
508 | Computing cost time: 10.2142
509 | Computing cost time: 9.84316
510 | Computing cost time: 9.8669
511 | Computing cost time: 8.70405
512 | Computing cost time: 8.92125
513 | Computing cost time: 10.9923
514 | Computing cost time: 8.64259
515 | Computing cost time: 9.58476
516 | Computing cost time: 5.96651
517 | Computing cost time: 12.0321
518 | Computing cost time: 10.9498
519 | Computing cost time: 10.8217
520 | Computing cost time: 12.4049
521 | Computing cost time: 14.2206
522 | Computing cost time: 7.4719
523 | Computing cost time: 6.27989
524 | Computing cost time: 6.68067
525 | Computing cost time: 6.09361
526 | Computing cost time: 5.97005
527 | Computing cost time: 10.9314
528 | Computing cost time: 6.66781
529 | Computing cost time: 6.43642
530 | Computing cost time: 10.6239
531 | Computing cost time: 10.4858
532 | Computing cost time: 6.56671
533 | Computing cost time: 6.17236
534 | Computing cost time: 6.35384
535 | Computing cost time: 5.71774
536 | Computing cost time: 10.3561
537 | Computing cost time: 11.1352
538 | Computing cost time: 8.06223
539 | Computing cost time: 6.32238
540 | Computing cost time: 5.85852
541 | Computing cost time: 4.96732
542 | Computing cost time: 9.53311
543 | Computing cost time: 9.25971
544 | Computing cost time: 5.96334
545 | Computing cost time: 5.54668
546 | Computing cost time: 1.82312
547 | Computing cost time: 1.79703
548 | Computing cost time: 1.88794
549 | Computing cost time: 1.99789
550 | Computing cost time: 2.06263
551 | Computing cost time: 2.09761
552 | Computing cost time: 2.0983
553 | Computing cost time: 2.3336
554 | Computing cost time: 1.77218
555 | Computing cost time: 2.42622
556 | Computing cost time: 3.87818
557 | Computing cost time: 2.00638
558 | Computing cost time: 2.03487
559 | Computing cost time: 1.79484
560 | Computing cost time: 1.87243
561 |
--------------------------------------------------------------------------------
/code/EXP1/mvn_cost_time.txt:
--------------------------------------------------------------------------------
1 | Computing cost time: 2.58228
2 | Computing cost time: 2.81188
3 | Computing cost time: 2.81924
4 | Computing cost time: 1.40161
5 | Computing cost time: 0.976611
6 | Computing cost time: 0.902834
7 | Computing cost time: 0.918307
8 | Computing cost time: 2.51113
9 | Computing cost time: 1.73114
10 | Computing cost time: 2.85684
11 | Computing cost time: 2.27624
12 | Computing cost time: 37.9044
13 | Computing cost time: 1.2007
14 | Computing cost time: 1.22408
15 | Computing cost time: 1.10245
16 | Computing cost time: 1.25779
17 | Computing cost time: 2.09408
18 | Computing cost time: 1.09975
19 | Computing cost time: 1.13851
20 | Computing cost time: 13.1514
21 | Computing cost time: 1.85594
22 | Computing cost time: 1.64796
23 | Computing cost time: 2.63425
24 | Computing cost time: 1.72125
25 | Computing cost time: 0.847284
26 | Computing cost time: 8.65201
27 | Computing cost time: 1.59183
28 | Computing cost time: 1.58087
29 | Computing cost time: 70.3988
30 | Computing cost time: 12.5728
31 | Computing cost time: 20.5479
32 | Computing cost time: 44.1257
33 | Computing cost time: 8.92964
34 | Computing cost time: 26.9609
35 | Computing cost time: 26.8267
36 | Computing cost time: 26.9331
37 | Computing cost time: 26.6534
38 | Computing cost time: 26.8126
39 | Computing cost time: 2.49771
40 | Computing cost time: 2.49152
41 | Computing cost time: 8.07907
42 | Computing cost time: 6.60717
43 | Computing cost time: 27.7292
44 | Computing cost time: 6.70176
45 | Computing cost time: 4.42095
46 | Computing cost time: 13.584
47 | Computing cost time: 22.9602
48 | Computing cost time: 28.2724
49 | Computing cost time: 55.9527
50 | Computing cost time: 43.2806
51 | Computing cost time: 56.1877
52 | Computing cost time: 39.3574
53 | Computing cost time: 10.7696
54 | Computing cost time: 10.0695
55 | Computing cost time: 21.9045
56 | Computing cost time: 9.23245
57 | Computing cost time: 9.39975
58 | Computing cost time: 9.41842
59 | Computing cost time: 9.23738
60 | Computing cost time: 10.4469
61 | Computing cost time: 10.4849
62 | Computing cost time: 10.618
63 | Computing cost time: 11.6035
64 | Computing cost time: 15.0017
65 | Computing cost time: 10.8325
66 | Computing cost time: 31.2564
67 | Computing cost time: 31.0847
68 | Computing cost time: 1.68496
69 | Computing cost time: 9.98491
70 | Computing cost time: 10.0415
71 | Computing cost time: 9.88633
72 | Computing cost time: 7.15985
73 | Computing cost time: 6.95357
74 | Computing cost time: 6.98266
75 | Computing cost time: 7.00731
76 | Computing cost time: 3.76609
77 | Computing cost time: 70.9162
78 | Computing cost time: 7.05737
79 | Computing cost time: 7.01821
80 | Computing cost time: 10.0411
81 | Computing cost time: 7.29782
82 | Computing cost time: 85.9678
83 | Computing cost time: 0.624511
84 | Computing cost time: 0.967238
85 | Computing cost time: 10.7657
86 | Computing cost time: 7.29784
87 | Computing cost time: 11.1091
88 | Computing cost time: 52.5246
89 | Computing cost time: 39.6712
90 | Computing cost time: 17.8789
91 | Computing cost time: 9.19516
92 | Computing cost time: 91.8417
93 | Computing cost time: 24.9205
94 | Computing cost time: 41.6391
95 | Computing cost time: 22.758
96 | Computing cost time: 114.421
97 | Computing cost time: 119.154
98 | Computing cost time: 9.4585
99 | Computing cost time: 52.2214
100 | Computing cost time: 16.1012
101 | Computing cost time: 0.821882
102 | Computing cost time: 0.814161
103 | Computing cost time: 0.80573
104 | Computing cost time: 11.6522
105 | Computing cost time: 1.1778
106 | Computing cost time: 7.12427
107 | Computing cost time: 1.18192
108 | Computing cost time: 1.84064
109 | Computing cost time: 1.83257
110 | Computing cost time: 2.03264
111 | Computing cost time: 1.91256
112 | Computing cost time: 0.727554
113 | Computing cost time: 0.752649
114 | Computing cost time: 1.95893
115 | Computing cost time: 1.89752
116 | Computing cost time: 1.90335
117 | Computing cost time: 2.00704
118 | Computing cost time: 1.96622
119 | Computing cost time: 1.92284
120 | Computing cost time: 1.94461
121 | Computing cost time: 1.9228
122 | Computing cost time: 1.90419
123 | Computing cost time: 1.93737
124 | Computing cost time: 1.92585
125 | Computing cost time: 1.97815
126 | Computing cost time: 0.68005
127 | Computing cost time: 0.649394
128 | Computing cost time: 0.605997
129 | Computing cost time: 0.609774
130 | Computing cost time: 0.665649
131 | Computing cost time: 0.631864
132 | Computing cost time: 0.632492
133 | Computing cost time: 0.635033
134 | Computing cost time: 0.659713
135 | Computing cost time: 0.654461
136 | Computing cost time: 0.615932
137 | Computing cost time: 0.611713
138 | Computing cost time: 0.643992
139 | Computing cost time: 0.657247
140 | Computing cost time: 0.588786
141 | Computing cost time: 0.64172
142 | Computing cost time: 0.649459
143 | Computing cost time: 0.658916
144 | Computing cost time: 0.645283
145 | Computing cost time: 0.648696
146 | Computing cost time: 0.656122
147 | Computing cost time: 0.66415
148 | Computing cost time: 0.662933
149 | Computing cost time: 0.662782
150 | Computing cost time: 0.645593
151 | Computing cost time: 0.660692
152 | Computing cost time: 0.643391
153 | Computing cost time: 0.600247
154 | Computing cost time: 0.603497
155 | Computing cost time: 0.649571
156 | Computing cost time: 0.635468
157 | Computing cost time: 0.673725
158 | Computing cost time: 0.66173
159 | Computing cost time: 0.633327
160 | Computing cost time: 0.728062
161 | Computing cost time: 0.610644
162 | Computing cost time: 0.640305
163 | Computing cost time: 0.621738
164 | Computing cost time: 0.634941
165 | Computing cost time: 0.630007
166 | Computing cost time: 0.637567
167 | Computing cost time: 0.643947
168 | Computing cost time: 0.677306
169 | Computing cost time: 0.764923
170 | Computing cost time: 0.659554
171 | Computing cost time: 0.65137
172 | Computing cost time: 0.669122
173 | Computing cost time: 0.651588
174 | Computing cost time: 0.654613
175 | Computing cost time: 0.652267
176 | Computing cost time: 0.651714
177 | Computing cost time: 0.659696
178 | Computing cost time: 0.658092
179 | Computing cost time: 0.654217
180 | Computing cost time: 0.657328
181 | Computing cost time: 0.652367
182 | Computing cost time: 0.658433
183 | Computing cost time: 0.654309
184 | Computing cost time: 0.670464
185 | Computing cost time: 0.664676
186 | Computing cost time: 0.679763
187 | Computing cost time: 0.672839
188 | Computing cost time: 0.678591
189 | Computing cost time: 0.665606
190 | Computing cost time: 0.656932
191 | Computing cost time: 0.62119
192 | Computing cost time: 0.632732
193 | Computing cost time: 0.610067
194 | Computing cost time: 0.674547
195 | Computing cost time: 0.643745
196 | Computing cost time: 0.639548
197 | Computing cost time: 0.612356
198 | Computing cost time: 0.645648
199 | Computing cost time: 0.620496
200 | Computing cost time: 0.663735
201 | Computing cost time: 0.657216
202 | Computing cost time: 0.66961
203 | Computing cost time: 0.676619
204 | Computing cost time: 0.726365
205 | Computing cost time: 0.71429
206 | Computing cost time: 0.654975
207 | Computing cost time: 0.702527
208 | Computing cost time: 0.653137
209 | Computing cost time: 0.647041
210 | Computing cost time: 0.635178
211 | Computing cost time: 0.65246
212 | Computing cost time: 0.66099
213 | Computing cost time: 0.660628
214 | Computing cost time: 0.68507
215 | Computing cost time: 0.655551
216 | Computing cost time: 0.669636
217 | Computing cost time: 0.687303
218 | Computing cost time: 0.673025
219 | Computing cost time: 0.6606
220 | Computing cost time: 0.679998
221 | Computing cost time: 2.56998
222 | Computing cost time: 13.1686
223 | Computing cost time: 175.191
224 | Computing cost time: 148.519
225 | Computing cost time: 43.1882
226 | Computing cost time: 14.395
227 | Computing cost time: 14.2782
228 | Computing cost time: 13.6366
229 | Computing cost time: 14.5063
230 | Computing cost time: 14.1306
231 | Computing cost time: 18.5557
232 | Computing cost time: 17.9537
233 | Computing cost time: 15.0285
234 | Computing cost time: 15.2195
235 | Computing cost time: 12.9319
236 | Computing cost time: 2.52258
237 | Computing cost time: 2.61241
238 | Computing cost time: 2.74965
239 | Computing cost time: 2.92806
240 | Computing cost time: 2.74063
241 | Computing cost time: 2.75658
242 | Computing cost time: 2.9506
243 | Computing cost time: 2.1843
244 | Computing cost time: 1.8828
245 | Computing cost time: 1.8199
246 | Computing cost time: 2.04072
247 | Computing cost time: 4.23822
248 | Computing cost time: 3.72794
249 | Computing cost time: 24.7193
250 | Computing cost time: 25.6057
251 | Computing cost time: 25.9175
252 | Computing cost time: 22.625
253 | Computing cost time: 19.2882
254 | Computing cost time: 17.7776
255 | Computing cost time: 1.8436
256 | Computing cost time: 1.99432
257 | Computing cost time: 1.22539
258 | Computing cost time: 0.701608
259 | Computing cost time: 1.21095
260 | Computing cost time: 0.757847
261 | Computing cost time: 0.751795
262 | Computing cost time: 0.74226
263 | Computing cost time: 0.704712
264 | Computing cost time: 0.689728
265 | Computing cost time: 0.690065
266 | Computing cost time: 0.739245
267 | Computing cost time: 0.718325
268 | Computing cost time: 0.72618
269 | Computing cost time: 0.701551
270 | Computing cost time: 0.692618
271 | Computing cost time: 0.706898
272 | Computing cost time: 0.70705
273 | Computing cost time: 0.716234
274 | Computing cost time: 0.746034
275 | Computing cost time: 0.688102
276 | Computing cost time: 0.687605
277 | Computing cost time: 0.770117
278 | Computing cost time: 0.725358
279 | Computing cost time: 0.703824
280 | Computing cost time: 0.707067
281 | Computing cost time: 0.758633
282 | Computing cost time: 0.754899
283 | Computing cost time: 0.699512
284 | Computing cost time: 0.71031
285 | Computing cost time: 0.703154
286 | Computing cost time: 0.715391
287 | Computing cost time: 0.866563
288 | Computing cost time: 0.794432
289 | Computing cost time: 0.835624
290 | Computing cost time: 1.02833
291 | Computing cost time: 0.977465
292 | Computing cost time: 0.975445
293 | Computing cost time: 1.09031
294 | Computing cost time: 0.936657
295 | Computing cost time: 0.893604
296 | Computing cost time: 0.958792
297 | Computing cost time: 0.853112
298 | Computing cost time: 0.847164
299 | Computing cost time: 0.85082
300 | Computing cost time: 1.01815
301 | Computing cost time: 1.03509
302 | Computing cost time: 0.958303
303 | Computing cost time: 0.911518
304 | Computing cost time: 0.885078
305 | Computing cost time: 0.845492
306 | Computing cost time: 0.836777
307 | Computing cost time: 0.937811
308 | Computing cost time: 0.908964
309 | Computing cost time: 0.95247
310 | Computing cost time: 0.912741
311 | Computing cost time: 0.940221
312 | Computing cost time: 1.14204
313 | Computing cost time: 1.16231
314 | Computing cost time: 1.12708
315 | Computing cost time: 1.12811
316 | Computing cost time: 1.14726
317 | Computing cost time: 1.12596
318 | Computing cost time: 1.12202
319 | Computing cost time: 1.12013
320 | Computing cost time: 1.13587
321 | Computing cost time: 1.10603
322 | Computing cost time: 1.09562
323 | Computing cost time: 1.13073
324 | Computing cost time: 1.15336
325 | Computing cost time: 1.16272
326 | Computing cost time: 1.16141
327 | Computing cost time: 1.19084
328 | Computing cost time: 1.10999
329 | Computing cost time: 1.21761
330 | Computing cost time: 1.19825
331 | Computing cost time: 1.11408
332 | Computing cost time: 1.15836
333 | Computing cost time: 1.51403
334 | Computing cost time: 1.41573
335 | Computing cost time: 1.20384
336 | Computing cost time: 1.24708
337 | Computing cost time: 1.24785
338 | Computing cost time: 1.62472
339 | Computing cost time: 1.20798
340 | Computing cost time: 1.19829
341 | Computing cost time: 1.6087
342 | Computing cost time: 1.32916
343 | Computing cost time: 1.18103
344 | Computing cost time: 1.20512
345 | Computing cost time: 1.23696
346 | Computing cost time: 1.13881
347 | Computing cost time: 1.21254
348 | Computing cost time: 1.21538
349 | Computing cost time: 1.19739
350 | Computing cost time: 1.12583
351 | Computing cost time: 1.12227
352 | Computing cost time: 1.19752
353 | Computing cost time: 1.12552
354 | Computing cost time: 1.23983
355 | Computing cost time: 1.13452
356 | Computing cost time: 1.23057
357 | Computing cost time: 1.14012
358 | Computing cost time: 0.555746
359 | Computing cost time: 0.839524
360 | Computing cost time: 1.22315
361 | Computing cost time: 2.22931
362 | Computing cost time: 14.0946
363 | Computing cost time: 0.944567
364 | Computing cost time: 0.478174
365 | Computing cost time: 0.451262
366 | Computing cost time: 1.39
367 | Computing cost time: 5.89359
368 | Computing cost time: 1.20718
369 | Computing cost time: 1.16846
370 | Computing cost time: 1.16544
371 | Computing cost time: 1.16734
372 | Computing cost time: 1.25472
373 | Computing cost time: 1.12547
374 | Computing cost time: 1.17023
375 | Computing cost time: 1.18333
376 | Computing cost time: 1.0947
377 | Computing cost time: 1.09725
378 | Computing cost time: 1.15871
379 | Computing cost time: 1.09895
380 | Computing cost time: 1.10257
381 | Computing cost time: 1.23545
382 | Computing cost time: 1.22882
383 | Computing cost time: 1.12615
384 | Computing cost time: 1.2021
385 | Computing cost time: 1.7466
386 | Computing cost time: 1.17294
387 | Computing cost time: 2.43876
388 | Computing cost time: 7.20839
389 | Computing cost time: 9.29293
390 | Computing cost time: 1.21764
391 | Computing cost time: 1.22919
392 | Computing cost time: 1.23834
393 | Computing cost time: 1.24854
394 | Computing cost time: 1.24247
395 | Computing cost time: 1.69337
396 | Computing cost time: 1.14644
397 | Computing cost time: 1.61525
398 | Computing cost time: 1.21008
399 | Computing cost time: 1.27499
400 | Computing cost time: 1.29611
401 | Computing cost time: 1.17515
402 | Computing cost time: 1.40772
403 | Computing cost time: 1.48659
404 | Computing cost time: 1.60326
405 | Computing cost time: 1.73925
406 | Computing cost time: 2.92124
407 | Computing cost time: 1.27779
408 | Computing cost time: 1.51543
409 | Computing cost time: 1.62108
410 | Computing cost time: 25.4271
411 | Computing cost time: 22.023
412 | Computing cost time: 1.57961
413 | Computing cost time: 1.54225
414 | Computing cost time: 1.58396
415 | Computing cost time: 1.24947
416 | Computing cost time: 2.60142
417 | Computing cost time: 2.99727
418 | Computing cost time: 3.25447
419 | Computing cost time: 3.92029
420 | Computing cost time: 4.35636
421 | Computing cost time: 7.76471
422 | Computing cost time: 6.20182
423 | Computing cost time: 5.43813
424 | Computing cost time: 4.90486
425 | Computing cost time: 4.3807
426 | Computing cost time: 4.37728
427 | Computing cost time: 3.48404
428 | Computing cost time: 3.45316
429 | Computing cost time: 1.5387
430 | Computing cost time: 1.87387
431 | Computing cost time: 1.58962
432 | Computing cost time: 7.98971
433 | Computing cost time: 6.96039
434 | Computing cost time: 1.46354
435 | Computing cost time: 1.49272
436 | Computing cost time: 1.157
437 | Computing cost time: 1.16304
438 | Computing cost time: 1.54362
439 | Computing cost time: 1.54733
440 | Computing cost time: 1.50641
441 | Computing cost time: 2.86265
442 | Computing cost time: 1.51028
443 | Computing cost time: 1.66141
444 | Computing cost time: 2.06665
445 | Computing cost time: 1.57461
446 | Computing cost time: 1.57875
447 | Computing cost time: 1.65644
448 | Computing cost time: 1.53191
449 | Computing cost time: 1.50223
450 | Computing cost time: 1.53297
451 | Computing cost time: 1.72676
452 | Computing cost time: 1.62496
453 | Computing cost time: 1.45113
454 | Computing cost time: 1.46155
455 | Computing cost time: 1.57479
456 | Computing cost time: 1.75886
457 | Computing cost time: 1.59179
458 | Computing cost time: 1.55869
459 | Computing cost time: 1.22902
460 | Computing cost time: 1.30329
461 | Computing cost time: 1.58717
462 | Computing cost time: 1.60134
463 | Computing cost time: 1.66098
464 | Computing cost time: 1.34885
465 | Computing cost time: 1.52454
466 | Computing cost time: 1.53998
467 | Computing cost time: 1.1665
468 | Computing cost time: 1.17336
469 | Computing cost time: 1.17159
470 | Computing cost time: 1.66702
471 | Computing cost time: 1.95719
472 | Computing cost time: 1.10196
473 | Computing cost time: 1.44593
474 | Computing cost time: 1.30717
475 | Computing cost time: 1.44858
476 | Computing cost time: 1.55246
477 | Computing cost time: 1.1339
478 | Computing cost time: 1.16142
479 | Computing cost time: 1.57428
480 | Computing cost time: 1.43465
481 | Computing cost time: 1.46381
482 | Computing cost time: 2.89581
483 | Computing cost time: 1.45766
484 | Computing cost time: 1.14377
485 | Computing cost time: 1.554
486 | Computing cost time: 1.42274
487 | Computing cost time: 2.16311
488 | Computing cost time: 1.17306
489 | Computing cost time: 1.51313
490 | Computing cost time: 1.27683
491 | Computing cost time: 1.20757
492 | Computing cost time: 1.47558
493 | Computing cost time: 1.47623
494 | Computing cost time: 1.28322
495 | Computing cost time: 1.24342
496 | Computing cost time: 1.58323
497 | Computing cost time: 1.57629
498 | Computing cost time: 1.24249
499 | Computing cost time: 1.21814
500 | Computing cost time: 1.61037
501 | Computing cost time: 1.62165
502 | Computing cost time: 1.21604
503 | Computing cost time: 2.19372
504 | Computing cost time: 2.17701
505 | Computing cost time: 1.36868
506 | Computing cost time: 1.41642
507 | Computing cost time: 1.40877
508 | Computing cost time: 1.5265
509 | Computing cost time: 1.53335
510 | Computing cost time: 1.26089
511 | Computing cost time: 1.28726
512 | Computing cost time: 2.12264
513 | Computing cost time: 42.762
514 | Computing cost time: 1.71441
515 | Computing cost time: 2.06211
516 | Computing cost time: 1.72566
517 | Computing cost time: 2.08268
518 | Computing cost time: 1.73662
519 | Computing cost time: 2.18326
520 | Computing cost time: 1.4402
521 | Computing cost time: 1.77882
522 | Computing cost time: 1.86067
523 | Computing cost time: 1.80358
524 | Computing cost time: 1.77137
525 | Computing cost time: 1.82109
526 | Computing cost time: 1.82347
527 | Computing cost time: 1.7893
528 | Computing cost time: 1.7501
529 | Computing cost time: 1.43955
530 | Computing cost time: 1.70947
531 | Computing cost time: 1.5089
532 | Computing cost time: 1.34774
533 | Computing cost time: 1.3791
534 | Computing cost time: 1.35393
535 | Computing cost time: 1.33896
536 | Computing cost time: 1.22679
537 | Computing cost time: 1.19458
538 | Computing cost time: 1.23612
539 | Computing cost time: 1.29324
540 | Computing cost time: 1.22286
541 | Computing cost time: 1.46872
542 | Computing cost time: 1.21965
543 | Computing cost time: 1.268
544 | Computing cost time: 1.2914
545 | Computing cost time: 1.26327
546 | Computing cost time: 1.24246
547 | Computing cost time: 1.23546
548 | Computing cost time: 1.25401
549 | Computing cost time: 1.26276
550 | Computing cost time: 1.21696
551 | Computing cost time: 1.27216
552 | Computing cost time: 1.30303
553 | Computing cost time: 1.25629
554 | Computing cost time: 1.24907
555 | Computing cost time: 1.25913
556 | Computing cost time: 1.28223
557 | Computing cost time: 1.24711
558 | Computing cost time: 1.34012
559 | Computing cost time: 1.25926
560 | Computing cost time: 1.22503
561 | Computing cost time: 1.2201
562 | Computing cost time: 1.2302
563 | Computing cost time: 1.22715
564 | Computing cost time: 1.21379
565 | Computing cost time: 1.26293
566 | Computing cost time: 1.27723
567 | Computing cost time: 1.30032
568 | Computing cost time: 1.30607
569 | Computing cost time: 1.44133
570 | Computing cost time: 2.31138
571 | Computing cost time: 1.22099
572 | Computing cost time: 0.970218
573 | Computing cost time: 0.960468
574 | Computing cost time: 1.24474
575 | Computing cost time: 1.24113
576 | Computing cost time: 1.29179
577 | Computing cost time: 1.40426
578 | Computing cost time: 0.917882
579 | Computing cost time: 0.957823
580 | Computing cost time: 1.25773
581 | Computing cost time: 2.04876
582 | Computing cost time: 1.07924
583 | Computing cost time: 1.96012
584 | Computing cost time: 0.857072
585 | Computing cost time: 1.23139
586 | Computing cost time: 1.20964
587 | Computing cost time: 1.21738
588 | Computing cost time: 1.21571
589 | Computing cost time: 0.933133
590 | Computing cost time: 0.943673
591 | Computing cost time: 0.921328
592 | Computing cost time: 1.26097
593 | Computing cost time: 1.26974
594 | Computing cost time: 1.05986
595 | Computing cost time: 1.04538
596 | Computing cost time: 1.09756
597 | Computing cost time: 1.04235
598 | Computing cost time: 1.33675
599 | Computing cost time: 1.32984
600 | Computing cost time: 1.9637
601 | Computing cost time: 0.910194
602 | Computing cost time: 1.12862
603 | Computing cost time: 1.22827
604 | Computing cost time: 0.933731
605 | Computing cost time: 1.24457
606 | Computing cost time: 78.7484
607 | Computing cost time: 48.1965
608 | Computing cost time: 65.2899
609 | Computing cost time: 56.6402
610 | Computing cost time: 66.9947
611 | Computing cost time: 7.51206
612 | Computing cost time: 5.91211
613 | Computing cost time: 4.98997
614 | Computing cost time: 5.46813
615 | Computing cost time: 4.36012
616 | Computing cost time: 6.24109
617 | Computing cost time: 5.22936
618 | Computing cost time: 4.66592
619 | Computing cost time: 6.06893
620 | Computing cost time: 5.13203
621 | Computing cost time: 3.70602
622 | Computing cost time: 2.84284
623 | Computing cost time: 2.61716
624 | Computing cost time: 2.47008
625 | Computing cost time: 3.26593
626 | Computing cost time: 2.00939
627 | Computing cost time: 1.90216
628 | Computing cost time: 1.96927
629 | Computing cost time: 3.40391
630 | Computing cost time: 4.45079
631 | Computing cost time: 2.58114
632 | Computing cost time: 4.00185
633 | Computing cost time: 2.53598
634 | Computing cost time: 4.87574
635 | Computing cost time: 4.28926
636 | Computing cost time: 2.44443
637 | Computing cost time: 2.20664
638 | Computing cost time: 2.95636
639 | Computing cost time: 2.40663
640 | Computing cost time: 4.81946
641 | Computing cost time: 6.5588
642 | Computing cost time: 2.68265
643 | Computing cost time: 2.30372
644 | Computing cost time: 4.35859
645 | Computing cost time: 5.82956
646 | Computing cost time: 4.57274
647 | Computing cost time: 2.29868
648 | Computing cost time: 4.35979
649 | Computing cost time: 4.01569
650 | Computing cost time: 5.52538
651 | Computing cost time: 4.43267
652 | Computing cost time: 2.63396
653 | Computing cost time: 2.71809
654 | Computing cost time: 3.25943
655 | Computing cost time: 5.26274
656 | Computing cost time: 1.56747
657 | Computing cost time: 0.888072
658 | Computing cost time: 0.780459
659 | Computing cost time: 0.740005
660 | Computing cost time: 0.688374
661 | Computing cost time: 0.758778
662 | Computing cost time: 67.7566
663 | Computing cost time: 0.86753
664 | Computing cost time: 0.713013
665 | Computing cost time: 0.806331
666 | Computing cost time: 0.664341
667 | Computing cost time: 1.09702
668 | Computing cost time: 0.866201
669 | Computing cost time: 0.919881
670 | Computing cost time: 0.896299
671 | Computing cost time: 0.946603
672 | Computing cost time: 1.08308
673 | Computing cost time: 0.685402
674 | Computing cost time: 0.599801
675 | Computing cost time: 0.646578
676 | Computing cost time: 0.894527
677 | Computing cost time: 0.898489
678 | Computing cost time: 1.3104
679 | Computing cost time: 1.30515
680 | Computing cost time: 0.8177
681 | Computing cost time: 0.801823
682 | Computing cost time: 0.727349
683 | Computing cost time: 0.69675
684 | Computing cost time: 0.925482
685 | Computing cost time: 0.93286
686 | Computing cost time: 0.806984
687 | Computing cost time: 1.00585
688 | Computing cost time: 0.691997
689 | Computing cost time: 0.835401
690 | Computing cost time: 1.10038
691 | Computing cost time: 0.844156
692 | Computing cost time: 0.781567
693 | Computing cost time: 1.48079
694 | Computing cost time: 1.03968
695 | Computing cost time: 0.93649
696 | Computing cost time: 0.810683
697 | Computing cost time: 1.27875
698 |
--------------------------------------------------------------------------------