├── 2Dletters ├── A.mat ├── B.mat ├── C.mat ├── D.mat ├── E.mat ├── F.mat ├── G.mat ├── H.mat ├── I.mat ├── J.mat ├── K.mat ├── L.mat ├── M.mat ├── N.mat ├── O.mat ├── P.mat ├── Q.mat ├── R.mat ├── S.mat ├── T.mat ├── U.mat ├── V.mat ├── W.mat ├── X.mat ├── Y.mat └── Z.mat ├── demos ├── demo_KMP01.m ├── demo_KMP02.m ├── demo_KMP_orientation.m ├── demo_KMP_uncertainty.m └── myColors.m ├── fcts ├── EM_GMM.m ├── GMR.m ├── gaussPDF.m ├── generate_orientation_data.m ├── init_GMM_timeBased.m ├── kernel_extend.m ├── kmp_estimateMatrix_mean.m ├── kmp_estimateMatrix_mean_var.m ├── kmp_insertPoint.m ├── kmp_pred_mean.m ├── kmp_pred_mean_var.m ├── minimum_jerk.m ├── minimum_jerk_spline.m ├── plotGMM.m ├── quat_conjugate.m ├── quat_exp.m ├── quat_log.m ├── quat_mult.m ├── quat_norm.m ├── quat_to_vel.m └── trans_angVel.m ├── images ├── kmp_adaptationB.png ├── kmp_adaptationG.png ├── kmp_orientation_ada.png ├── kmp_orientation_demos.png ├── kmp_uncertainty.png ├── modelLetterB.png └── modelLetterG.png ├── octave-jupyter-notebooks ├── README.md ├── demo_KMP01.ipynb ├── demo_KMP02.ipynb └── myColors.m └── readMe.md /2Dletters/A.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/A.mat -------------------------------------------------------------------------------- /2Dletters/B.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/B.mat -------------------------------------------------------------------------------- /2Dletters/C.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/C.mat -------------------------------------------------------------------------------- /2Dletters/D.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/D.mat -------------------------------------------------------------------------------- /2Dletters/E.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/E.mat -------------------------------------------------------------------------------- /2Dletters/F.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/F.mat -------------------------------------------------------------------------------- /2Dletters/G.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/G.mat -------------------------------------------------------------------------------- /2Dletters/H.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/H.mat -------------------------------------------------------------------------------- /2Dletters/I.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/I.mat -------------------------------------------------------------------------------- /2Dletters/J.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/J.mat -------------------------------------------------------------------------------- /2Dletters/K.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/K.mat -------------------------------------------------------------------------------- /2Dletters/L.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/L.mat -------------------------------------------------------------------------------- /2Dletters/M.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/M.mat -------------------------------------------------------------------------------- /2Dletters/N.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/N.mat -------------------------------------------------------------------------------- /2Dletters/O.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/O.mat -------------------------------------------------------------------------------- /2Dletters/P.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/P.mat -------------------------------------------------------------------------------- /2Dletters/Q.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/Q.mat -------------------------------------------------------------------------------- /2Dletters/R.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/R.mat -------------------------------------------------------------------------------- /2Dletters/S.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/S.mat -------------------------------------------------------------------------------- /2Dletters/T.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/T.mat -------------------------------------------------------------------------------- /2Dletters/U.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/U.mat -------------------------------------------------------------------------------- /2Dletters/V.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/V.mat -------------------------------------------------------------------------------- /2Dletters/W.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/W.mat -------------------------------------------------------------------------------- /2Dletters/X.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/X.mat -------------------------------------------------------------------------------- /2Dletters/Y.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/Y.mat -------------------------------------------------------------------------------- /2Dletters/Z.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/2Dletters/Z.mat -------------------------------------------------------------------------------- /demos/demo_KMP01.m: -------------------------------------------------------------------------------- 1 | %-----------function demo_KMP01---------- 2 | % This file provide a simple demo of using kmp, where trajectory adaptations towards 3 | % various desired points in terms of positions and velocities are studied. 4 | % 5 | % This code is written by Dr. Yanlong Huang 6 | 7 | % @InProceedings{Huang19IJRR, 8 | % Title = {Kernelized Movement Primitives}, 9 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 10 | % Booktitle = {International Journal of Robotics Research}, 11 | % Year= {2019}, 12 | % pages= {833--852} 13 | % } 14 | 15 | % @InProceedings{Huang19ICRA_1, 16 | % Title = {Non-parametric Imitation Learning of Robot Motor Skills}, 17 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 18 | % Booktitle = {Proc. {International Conference on Robotics and Automation ({ICRA})}, 19 | % Year = {2019}, 20 | % Address = {Montreal, Canada}, 21 | % Month = {May}, 22 | % pages = {5266--5272} 23 | % } 24 | 25 | %% 26 | clear; close all; 27 | myColors; 28 | addpath('../fcts/'); 29 | 30 | %% Extract position and velocity from demos 31 | load('../2Dletters/G.mat'); 32 | demoNum=5; % number of demos 33 | demo_dt=0.01; % time interval of data 34 | demoLen=size(demos{1}.pos,2); % size of each demo; 35 | demo_dura=demoLen*demo_dt; % time length of each demo 36 | dim=2; % dimension of demo 37 | 38 | totalNum=0; 39 | for i=1:demoNum 40 | for j=1:demoLen 41 | totalNum=totalNum+1; 42 | Data(1,totalNum)=j*demo_dt; 43 | Data(2:dim+1,totalNum)=demos{i}.pos(1:dim,j); 44 | end 45 | lowIndex=(i-1)*demoLen+1; 46 | upIndex=i*demoLen; 47 | for k=1:dim 48 | Data(dim+1+k,lowIndex:upIndex)=gradient(Data(1+k,lowIndex:upIndex))/demo_dt; 49 | end 50 | end 51 | 52 | %% Extract the reference trajectory 53 | model.nbStates = 8; % Number of states in the GMM 54 | model.nbVar =1+2*dim; % Number of variables [t,x1,x2,.. vx1,vx2...] 55 | model.dt = 0.005; % Time step duration 56 | nbData = demo_dura/model.dt; % Length of each trajectory 57 | 58 | model = init_GMM_timeBased(Data, model); 59 | model = EM_GMM(Data, model); 60 | [DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:model.nbVar); %see Eq. (17)-(19) 61 | 62 | for i=1:nbData 63 | refTraj(i).t=i*model.dt; 64 | refTraj(i).mu=DataOut(:,i); 65 | refTraj(i).sigma=SigmaOut(:,:,i); 66 | end 67 | 68 | %% Set kmp parameters 69 | dt=0.005; 70 | len=demo_dura/dt; 71 | lamda=1; % control mean prediction 72 | kh=6; 73 | 74 | %% Set desired points 75 | viaFlag=[1 1 1 1]; % determine which via-points are used 76 | 77 | viaNum=4; 78 | via_time(1)=dt; 79 | via_point(:,1)=[8 10 -50 0]'; % format:[2D-pos 2D-vel] 80 | via_time(2)=0.25; 81 | via_point(:,2)=[-1 6 -25 -40]'; 82 | via_time(3)=1.2; 83 | via_point(:,3)=[8 -4 30 10]'; 84 | via_time(4)=2; 85 | via_point(:,4)=[-3 1 -10 3]'; 86 | 87 | via_var=1E-6*eye(4); % adaptation precision 88 | % via_var(3,3)=1000;via_var(4,4)=1000; % low adaptation precision for velocity 89 | 90 | %% Update the reference trajectory using desired points 91 | newRef=refTraj; 92 | newLen=len; 93 | for viaIndex=1:viaNum 94 | if viaFlag(viaIndex)==1 95 | [newRef,newLen] = kmp_insertPoint(newRef,newLen,via_time(viaIndex),via_point(:,viaIndex),via_var); 96 | end 97 | end 98 | 99 | %% Prediction using kmp 100 | Kinv = kmp_estimateMatrix_mean(newRef,newLen,kh,lamda,dim); 101 | 102 | for index=1:len 103 | t=index*dt; 104 | mu=kmp_pred_mean(t,newRef,newLen,kh,Kinv,dim); 105 | kmpPredTraj(index).t=index*dt; 106 | kmpPredTraj(index).mu=mu; 107 | end 108 | 109 | for i=1:len 110 | gmr(:,i)=refTraj(i).mu; % format:[2D-pos 2D-vel] 111 | kmp(:,i)=kmpPredTraj(i).mu; % format:[2D-pos 2D-vel] 112 | end 113 | 114 | %% Show demonstrations and the corresponding reference trajectory 115 | figure 116 | set(gcf,'position',[468,875,1914,401]) 117 | %% show demonstrations 118 | for i=1:demoNum 119 | subplot(2,3,1) 120 | hold on 121 | plot([demo_dt:demo_dt:demo_dura],Data(2,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 122 | box on 123 | ylabel('x [cm]','interpreter','tex'); 124 | set(gca,'xtick',[0 1 2]) 125 | set(gca,'ytick',[-10 0 10]) 126 | set(gca,'FontSize',12) 127 | grid on 128 | set(gca,'gridlinestyle','--') 129 | ax=gca; 130 | ax.GridAlpha=0.3; 131 | 132 | subplot(2,3,4) 133 | hold on 134 | plot([demo_dt:demo_dt:demo_dura],Data(3,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 135 | box on 136 | xlabel('t [s]','interpreter','tex'); 137 | ylabel('y [cm]','interpreter','tex'); 138 | set(gca,'xtick',[0 1 2]) 139 | set(gca,'ytick',[-10 0 10]) 140 | set(gca,'FontSize',12) 141 | grid on 142 | set(gca,'gridlinestyle','--') 143 | ax=gca; 144 | ax.GridAlpha=0.3; 145 | end 146 | 147 | %% show GMM 148 | subplot(2,3,[2 5]) 149 | for i=1:demoNum 150 | hold on 151 | plot(Data(2,(i-1)*demoLen+1:i*demoLen),Data(3,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 152 | end 153 | hold on 154 | for i=1:demoLen:demoLen*demoNum 155 | hold on 156 | plot(Data(2,i),Data(3,i),'*','markersize',12,'color',mycolors.g); 157 | end 158 | for i=demoLen:demoLen:demoLen*demoNum 159 | hold on 160 | plot(Data(2,i),Data(3,i),'+','markersize',12,'color',mycolors.g); 161 | end 162 | hold on 163 | plotGMM(model.Mu(2:3,:), model.Sigma(2:3,2:3,:), [.8 0 0], .5); 164 | box on 165 | grid on 166 | set(gca,'gridlinestyle','--') 167 | xlabel('x [cm]','interpreter','tex'); 168 | ylabel('y [cm]','interpreter','tex'); 169 | set(gca,'xtick',[-10 0 10]) 170 | set(gca,'ytick',[-10 0 10]) 171 | set(gca,'FontSize',12) 172 | ax=gca; 173 | ax.GridAlpha=0.3; 174 | 175 | %% show reference trajectory 176 | subplot(2,3,[3 6]) 177 | hold on 178 | plotGMM(DataOut(1:2,:), SigmaOut(1:2,1:2,:), mycolors.g, .025); 179 | hold on 180 | plot(DataOut(1,:),DataOut(2,:),'color',mycolors.g,'linewidth',3.0); 181 | hold on 182 | plot(DataOut(1,1),DataOut(2,1),'*','markersize',15,'color',mycolors.g) 183 | hold on 184 | plot(DataOut(1,end),DataOut(2,end),'+','markersize',15,'color',mycolors.g) 185 | box on 186 | xlim([-10.5 10.5]) 187 | xlabel('x [cm]','interpreter','tex'); 188 | ylabel('y [cm]','interpreter','tex'); 189 | set(gca,'xtick',[-10 0 10]) 190 | set(gca,'ytick',[-10 0 10]) 191 | set(gca,'FontSize',12) 192 | grid on 193 | set(gca,'gridlinestyle','--') 194 | ax=gca; 195 | ax.GridAlpha=0.3; 196 | 197 | %% Show kmp predictions 198 | value=[0.5 0 0.5]; 199 | curveValue=mycolors.o; 200 | 201 | figure('units','normalized','outerposition',[0 0 1 1]) 202 | set(gcf, 'Position', [0.0465 0.1794 0.9535 0.2394]) 203 | 204 | %% plot px-py 205 | subplot(1,5,1) 206 | hold on 207 | plot(gmr(1,:),gmr(2,:),'--','color',mycolors.gy,'linewidth',1.5); 208 | hold on 209 | plot(kmp(1,:),kmp(2,:),'color',curveValue,'linewidth',2); 210 | hold on 211 | plot(gmr(1,1),gmr(2,1),'*','markersize',12,'color',mycolors.gy); 212 | hold on 213 | plot(gmr(1,end),gmr(2,end),'+','markersize',12,'color',mycolors.gy); 214 | 215 | for viaIndex=1:viaNum 216 | if viaFlag(viaIndex)==1 217 | plot(via_point(1,viaIndex),via_point(2,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 218 | end 219 | end 220 | 221 | box on 222 | xlim([-12 12]) 223 | ylim([-12 12]) 224 | xlabel('${x}$ [cm]','interpreter','latex'); 225 | ylabel('${y}$ [cm]','interpreter','latex'); 226 | set(gca,'xtick',[-12 0 12]); 227 | set(gca,'ytick',[-12 0 12]); 228 | set(gca,'FontSize',17) 229 | grid on 230 | set(gca,'gridlinestyle','--') 231 | ax=gca; 232 | ax.GridAlpha=0.3; 233 | 234 | %% plot t-px, t-py, t-vx and t-vy 235 | for plotIndex=1:4 236 | subplot(1,5,1+plotIndex) 237 | hold on 238 | plot([dt:dt:dt*len],gmr(plotIndex,:),'--','color',mycolors.gy,'linewidth',1.5) 239 | hold on 240 | plot([dt:dt:dt*len],kmp(plotIndex,:),'color',curveValue,'linewidth',2.0) 241 | 242 | for viaIndex=1:viaNum 243 | if viaFlag(viaIndex)==1 244 | plot(via_time(viaIndex),via_point(plotIndex,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 245 | end 246 | end 247 | 248 | box on 249 | xlim([0 len*dt]) 250 | if plotIndex==1 || plotIndex==2 251 | if plotIndex==1 252 | ylabel('${x}$ [cm]','interpreter','latex'); 253 | end 254 | if plotIndex==2 255 | ylabel('${y}$ [cm]','interpreter','latex'); 256 | end 257 | ylim([-12 12]) 258 | set(gca,'ytick',[-12 0 12]); 259 | end 260 | if plotIndex==3 || plotIndex==4 261 | if plotIndex==3 262 | ylabel('$\dot{x}$ [cm/s]','interpreter','latex'); 263 | end 264 | if plotIndex==4 265 | ylabel('$\dot{y}$ [cm/s]','interpreter','latex'); 266 | end 267 | ylim([-80 80]) 268 | set(gca,'ytick',[-80 0 80]); 269 | end 270 | 271 | xlabel('$t$ [s]','interpreter','latex'); 272 | set(gca,'xtick',[0 1 2]); 273 | 274 | set(gca,'FontSize',17) 275 | grid on 276 | set(gca,'gridlinestyle','--') 277 | ax=gca; 278 | ax.GridAlpha=0.3; 279 | end 280 | 281 | -------------------------------------------------------------------------------- /demos/demo_KMP02.m: -------------------------------------------------------------------------------- 1 | %-----------function demo_KMP02---------- 2 | % This file provide a simple demo of using kmp, where trajectory adaptations towards 3 | % various desired points in terms of positions and velocities are studied. 4 | % 5 | % This code is written by Dr. Yanlong Huang 6 | 7 | % @InProceedings{Huang19IJRR, 8 | % Title = {Kernelized Movement Primitives}, 9 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 10 | % Booktitle = {International Journal of Robotics Research}, 11 | % Year= {2019}, 12 | % pages= {833--852} 13 | % } 14 | 15 | % @InProceedings{Huang19ICRA_1, 16 | % Title = {Non-parametric Imitation Learning of Robot Motor Skills}, 17 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 18 | % Booktitle = {Proc. {International Conference on Robotics and Automation ({ICRA})}, 19 | % Year = {2019}, 20 | % Address = {Montreal, Canada}, 21 | % Month = {May}, 22 | % pages = {5266--5272} 23 | % } 24 | 25 | %% 26 | clear; close all; 27 | myColors; 28 | addpath('../fcts/'); 29 | 30 | %% Extract position and velocity from demos 31 | load('../2Dletters/B.mat'); 32 | demoNum=5; % number of demos 33 | demo_dt=0.01; % time interval of data 34 | demoLen=size(demos{1}.pos,2); % size of each demo; 35 | demo_dura=demoLen*demo_dt; % time length of each demo 36 | dim=2; % dimension of demo 37 | 38 | totalNum=0; 39 | for i=1:demoNum 40 | for j=1:demoLen 41 | totalNum=totalNum+1; 42 | Data(1,totalNum)=j*demo_dt; 43 | Data(2:dim+1,totalNum)=demos{i}.pos(1:dim,j); 44 | end 45 | lowIndex=(i-1)*demoLen+1; 46 | upIndex=i*demoLen; 47 | for k=1:dim 48 | Data(dim+1+k,lowIndex:upIndex)=gradient(Data(1+k,lowIndex:upIndex))/demo_dt; 49 | end 50 | end 51 | 52 | %% Extract the reference trajectory 53 | model.nbStates = 8; % Number of states in the GMM 54 | model.nbVar =1+2*dim; % Number of variables [t,x1,x2,.. vx1,vx2...] 55 | model.dt = 0.005; % Time step duration 56 | nbData = demo_dura/model.dt; % Length of each trajectory 57 | 58 | model = init_GMM_timeBased(Data, model); 59 | model = EM_GMM(Data, model); 60 | [DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:model.nbVar); %see Eq. (17)-(19) 61 | 62 | for i=1:nbData 63 | refTraj(i).t=i*model.dt; 64 | refTraj(i).mu=DataOut(:,i); 65 | refTraj(i).sigma=SigmaOut(:,:,i); 66 | end 67 | 68 | %% Set kmp parameters 69 | dt=0.005; 70 | len=demo_dura/dt; 71 | lamda=1; % control mean prediction 72 | lamdac=60;% control variance prediction 73 | kh=6; 74 | 75 | %% Set desired points 76 | viaFlag=[1 1 1]; % determine which via-points are used 77 | 78 | viaNum=3; 79 | via_time(1)=dt; 80 | via_point(:,1)=[-12 -12 0 0]'; % format:[2D-pos 2D-vel] 81 | via_time(2)=1; 82 | via_point(:,2)=[0 -1 0 0]'; 83 | via_time(3)=2; 84 | via_point(:,3)=[-14 -8 0 0]'; 85 | 86 | via_var=1E-6*eye(4); % adaptation precision 87 | via_var(3,3)=1000;via_var(4,4)=1000; % low adaptation precision for velocity 88 | 89 | %% Update the reference trajectory using desired points 90 | newRef=refTraj; 91 | newLen=len; 92 | for viaIndex=1:viaNum 93 | if viaFlag(viaIndex)==1 94 | [newRef,newLen] = kmp_insertPoint(newRef,newLen,via_time(viaIndex),via_point(:,viaIndex),via_var); 95 | end 96 | end 97 | 98 | %% Prediction using kmp 99 | [Kinv1, Kinv2] = kmp_estimateMatrix_mean_var(newRef,newLen,kh,lamda,lamdac,dim); 100 | 101 | for index=1:len 102 | t=index*dt; 103 | [mu sigma]=kmp_pred_mean_var(t,newRef,newLen,kh,Kinv1,Kinv2,lamdac,dim); 104 | kmpPredTraj(index).t=index*dt; 105 | kmpPredTraj(index).mu=mu; 106 | kmpPredTraj(index).sigma=sigma; 107 | end 108 | 109 | for i=1:len 110 | gmr(:,i)=refTraj(i).mu; % format:[2D-pos 2D-vel] 111 | kmp(:,i)=kmpPredTraj(i).mu; % format:[2D-pos 2D-vel] 112 | for h=1:2*dim 113 | gmrVar(h,i)=refTraj(i).sigma(h,h); 114 | gmrVar(h,i)=sqrt(gmrVar(h,i)); 115 | 116 | kmpVar(h,i)=kmpPredTraj(i).sigma(h,h); 117 | kmpVar(h,i)=sqrt(kmpVar(h,i)); 118 | end 119 | 120 | SigmaOut_kmp(:,:,i)=kmpPredTraj(i).sigma; 121 | SigmaOut_gmr(:,:,i)=refTraj(i).sigma; 122 | end 123 | 124 | %% Show demonstrations and the corresponding reference trajectory 125 | figure 126 | set(gcf,'position',[468,875,1914,401]) 127 | %% show demonstrations 128 | for i=1:demoNum 129 | subplot(2,3,1) 130 | hold on 131 | plot([demo_dt:demo_dt:demo_dura],Data(2,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 132 | box on 133 | ylabel('x [cm]','interpreter','tex'); 134 | set(gca,'xtick',[0 1 2]) 135 | set(gca,'ytick',[-10 0 10]) 136 | set(gca,'FontSize',12) 137 | grid on 138 | set(gca,'gridlinestyle','--') 139 | ax=gca; 140 | ax.GridAlpha=0.3; 141 | 142 | subplot(2,3,4) 143 | hold on 144 | plot([demo_dt:demo_dt:demo_dura],Data(3,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 145 | box on 146 | xlabel('t [s]','interpreter','tex'); 147 | ylabel('y [cm]','interpreter','tex'); 148 | set(gca,'xtick',[0 1 2]) 149 | set(gca,'ytick',[-10 0 10]) 150 | set(gca,'FontSize',12) 151 | grid on 152 | set(gca,'gridlinestyle','--') 153 | ax=gca; 154 | ax.GridAlpha=0.3; 155 | end 156 | 157 | %% show GMM 158 | subplot(2,3,[2 5]) 159 | for i=1:demoNum 160 | hold on 161 | plot(Data(2,(i-1)*demoLen+1:i*demoLen),Data(3,(i-1)*demoLen+1:i*demoLen),'linewidth',3,'color',mycolors.g); 162 | end 163 | hold on 164 | for i=1:demoLen:demoLen*demoNum 165 | hold on 166 | plot(Data(2,i),Data(3,i),'*','markersize',12,'color',mycolors.g); 167 | end 168 | for i=demoLen:demoLen:demoLen*demoNum 169 | hold on 170 | plot(Data(2,i),Data(3,i),'+','markersize',12,'color',mycolors.g); 171 | end 172 | hold on 173 | plotGMM(model.Mu(2:3,:), model.Sigma(2:3,2:3,:), [.8 0 0], .5); 174 | box on 175 | grid on 176 | set(gca,'gridlinestyle','--') 177 | xlabel('x [cm]','interpreter','tex'); 178 | ylabel('y [cm]','interpreter','tex'); 179 | set(gca,'xtick',[-10 0 10]) 180 | set(gca,'ytick',[-10 0 10]) 181 | set(gca,'FontSize',12) 182 | ax=gca; 183 | ax.GridAlpha=0.3; 184 | 185 | %% show reference trajectory 186 | subplot(2,3,[3 6]) 187 | hold on 188 | plotGMM(DataOut(1:2,:), SigmaOut(1:2,1:2,:), mycolors.g, .025); 189 | hold on 190 | plot(DataOut(1,:),DataOut(2,:),'color',mycolors.g,'linewidth',3.0); 191 | hold on 192 | plot(DataOut(1,1),DataOut(2,1),'*','markersize',15,'color',mycolors.g) 193 | hold on 194 | plot(DataOut(1,end),DataOut(2,end),'+','markersize',15,'color',mycolors.g) 195 | box on 196 | xlim([-10.5 10.5]) 197 | xlabel('x [cm]','interpreter','tex'); 198 | ylabel('y [cm]','interpreter','tex'); 199 | set(gca,'xtick',[-10 0 10]) 200 | set(gca,'ytick',[-10 0 10]) 201 | set(gca,'FontSize',12) 202 | grid on 203 | set(gca,'gridlinestyle','--') 204 | ax=gca; 205 | ax.GridAlpha=0.3; 206 | 207 | %% Show kmp predictions 208 | value=[0.5 0 0.5]; 209 | curveValue=mycolors.o; 210 | 211 | figure('units','normalized','outerposition',[0 0 1 1]) 212 | set(gcf, 'Position', [0.1597 0.1311 0.6733 0.2561]) 213 | 214 | %% plot px-py 215 | subplot(1,3,1) 216 | plotGMM(gmr(1:2,:), SigmaOut_gmr(1:2,1:2,:), mycolors.gy, .01); 217 | hold on 218 | plotGMM(kmp(1:2,:), SigmaOut_kmp(1:2,1:2,:), curveValue, .03); 219 | hold on 220 | plot(gmr(1,:),gmr(2,:),'--','color',mycolors.gy,'linewidth',1.5); 221 | hold on 222 | plot(kmp(1,:),kmp(2,:),'color',curveValue,'linewidth',2); 223 | hold on 224 | plot(gmr(1,1),gmr(2,1),'*','markersize',12,'color',mycolors.gy); 225 | hold on 226 | plot(gmr(1,end),gmr(2,end),'+','markersize',12,'color',mycolors.gy); 227 | 228 | for viaIndex=1:viaNum 229 | if viaFlag(viaIndex)==1 230 | plot(via_point(1,viaIndex),via_point(2,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 231 | end 232 | end 233 | 234 | box on 235 | xlim([-15 15]) 236 | ylim([-15 15]) 237 | xlabel('${x}$ [cm]','interpreter','latex'); 238 | ylabel('${y}$ [cm]','interpreter','latex'); 239 | set(gca,'xtick',[-10 0 10]); 240 | set(gca,'ytick',[-10 0 10]); 241 | set(gca,'FontSize',17) 242 | grid on 243 | set(gca,'gridlinestyle','--') 244 | ax=gca; 245 | ax.GridAlpha=0.3; 246 | 247 | %% plot t-px and t-py 248 | shadowT=dt:dt:dt*len; 249 | shadow_time=[shadowT,fliplr(shadowT)]; 250 | 251 | for plotIndex=1:2 252 | subplot(1,3,1+plotIndex) 253 | 254 | shadowUpGmr=gmr(plotIndex,:)+gmrVar(plotIndex,:); 255 | shadowLowGmr=gmr(plotIndex,:)-gmrVar(plotIndex,:); 256 | shadow_gmr=[shadowUpGmr,fliplr(shadowLowGmr)]; 257 | shadowUpKmp=kmp(plotIndex,:)+kmpVar(plotIndex,:); 258 | shadowLowKmp=kmp(plotIndex,:)-kmpVar(plotIndex,:); 259 | shadow_kmp=[shadowUpKmp,fliplr(shadowLowKmp)]; 260 | 261 | fill(shadow_time,shadow_gmr',mycolors.gy,'facealpha',.2,'edgecolor','none') 262 | hold on 263 | fill(shadow_time,shadow_kmp',curveValue,'facealpha',.3,'edgecolor','none') 264 | hold on 265 | plot([dt:dt:dt*len],gmr(plotIndex,:),'--','color',mycolors.gy,'linewidth',1.5) 266 | hold on 267 | plot([dt:dt:dt*len],kmp(plotIndex,:),'color',curveValue,'linewidth',2.0) 268 | 269 | for viaIndex=1:viaNum 270 | if viaFlag(viaIndex)==1 271 | plot(via_time(viaIndex),via_point(plotIndex,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 272 | end 273 | end 274 | 275 | box on 276 | xlim([0 len*dt]) 277 | if plotIndex==1 || plotIndex==2 278 | if plotIndex==1 279 | ylabel('${x}$ [cm]','interpreter','latex'); 280 | end 281 | if plotIndex==2 282 | ylabel('${y}$ [cm]','interpreter','latex'); 283 | end 284 | ylim([-15 15]) 285 | set(gca,'ytick',[-10 0 10]); 286 | end 287 | 288 | xlabel('$t$ [s]','interpreter','latex'); 289 | set(gca,'xtick',[0 1 2]); 290 | set(gca,'FontSize',17) 291 | grid on 292 | set(gca,'gridlinestyle','--') 293 | ax=gca; 294 | ax.GridAlpha=0.3; 295 | end 296 | 297 | -------------------------------------------------------------------------------- /demos/demo_KMP_orientation.m: -------------------------------------------------------------------------------- 1 | %-----------demo_KMP_orientation---------- 2 | % This file provide a simple demo of using orientation kmp, where orientation adaptations towards 3 | % various desired points in terms of quaternions and angular velocities are studied. 4 | % 5 | % This code is written by Dr. Yanlong Huang 6 | 7 | % @InProceedings{Huang19ICRA_2, 8 | % Title = {Generalized Orientation Learning in Robot Task Space}, 9 | % Author = {Huang, Y. and Abu-Dakka, F. and Silv\'erio, J. and Caldwell, D. G.}, 10 | % Booktitle = {Proc. {International Conference on Robotics and Automation ({ICRA})}, 11 | % Year = {2019}, 12 | % Address = {Montreal, Canada}, 13 | % Month = {May}, 14 | % pages = {2531--2537} 15 | % } 16 | 17 | % @InProceedings{Huang19IJRR, 18 | % Title = {Kernelized Movement Primitives}, 19 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 20 | % Booktitle = {International Journal of Robotics Research}, 21 | % Year= {2019}, 22 | % pages= {833--852} 23 | % } 24 | 25 | % @InProceedings{Huang19ICRA_1, 26 | % Title = {Non-parametric Imitation Learning of Robot Motor Skills}, 27 | % Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 28 | % Booktitle = {Proc. {International Conference on Robotics and Automation ({ICRA})}, 29 | % Year = {2019}, 30 | % Address = {Montreal, Canada}, 31 | % Month = {May}, 32 | % pages = {5266--5272} 33 | % } 34 | 35 | %% 36 | clear; close all; 37 | myColors; 38 | addpath('../fcts/'); 39 | 40 | %% Generate synthetic demos(quaternions) 41 | tau=10; 42 | dt=0.01; 43 | len=round(tau / dt); 44 | 45 | tau1=0.4*tau; 46 | tau2=tau-tau1; 47 | N1=tau1/dt; 48 | N2=len-N1; 49 | demosAll=[]; % save demos 50 | demoNum=5; 51 | for num=1:demoNum 52 | a=[1 1.2 1 1.5]; 53 | b=[2 3.4 1.5 4]+[0.5 -0.1 1 -0.2]*num/5*0.5; 54 | c=[4 3 2.5 3]+[0.5 -0.0 -0.1 -1.5]*num/5*0.2; 55 | q1_ini=quatnormalize(a); 56 | q2_mid=quatnormalize(b); 57 | q1.s=q1_ini(1); q1.v=q1_ini(2:4)'; 58 | q2.s=q2_mid(1); q2.v=q2_mid(2:4)'; 59 | q3_end=quatnormalize(c); 60 | q3.s=q3_end(1); q3.v=q3_end(2:4)'; 61 | 62 | [q, omega, domega, t] = generate_orientation_data(q1, q2, tau1, dt); % generate quaternion data (first part) 63 | for i=1:N1 64 | demo(1,i) = i*dt; 65 | demo(2,i) = q(i).s; 66 | demo(3:5,i)= q(i).v; 67 | demo(6:8,i)= omega(:,i); 68 | end 69 | [q, omega, domega, t] = generate_orientation_data(q2, q3, tau2, dt); % generate quaternion data (second part) 70 | for i=1:N2 71 | demo(1,i+N1) = i*dt+N1*dt; 72 | demo(2,i+N1) = q(i).s; 73 | demo(3:5,i+N1)= q(i).v; 74 | demo(6:8,i+N1)= omega(:,i); 75 | end 76 | demosAll=[demosAll demo]; 77 | end 78 | 79 | %% Project demos into Euclidean space and model new trajectories using GMM/GMR 80 | Data=[]; % save transformed data 81 | for i=1:demoNum 82 | for j=1:len 83 | 84 | qindex=(i-1)*len+j; 85 | time=demosAll(1,qindex); 86 | qtemp.s=demosAll(2,qindex); 87 | qtemp.v=demosAll(3:5,qindex); 88 | 89 | zeta(1,j)=time; 90 | zeta(2:4,j)=quat_log(qtemp, q1); %q1 serves as the auxiliary quaternion 91 | end 92 | 93 | for h=1:3 94 | zeta(4+h,:)=gradient(zeta(h+1,:))/dt; 95 | end 96 | 97 | Data=[Data zeta]; 98 | end 99 | 100 | model.nbStates = 5; %Number of states in the GMM 101 | model.nbVar =7; %Number of variables [t,wx,wy,wz,ax,ay,az] 102 | model.dt = 0.01; %Time step duration 103 | nbData = 1000; %Length of each trajectory 104 | 105 | model = init_GMM_timeBased(Data, model); 106 | model = EM_GMM(Data, model); 107 | [DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:model.nbVar); 108 | 109 | for i=1:nbData 110 | quatRef(i).t=i*model.dt; 111 | quatRef(i).mu=DataOut(:,i); 112 | quatRef(i).sigma=SigmaOut(:,:,i); 113 | end 114 | 115 | %% Set kmp parameters 116 | lamda=1; 117 | kh=0.01; 118 | dim=3; 119 | 120 | %% Set desired quaternion and angular velocity 121 | viaFlag=[1 1 1]; % specify desired point 122 | % viaFlag=[0 0 0]; % reproduction 123 | 124 | via_time(1)=0; % desired time 125 | qDes_temp(:,1)=q1_ini; 126 | qDes1.s=qDes_temp(1,1); qDes1.v=qDes_temp(2:4,1); % desired quaternion 127 | dqDes(:,1)=[0.0 0.0 0.0]'; % desire angular velocity 128 | 129 | via_time(2)=4.0; 130 | des_temp2=[0.2 0.6 0.4 0.8]; 131 | qDes_temp(:,2)=quatnormalize(des_temp2); 132 | qDes2.s=qDes_temp(1,2); qDes2.v=qDes_temp(2:4,2); 133 | dqDes(:,2)=[0.1 0.1 0.0]'; 134 | 135 | via_time(3)=10.0; 136 | des_temp3=[0.6 0.4 0.6 0.4]; 137 | qDes_temp(:,3)=quatnormalize(des_temp3); 138 | qDes3.s=qDes_temp(1,3); qDes3.v=qDes_temp(2:4,3); 139 | dqDes(:,3)=[0 0.3 0.3]'; 140 | 141 | %% Transform desired points into Euclidean space 142 | via_point(1:3,1)=quat_log(qDes1, q1); 143 | via_point(4:6,1)=trans_angVel(qDes1, dqDes(:,1), dt, q1); 144 | via_point(1:3,2)=quat_log(qDes2, q1); 145 | via_point(4:6,2)=trans_angVel(qDes2, dqDes(:,2), dt, q1); 146 | via_point(1:3,3)=quat_log(qDes3, q1); 147 | via_point(4:6,3)=trans_angVel(qDes3, dqDes(:,3), dt, q1); 148 | 149 | via_var=1E-10*eye(6); 150 | % via_var(4,4)=1000;via_var(5,5)=1000;via_var(6,6)=1000; 151 | 152 | %% Update the reference trajectory using transformed desired points 153 | interval=5; % speed up the computation 154 | num=round(len/interval)+1; 155 | for i=1:num 156 | if i==1 index=1; 157 | else index=(i-1)*interval; 158 | end 159 | sampleData(i).t=quatRef(index).t; 160 | sampleData(i).mu=quatRef(index).mu; 161 | sampleData(i).sigma=quatRef(index).sigma; 162 | end 163 | 164 | for i=1:3 165 | if viaFlag(i) [sampleData,num] = kmp_insertPoint(sampleData,num,via_time(i),via_point(:,i),via_var);end 166 | end 167 | 168 | %% KMP prediction 169 | Kinv = kmp_estimateMatrix_mean(sampleData,num,kh,lamda,dim); 170 | 171 | for index=1:len 172 | t=index*dt; 173 | mu=kmp_pred_mean(t,sampleData,num,kh,Kinv,dim); 174 | kmpTraj(1,index)=t; 175 | kmpTraj(2:7,index)=mu; 176 | end 177 | 178 | %% Project predicted trajectory from Euclidean space into quaternion space 179 | for i=1:len 180 | qnew=quat_exp(kmpTraj(2:4,i)); 181 | trajAdaQuat(i)=quat_mult(qnew,q1); % save final predicted quaternion 182 | 183 | trajAda(1,i)=kmpTraj(1,i); 184 | trajAda(2,i)=trajAdaQuat(i).s; 185 | trajAda(3:5,i)=trajAdaQuat(i).v; 186 | end 187 | [ adaOmega, adaDomega ] = quat_to_vel(trajAdaQuat, dt, tau); % estimate angular vel/acc 188 | 189 | %% Show demonstrations 190 | figure 191 | set(gcf, 'Position', [695 943 1350 425]) 192 | subplot(1,2,1) 193 | hold on 194 | plot(demosAll(1,1:1:end),demosAll(2:5,1:1:end),'.') 195 | xlabel('t [s]','interpreter','tex') 196 | ylabel(' ','interpreter','tex') 197 | ylim([0 1]) 198 | set(gca,'xtick',[0 5 10]) 199 | set(gca,'ytick',[0 0.5 1]) 200 | set(gca,'FontSize',18) 201 | grid on 202 | box on 203 | set(gca,'gridlinestyle','--') 204 | ax=gca; 205 | ax.GridAlpha=0.3; 206 | legend({'$q_s$','$q_x$','$q_y$','$q_z$'},'interpreter','latex','Orientation','horizontal','FontSize',20) 207 | 208 | subplot(1,2,2) 209 | hold on 210 | plot(demosAll(1,1:1:end),demosAll(6:8,1:1:end),'.') 211 | xlabel('t [s]','interpreter','tex') 212 | ylabel(' [rad/s]','interpreter','tex') 213 | ylim([-0.5 0.5]) 214 | set(gca,'xtick',[0 5 10]) 215 | set(gca,'ytick',[-0.5 0 0.5]) 216 | set(gca,'FontSize',18) 217 | grid on 218 | box on 219 | set(gca,'gridlinestyle','--') 220 | ax=gca; 221 | ax.GridAlpha=0.3; 222 | legend({'$\omega_x$','$\omega_y$','$\omega_z$'},'interpreter','latex','Orientation','horizontal','FontSize',20) 223 | 224 | %% Show kmp predictions 225 | figure 226 | set(gcf, 'Position', [690 384 1357 425]) 227 | subplot(1,2,1) 228 | plot(trajAda(1,:),trajAda(2:5,:),'linewidth',3.0) % plot quaternion trajectory 229 | for plotIndex=1:4 230 | for viaIndex=1:3 231 | if viaFlag(viaIndex)==1 232 | hold on 233 | plot(via_time(viaIndex),qDes_temp(plotIndex,viaIndex),'o','color',mycolors.nr,'markersize',8,'linewidth',1.5); 234 | end 235 | end 236 | end 237 | 238 | xlabel('t [s]','interpreter','tex') 239 | ylabel(' ','interpreter','tex') 240 | ylim([0 1]) 241 | set(gca,'xtick',[0 5 10]) 242 | set(gca,'ytick',[0 0.5 1]) 243 | set(gca,'FontSize',18) 244 | grid on 245 | set(gca,'gridlinestyle','--') 246 | ax=gca; 247 | ax.GridAlpha=0.3; 248 | legend({'$q_s$','$q_x$','$q_y$','$q_z$'},'interpreter','latex','Orientation','horizontal','FontSize',20) 249 | 250 | subplot(1,2,2) 251 | plot(trajAda(1,:),adaOmega(1:3,:),'linewidth',3.0) % plot angular velocity 252 | for plotIndex=1:3 253 | for viaIndex=1:3 254 | if viaFlag(viaIndex)==1 255 | hold on 256 | plot(via_time(viaIndex),dqDes(plotIndex,viaIndex),'o','color',mycolors.nr,'markersize',8,'linewidth',1.5); 257 | end 258 | end 259 | end 260 | 261 | xlabel('t [s]','interpreter','tex') 262 | ylabel(' [rad/s]','interpreter','tex') 263 | ylim([-0.5 0.5]) 264 | set(gca,'xtick',[0 5 10]) 265 | set(gca,'ytick',[-0.5 0 0.5]) 266 | set(gca,'FontSize',18) 267 | grid on 268 | set(gca,'gridlinestyle','--') 269 | ax=gca; 270 | ax.GridAlpha=0.3; 271 | legend({'$\omega_x$','$\omega_y$','$\omega_z$'},'interpreter','latex','Orientation','horizontal','FontSize',20) 272 | 273 | -------------------------------------------------------------------------------- /demos/demo_KMP_uncertainty.m: -------------------------------------------------------------------------------- 1 | %-----------demo_KMP_uncertainty---------- 2 | % This file provide a simple demo of using kmp, where both the trajectory covariance 3 | % and uncertainty are predicted. 4 | % 5 | % This code is written by Dr. Yanlong Huang 6 | 7 | % @InProceedings{silverio2019uncertainty, 8 | % Title = {Uncertainty-Aware Imitation Learning using Kernelized Movement Primitives}, 9 | % Author = {Silv\'erio, J. and Huang, Y. and Abu-Dakka, Fares J and Rozo, L. and Caldwell, D. G.}, 10 | % Booktitle = {Proc. {IEEE/RSJ} International Conference on Intelligent Robots and Systems ({IROS})}, 11 | % Year = {2019, to appear}, 12 | % } 13 | 14 | %% 15 | clear; close all; 16 | myColors; 17 | addpath('../fcts/'); 18 | 19 | %% Extract position and velocity from demos 20 | load('../2Dletters/F.mat'); 21 | demoNum=5; % number of demos 22 | demo_dt=0.01; % time interval of data 23 | demoLen=size(demos{1}.pos,2); % size of each demo; 24 | demo_dura=demoLen*demo_dt; % time length of each demo 25 | dim=2; % dimension of demo 26 | 27 | totalNum=0; 28 | for i=1:demoNum 29 | for j=1:demoLen 30 | totalNum=totalNum+1; 31 | Data(1,totalNum)=j*demo_dt; 32 | Data(2:dim+1,totalNum)=demos{i}.pos(1:dim,j); 33 | end 34 | lowIndex=(i-1)*demoLen+1; 35 | upIndex=i*demoLen; 36 | for k=1:dim 37 | Data(dim+1+k,lowIndex:upIndex)=gradient(Data(1+k,lowIndex:upIndex))/demo_dt; 38 | end 39 | end 40 | 41 | %% Extract the reference trajectory 42 | model.nbStates = 8; % Number of states in the GMM 43 | model.nbVar =1+2*dim; % Number of variables [t,x1,x2,.. vx1,vx2...] 44 | model.dt = 0.005; % Time step duration 45 | nbData = demo_dura/model.dt; % Length of each trajectory 46 | 47 | model = init_GMM_timeBased(Data, model); 48 | model = EM_GMM(Data, model); 49 | [DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:model.nbVar); %see Eq. (17)-(19) 50 | 51 | for i=1:nbData 52 | refTraj(i).t=i*model.dt; 53 | refTraj(i).mu=DataOut(:,i); 54 | refTraj(i).sigma=SigmaOut(:,:,i); 55 | end 56 | 57 | %% Set kmp parameters 58 | dt=0.005; 59 | len=demo_dura/dt; 60 | lamda=1; % control mean prediction 61 | lamdac=60;% control variance prediction 62 | kh=6; 63 | 64 | %% Set desired points 65 | viaFlag=[1 1]; % determine which via-points are used 66 | 67 | viaNum=2; 68 | via_time(1)=dt; 69 | via_point(:,1)=[-4 -12 0 0]'; % format:[2D-pos 2D-vel] 70 | via_time(2)=2; 71 | via_point(:,2)=[8 0 0 0]'; 72 | 73 | via_var=1E-6*eye(4); % adaptation precision 74 | via_var(3,3)=1000;via_var(4,4)=1000; % low adaptation precision for velocity 75 | 76 | %% Update the reference trajectory using desired points 77 | newRef=refTraj; 78 | newLen=len; 79 | for viaIndex=1:viaNum 80 | if viaFlag(viaIndex)==1 81 | [newRef,newLen] = kmp_insertPoint(newRef,newLen,via_time(viaIndex),via_point(:,viaIndex),via_var); 82 | end 83 | end 84 | 85 | %% Prediction mean and variance INSIDE AND OUTSIDE the training region using kmp 86 | [Kinv1, Kinv2] = kmp_estimateMatrix_mean_var(newRef,newLen,kh,lamda,lamdac,dim); 87 | 88 | uncertainLen=0.8*len; % the length of uncertain region 89 | totalLen=len+uncertainLen; 90 | 91 | for index=1:totalLen 92 | t=index*dt; 93 | [mu sigma]=kmp_pred_mean_var(t,newRef,newLen,kh,Kinv1,Kinv2,lamdac,dim); 94 | kmpPredTraj(index).t=t; 95 | kmpPredTraj(index).mu=mu; 96 | kmpPredTraj(index).sigma=sigma; 97 | end 98 | 99 | for i=1:totalLen 100 | kmp(:,i)=kmpPredTraj(i).mu; % format:[2D-pos 2D-vel] 101 | for h=1:2*dim 102 | kmpVar(h,i)=kmpPredTraj(i).sigma(h,h); 103 | kmpVar(h,i)=sqrt(kmpVar(h,i)); 104 | end 105 | SigmaOut_kmp(:,:,i)=kmpPredTraj(i).sigma; 106 | end 107 | 108 | 109 | %% Show kmp predictions (mean and covariance/uncertainty) 110 | value=[0.5 0 0.5]; 111 | curveValue=mycolors.o; 112 | 113 | figure('units','normalized','outerposition',[0 0 1 1]) 114 | set(gcf, 'Position', [0.1597 0.1311 0.6733 0.2561]) 115 | 116 | %% plot px-py within the training region 117 | subplot(1,3,1) 118 | plotGMM(kmp(1:2,1:len), SigmaOut_kmp(1:2,1:2,1:len), curveValue, .03); 119 | hold on 120 | plot(kmp(1,1:len),kmp(2,1:len),'color',curveValue,'linewidth',2); 121 | hold on 122 | plot(Data(2,:),Data(3,:),'.','markersize',5,'color','k'); % original data 123 | 124 | for viaIndex=1:viaNum 125 | if viaFlag(viaIndex)==1 126 | plot(via_point(1,viaIndex),via_point(2,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 127 | end 128 | end 129 | 130 | box on 131 | xlim([-15 15]) 132 | ylim([-15 15]) 133 | xlabel('${x}$ [cm]','interpreter','latex'); 134 | ylabel('${y}$ [cm]','interpreter','latex'); 135 | set(gca,'xtick',[-10 0 10]); 136 | set(gca,'ytick',[-10 0 10]); 137 | set(gca,'FontSize',17) 138 | grid on 139 | set(gca,'gridlinestyle','--') 140 | ax=gca; 141 | ax.GridAlpha=0.3; 142 | 143 | %% plot t-px and t-py 144 | for plotIndex=1:2 145 | subplot(1,3,1+plotIndex) 146 | 147 | % plot original data 148 | hold on 149 | plot(Data(1,1:1:end),Data(1+plotIndex,1:1:end),'.','markersize',5,'color','k'); 150 | 151 | % plot kmp prediction INSIDE the training region 152 | shadowT=dt:dt:dt*len; 153 | shadow_time=[shadowT,fliplr(shadowT)]; 154 | 155 | shadowUpKmp=kmp(plotIndex,1:len)+kmpVar(plotIndex,1:len); 156 | shadowLowKmp=kmp(plotIndex,1:len)-kmpVar(plotIndex,1:len); 157 | shadow_kmp=[shadowUpKmp,fliplr(shadowLowKmp)]; 158 | 159 | fill(shadow_time,shadow_kmp',curveValue,'facealpha',.3,'edgecolor','none') 160 | hold on 161 | plot(shadowT,kmp(plotIndex,1:len),'color',curveValue,'linewidth',2.0) 162 | 163 | % plot kmp prediction OUTSIDE the training region 164 | shadowT=dt*(len+1):dt:dt*totalLen; 165 | shadow_time=[shadowT,fliplr(shadowT)]; 166 | 167 | shadowUpKmp=kmp(plotIndex,len+1:totalLen)+kmpVar(plotIndex,len+1:totalLen); 168 | shadowLowKmp=kmp(plotIndex,len+1:totalLen)-kmpVar(plotIndex,len+1:totalLen); 169 | shadow_kmp=[shadowUpKmp,fliplr(shadowLowKmp)]; 170 | 171 | fill(shadow_time,shadow_kmp',mycolors.y,'facealpha',.3,'edgecolor','none') 172 | hold on 173 | plot(shadowT,kmp(plotIndex,len+1:totalLen),'color',mycolors.y,'linewidth',2.0) 174 | 175 | % plot desired points 176 | for viaIndex=1:viaNum 177 | if viaFlag(viaIndex)==1 178 | plot(via_time(viaIndex),via_point(plotIndex,viaIndex),'o','color',value,'markersize',12,'linewidth',1.5); 179 | end 180 | end 181 | 182 | box on 183 | xlim([0 totalLen*dt]) 184 | if plotIndex==1 || plotIndex==2 185 | if plotIndex==1 186 | ylabel('${x}$ [cm]','interpreter','latex'); 187 | end 188 | if plotIndex==2 189 | ylabel('${y}$ [cm]','interpreter','latex'); 190 | end 191 | ylim([-15 15]) 192 | set(gca,'ytick',[-10 0 10]); 193 | end 194 | 195 | xlabel('$t$ [s]','interpreter','latex'); 196 | set(gca,'xtick',[0 1 2]); 197 | set(gca,'FontSize',17) 198 | grid on 199 | set(gca,'gridlinestyle','--') 200 | ax=gca; 201 | ax.GridAlpha=0.3; 202 | end 203 | 204 | -------------------------------------------------------------------------------- /demos/myColors.m: -------------------------------------------------------------------------------- 1 | %This code is initially created by Dr. João Silvério, 2 | %and later modified by Dr. Yanlong Huang 3 | 4 | mycolors = struct('nr', [213,15,37]/255, ... %new red 5 | 'ng', [0,153,37]/255, ... %new green 6 | 'nb', [51,105,232]/255, ... %new blue 7 | 'ny', [238,178,17]/255, ... %new yellow 8 | 'r', [180,20,47]/255, ... %red 9 | 'b', [0,114,189]/255, ... %blue 10 | 'db', [0,100,200]/255, ... %blue 11 | 'g', [119,172,48]/255, ... %green 12 | 'o', [217,83,25]/255, ... % orange 13 | 'y', [237,177,32]/255, ... % yellow 14 | 'p', [126,47,142]/255, ... % purple 15 | 'pi', [204,102,102]/255, ... % pink 16 | 'lb', [77,190,238]/255, ... % light blue 17 | 'li', [164,196,0]/255, ... % lime 18 | 'lr', [229,20,0]/255, ... % light red 19 | 'lg', [220,220,220]/255, ... % light gray 20 | 'dr', [102,0,0]/255, ... % dark red 21 | 'em', [0,138,0]/255, ... % emerald 22 | 'br', [0.6510, 0.5725, 0.3412], ... %bronze 23 | 'gy', [0.6, 0.6, 0.6],... % gray 24 | 'vgy', [160 160 160]/255,... % between gray and dark 25 | 'm', [1 0 1],... % magenta 26 | 'c', [0 1 1],... % cyan 27 | 'rr', [1.0 0.4 0.4],... % clear red 28 | 'gl', [0.8314, 0.7020, 0.7843] ); %greyed lavender 29 | -------------------------------------------------------------------------------- /fcts/EM_GMM.m: -------------------------------------------------------------------------------- 1 | function [model, GAMMA2, LL] = EM_GMM(Data, model) 2 | % Training of a Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. 3 | % 4 | % Writing code takes time. Polishing it and making it available to others takes longer! 5 | % If some parts of the code were useful for your research of for a better understanding 6 | % of the algorithms, please reward the authors by citing the related publications, 7 | % and consider making your own research available in this way. 8 | % 9 | % @article{Calinon15, 10 | % author="Calinon, S.", 11 | % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", 12 | % journal="Intelligent Service Robotics", 13 | % year="2015" 14 | % } 15 | % 16 | % Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/ 17 | % Written by Sylvain Calinon, http://calinon.ch/ 18 | % 19 | % This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/ 20 | % 21 | % PbDlib is free software: you can redistribute it and/or modify 22 | % it under the terms of the GNU General Public License version 3 as 23 | % published by the Free Software Foundation. 24 | % 25 | % PbDlib is distributed in the hope that it will be useful, 26 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | % GNU General Public License for more details. 29 | % 30 | % You should have received a copy of the GNU General Public License 31 | % along with PbDlib. If not, see . 32 | 33 | 34 | %Parameters of the EM algorithm 35 | nbData = size(Data,2); 36 | if ~isfield(model,'params_nbMinSteps') 37 | model.params_nbMinSteps = 5; %Minimum number of iterations allowed 38 | end 39 | if ~isfield(model,'params_nbMaxSteps') 40 | model.params_nbMaxSteps = 100; %Maximum number of iterations allowed 41 | end 42 | if ~isfield(model,'params_maxDiffLL') 43 | model.params_maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm 44 | end 45 | if ~isfield(model,'params_diagRegFact') 46 | %model.params.diagRegFact = 1E-8; %Regularization term is optional 47 | model.params_diagRegFact = 1E-4; %Regularization term is optional 48 | end 49 | if ~isfield(model,'params_updateComp') 50 | model.params_updateComp = ones(3,1); 51 | end 52 | 53 | for nbIter=1:model.params_nbMaxSteps 54 | fprintf('.'); 55 | 56 | %E-step 57 | [L, GAMMA] = computeGamma(Data, model); %See 'computeGamma' function below 58 | GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData); 59 | 60 | %M-step 61 | for i=1:model.nbStates 62 | %Update Priors 63 | if model.params_updateComp(1) 64 | model.Priors(i) = sum(GAMMA(i,:)) / nbData; 65 | end 66 | %Update Mu 67 | if model.params_updateComp(2) 68 | model.Mu(:,i) = Data * GAMMA2(i,:)'; 69 | end 70 | %Update Sigma 71 | if model.params_updateComp(3) 72 | DataTmp = Data - repmat(model.Mu(:,i),1,nbData); 73 | model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(Data,1)) * model.params_diagRegFact; 74 | end 75 | end 76 | 77 | %Compute average log-likelihood 78 | LL(nbIter) = sum(log(sum(L,1))) / nbData; 79 | %Stop the algorithm if EM converged (small change of LL) 80 | if nbIter>model.params_nbMinSteps 81 | if LL(nbIter)-LL(nbIter-1). 32 | 33 | 34 | nbData = size(DataIn,2); 35 | nbVarOut = length(out); 36 | diagRegularizationFactor = 1E-8; %Optional regularization term 37 | 38 | MuTmp = zeros(nbVarOut,model.nbStates); 39 | expData = zeros(nbVarOut,nbData); 40 | expSigma = zeros(nbVarOut,nbVarOut,nbData); 41 | for t=1:nbData 42 | %Compute activation weight 43 | for i=1:model.nbStates 44 | H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i)); 45 | end 46 | H(:,t) = H(:,t) / sum(H(:,t)+realmin); 47 | %Compute conditional means 48 | for i=1:model.nbStates 49 | MuTmp(:,i) = model.Mu(out,i) + model.Sigma(out,in,i)/model.Sigma(in,in,i) * (DataIn(:,t)-model.Mu(in,i)); 50 | expData(:,t) = expData(:,t) + H(i,t) * MuTmp(:,i); 51 | end 52 | %Compute conditional covariances 53 | for i=1:model.nbStates 54 | SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i); 55 | expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)'); 56 | end 57 | expSigma(:,:,t) = expSigma(:,:,t) - expData(:,t)*expData(:,t)' + eye(nbVarOut) * diagRegularizationFactor; 58 | end 59 | 60 | -------------------------------------------------------------------------------- /fcts/gaussPDF.m: -------------------------------------------------------------------------------- 1 | function prob = gaussPDF(Data, Mu, Sigma) 2 | % Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance. 3 | % Inputs ----------------------------------------------------------------- 4 | % o Data: D x N array representing N datapoints of D dimensions. 5 | % o Mu: D x 1 vector representing the center of the Gaussian. 6 | % o Sigma: D x D array representing the covariance matrix of the Gaussian. 7 | % Output ----------------------------------------------------------------- 8 | % o prob: 1 x N vector representing the likelihood of the N datapoints. 9 | % 10 | % Writing code takes time. Polishing it and making it available to others takes longer! 11 | % If some parts of the code were useful for your research of for a better understanding 12 | % of the algorithms, please reward the authors by citing the related publications, 13 | % and consider making your own research available in this way. 14 | % 15 | % @article{Calinon15, 16 | % author="Calinon, S.", 17 | % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", 18 | % journal="Intelligent Service Robotics", 19 | % year="2015" 20 | % } 21 | % 22 | % Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/ 23 | % Written by Sylvain Calinon, http://calinon.ch/ 24 | % 25 | % This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/ 26 | % 27 | % PbDlib is free software: you can redistribute it and/or modify 28 | % it under the terms of the GNU General Public License version 3 as 29 | % published by the Free Software Foundation. 30 | % 31 | % PbDlib is distributed in the hope that it will be useful, 32 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 33 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 34 | % GNU General Public License for more details. 35 | % 36 | % You should have received a copy of the GNU General Public License 37 | % along with PbDlib. If not, see . 38 | 39 | 40 | [nbVar,nbData] = size(Data); 41 | Data = Data' - repmat(Mu',nbData,1); 42 | prob = sum((Data/Sigma).*Data, 2); 43 | prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * abs(det(Sigma)) + realmin); 44 | -------------------------------------------------------------------------------- /fcts/generate_orientation_data.m: -------------------------------------------------------------------------------- 1 | function [q, omega, domega, t] = generate_orientation_data(q1, q2, tau, dt) 2 | %------------------------------------------------------------------------- 3 | % Generate quaternion data, angular velocities and accelerations for DMP 4 | % learning 5 | % Copyright (C) Fares J. Abu-Dakka 2013 6 | 7 | N = round(tau / dt); 8 | t = linspace(0, tau, N); 9 | 10 | s = struct('s',cell(1),'v',cell(1)); 11 | q = repmat(s,100,1); 12 | qq = zeros(4,N); 13 | dqq = zeros(4,N); 14 | omega = zeros(3,N); 15 | domega = zeros(3,N); 16 | 17 | % Generate spline data from q1 to q2 18 | a = minimum_jerk_spline(q1.s, 0, 0, q2.s, 0, 0, tau); 19 | for i = 1:N 20 | q(i).s = minimum_jerk(t(i), a); 21 | end 22 | for j = 1:3 23 | a = minimum_jerk_spline(q1.v(j), 0, 0, q2.v(j), 0, 0, tau); 24 | for i = 1:N 25 | q(i).v(j,1) = minimum_jerk(t(i), a); 26 | end 27 | end 28 | 29 | % Normalize quaternions 30 | for i = 1:N 31 | tmp = quat_norm(q(i)); 32 | q(i).s = q(i).s / tmp; 33 | q(i).v = q(i).v / tmp; 34 | qq(:,i) = [q(i).s; q(i).v]; 35 | end 36 | 37 | % Calculate derivatives 38 | for j = 1:4 39 | dqq(j,:) = gradient(qq(j,:), t); 40 | end 41 | 42 | % Calculate omega and domega 43 | for i = 1:N 44 | dq.s = dqq(1,i); 45 | for j = 1:3 46 | dq.v(j,1) = dqq(j+1,i); 47 | end 48 | omega_q = quat_mult(dq, quat_conjugate(q(i))); 49 | omega(:,i) = 2*omega_q.v; 50 | end 51 | for j = 1:3 52 | domega(j,:) = gradient(omega(j,:), t); 53 | end 54 | 55 | omega(:,1) = [0; 0; 0]; 56 | omega(:,N) = [0; 0; 0]; 57 | domega(:,1) = [0; 0; 0]; 58 | domega(:,N) = [0; 0; 0]; 59 | end 60 | -------------------------------------------------------------------------------- /fcts/init_GMM_timeBased.m: -------------------------------------------------------------------------------- 1 | function model = init_GMM_timeBased(Data, model) 2 | % This function initializes the parameters of a Gaussian Mixture Model 3 | % (GMM) by splitting the data into equal bins (time-based clustering). 4 | % Inputs ----------------------------------------------------------------- 5 | % o Data: D x N array representing N datapoints of D dimensions. 6 | % o nbStates: Number K of GMM components. 7 | % Outputs ---------------------------------------------------------------- 8 | % o Priors: 1 x K array representing the prior probabilities of the 9 | % K GMM components. 10 | % o Mu: D x K array representing the centers of the K GMM components. 11 | % o Sigma: D x D x K array representing the covariance matrices of the 12 | % K GMM components. 13 | % 14 | % Writing code takes time. Polishing it and making it available to others takes longer! 15 | % If some parts of the code were useful for your research of for a better understanding 16 | % of the algorithms, please reward the authors by citing the related publications, 17 | % and consider making your own research available in this way. 18 | % 19 | % @article{Calinon15, 20 | % author="Calinon, S.", 21 | % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", 22 | % journal="Intelligent Service Robotics", 23 | % year="2015" 24 | % } 25 | % 26 | % Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/ 27 | % Written by Sylvain Calinon, http://calinon.ch/ 28 | % 29 | % This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/ 30 | % 31 | % PbDlib is free software: you can redistribute it and/or modify 32 | % it under the terms of the GNU General Public License version 3 as 33 | % published by the Free Software Foundation. 34 | % 35 | % PbDlib is distributed in the hope that it will be useful, 36 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 37 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 38 | % GNU General Public License for more details. 39 | % 40 | % You should have received a copy of the GNU General Public License 41 | % along with PbDlib. If not, see . 42 | 43 | 44 | [nbVar, nbData] = size(Data); 45 | %diagRegularizationFactor = 1E-2; %Optional regularization term 46 | diagRegularizationFactor = 1E-8; %Optional regularization term 47 | 48 | TimingSep = linspace(min(Data(1,:)), max(Data(1,:)), model.nbStates+1); 49 | 50 | for i=1:model.nbStates 51 | idtmp = find( Data(1,:)>=TimingSep(i) & Data(1,:). 37 | 38 | 39 | nbStates = size(Mu,2); 40 | nbDrawingSeg = 35; 41 | darkcolor = color*0.5; %max(color-0.5,0); 42 | t = linspace(-pi, pi, nbDrawingSeg); 43 | 44 | h=[]; 45 | for i=1:nbStates 46 | %R = real(sqrtm(1.0.*Sigma(:,:,i))); 47 | [V,D] = eig(Sigma(:,:,i)); 48 | R = real(V*D.^.5); 49 | X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg); 50 | if nargin>3 %Plot with alpha transparency 51 | h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor, 'facealpha', valAlpha,'edgealpha', valAlpha)]; 52 | %MuTmp = [cos(t); sin(t)] * 0.3 + repmat(Mu(:,i),1,nbDrawingSeg); 53 | %h = [h patch(MuTmp(1,:), MuTmp(2,:), darkcolor, 'LineStyle', 'none', 'facealpha', valAlpha)]; 54 | % h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)]; 55 | else %Plot without transparency 56 | h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor)]; 57 | % h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)]; 58 | end 59 | end 60 | -------------------------------------------------------------------------------- /fcts/quat_conjugate.m: -------------------------------------------------------------------------------- 1 | function qc = quat_conjugate(q) 2 | %------------------------------------------------------------------------- 3 | % Quaternion conjugation 4 | % Copyright (C) Fares J. Abu-Dakka 2013 5 | 6 | qc.s = q.s; 7 | qc.v = -q.v; 8 | end 9 | -------------------------------------------------------------------------------- /fcts/quat_exp.m: -------------------------------------------------------------------------------- 1 | function [ q ] = quat_exp( w ) 2 | % transform a 3-D angular velociy to 4-D quaternion 3 | tmp = norm(w); 4 | if tmp >1e-12 5 | q_w = [ cos(tmp) ; sin(tmp) * w/tmp ]; 6 | else 7 | q_w = [1; 0; 0; 0]; 8 | end 9 | dq = q_w/norm(q_w); 10 | 11 | q.s=dq(1); 12 | q.v=dq(2:4); 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /fcts/quat_log.m: -------------------------------------------------------------------------------- 1 | function log_q = quat_log(q1, q2) 2 | %------------------------------------------------------------------------- 3 | % Calculates logarithm of orientation difference between quaternions 4 | % Copyright (C) Fares J. Abu-Dakka 2013 5 | 6 | q2c = quat_conjugate(q2); 7 | q = quat_mult(q1, q2c); 8 | 9 | tmp = quat_norm(q); 10 | q.s = q.s/tmp; 11 | q.v = q.v/tmp; 12 | 13 | % if q.s < 0 14 | % q.s = -q.s; 15 | % q.v = -q.v; 16 | % end 17 | 18 | if norm(q.v) > 1.0e-12 19 | log_q = acos(q.s) * q.v / norm(q.v); 20 | else 21 | log_q = [0; 0; 0]; 22 | end 23 | end 24 | -------------------------------------------------------------------------------- /fcts/quat_mult.m: -------------------------------------------------------------------------------- 1 | function q = quat_mult(q1, q2) 2 | %------------------------------------------------------------------------- 3 | % Quaternion multiplication 4 | % Copyright (C) Fares J. Abu-Dakka 2013 5 | 6 | 7 | q.s = q1.s * q2.s - q1.v' * q2.v; 8 | q.v = q1.s * q2.v + q2.s * q1.v + [q1.v(2)*q2.v(3) - q1.v(3)*q2.v(2); ... 9 | q1.v(3)*q2.v(1) - q1.v(1)*q2.v(3); ... 10 | q1.v(1)*q2.v(2) - q1.v(2)*q2.v(1)]; 11 | end 12 | -------------------------------------------------------------------------------- /fcts/quat_norm.m: -------------------------------------------------------------------------------- 1 | function [ qnorm ] = quat_norm ( q ) 2 | % calculate the norm of a quaternion 3 | a=q.s; 4 | b=q.v; 5 | qnorm=norm([a b']); 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /fcts/quat_to_vel.m: -------------------------------------------------------------------------------- 1 | function [ omega, domega ] = quat_to_vel(q, dt, tau) 2 | % calculate angular vel/acc using quaternions 3 | 4 | N=tau/dt; 5 | 6 | for i = 1:N 7 | qq(:,i) = [q(i).s; q(i).v]; 8 | end 9 | 10 | % Calculate derivatives 11 | for j = 1:4 12 | dqq(j,:) = gradient(qq(j,:))/dt; 13 | end 14 | 15 | % Calculate omega and domega 16 | for i = 1:N 17 | dq.s = dqq(1,i); 18 | for j = 1:3 19 | dq.v(j,1) = dqq(j+1,i); 20 | end 21 | omega_q = quat_mult(dq, quat_conjugate(q(i))); 22 | omega(:,i) = 2*omega_q.v; 23 | end 24 | for j = 1:3 25 | domega(j,:) = gradient(omega(j,:))/dt; 26 | end 27 | 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /fcts/trans_angVel.m: -------------------------------------------------------------------------------- 1 | function [ localVel ] = trans_angVel(qDes, wDes, dt, q0) 2 | % transform desired angular velocity into the Euclidean space 3 | 4 | q_new=quat_mult(quat_exp(wDes/2*dt), qDes); % desired quaternion at next time step 5 | 6 | zeta1=quat_log(qDes, q0); 7 | zeta2=quat_log(q_new,q0); 8 | 9 | localVel=(zeta2-zeta1)/dt; 10 | 11 | end 12 | 13 | -------------------------------------------------------------------------------- /images/kmp_adaptationB.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/kmp_adaptationB.png -------------------------------------------------------------------------------- /images/kmp_adaptationG.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/kmp_adaptationG.png -------------------------------------------------------------------------------- /images/kmp_orientation_ada.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/kmp_orientation_ada.png -------------------------------------------------------------------------------- /images/kmp_orientation_demos.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/kmp_orientation_demos.png -------------------------------------------------------------------------------- /images/kmp_uncertainty.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/kmp_uncertainty.png -------------------------------------------------------------------------------- /images/modelLetterB.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/modelLetterB.png -------------------------------------------------------------------------------- /images/modelLetterG.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yanlongtu/robInfLib-matlab/c665336ec94fb84609a65950ede5d503bbead70b/images/modelLetterG.png -------------------------------------------------------------------------------- /octave-jupyter-notebooks/README.md: -------------------------------------------------------------------------------- 1 | To run the notebooks the Octave kernel should be installed: 2 | 3 | pip install octave-kernel 4 | -------------------------------------------------------------------------------- /octave-jupyter-notebooks/myColors.m: -------------------------------------------------------------------------------- 1 | %This code is initially created by Dr. João Silvério, 2 | %and later modified by Dr. Yanlong Huang 3 | 4 | mycolors = struct('nr', [213,15,37]/255, ... %new red 5 | 'ng', [0,153,37]/255, ... %new green 6 | 'nb', [51,105,232]/255, ... %new blue 7 | 'ny', [238,178,17]/255, ... %new yellow 8 | 'r', [180,20,47]/255, ... %red 9 | 'b', [0,114,189]/255, ... %blue 10 | 'db', [0,100,200]/255, ... %blue 11 | 'g', [119,172,48]/255, ... %green 12 | 'o', [217,83,25]/255, ... % orange 13 | 'y', [237,177,32]/255, ... % yellow 14 | 'p', [126,47,142]/255, ... % purple 15 | 'pi', [204,102,102]/255, ... % pink 16 | 'lb', [77,190,238]/255, ... % light blue 17 | 'li', [164,196,0]/255, ... % lime 18 | 'lr', [229,20,0]/255, ... % light red 19 | 'lg', [220,220,220]/255, ... % light gray 20 | 'dr', [102,0,0]/255, ... % dark red 21 | 'em', [0,138,0]/255, ... % emerald 22 | 'br', [0.6510, 0.5725, 0.3412], ... %bronze 23 | 'gy', [0.6, 0.6, 0.6],... % gray 24 | 'vgy', [160 160 160]/255,... % between gray and dark 25 | 'm', [1 0 1],... % magenta 26 | 'c', [0 1 1],... % cyan 27 | 'rr', [1.0 0.4 0.4],... % clear red 28 | 'gl', [0.8314, 0.7020, 0.7843] ); %greyed lavender 29 | -------------------------------------------------------------------------------- /readMe.md: -------------------------------------------------------------------------------- 1 | # robInfLib-matlab 2 | ``` 3 | Robot Inference Library (robInfLib) provides various demos of using Kernelized Movement Primitives, 4 | which is maintained by Dr. Yanlong Huang (University of Leeds) 5 | Part of the codes are provided by Dr. João Silvério, Dr. Fares J. Abu-Dakka and Dr. Sylvain Calinon, 6 | which have been acknowledged in the corresponding files. 7 | ``` 8 | 9 | ### DEMOS ILLUSTRATIONS 10 | 11 | > KERNELIZED MOVEMENT PRIMITIVES I (demo_KMP01.m, ref. [1]) 12 |

13 | **Model letters 'G' using GMM/GMR**
14 | 15 |

16 | 17 |

18 | **Trajectory adaptation by using KMP (mean)**
19 | 20 |

21 | 22 | 23 | 24 | > KERNELIZED MOVEMENT PRIMITIVES II (demo_KMP02.m, ref. [2]) 25 |

26 | **Model letters 'B' using GMM/GMR**
27 | 28 |

29 | 30 |

31 | **Trajectory adaptation by using KMP (mean and covariance)**
32 | 33 |

34 | 35 | 36 | 37 | > ORIENTATION KERNELIZED MOVEMENT PRIMITIVES (demo_KMP_orientation.m, ref. [3]) 38 |

39 | **Demonstrated quaternions and angular velocities**
40 | 41 |

42 |

43 | **Adaptation by using orientation-KMP**
44 | 45 |

46 | 47 | > UNCERTAINTY-AWARE KERNELIZED MOVEMENT PRIMITIVES (demo_KMP_uncertainty.m, ref. [4]) 48 |

49 | **Covariance and uncertainty prediction by using KMP while considering adaptations**
50 | 51 |

52 | 53 | ### REFERENCE 54 | 55 | #### [1] KMP: learning and adaptation 56 | [[Link to publication]](https://www.researchgate.net/publication/331481661_Non-parametric_Imitation_Learning_of_Robot_Motor_Skills) 57 | ``` 58 | @InProceedings{Huang19ICRA_1, 59 | Title = {Non-parametric Imitation Learning of Robot Motor Skills}, 60 | Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 61 | Booktitle = {Proc. {IEEE} International Conference on Robotics and Automation ({ICRA})}, 62 | Year = {2019}, 63 | Address = {Montreal, Canada}, 64 | Month = {May}, 65 | Pages = {5266--5272} 66 | } 67 | ``` 68 | 69 | #### [2] KMP: learning, adaptation, superposition and extrapolaton. 70 | [[Link to publication]](https://www.researchgate.net/publication/319349682_Kernelized_Movement_Primitives) 71 | [[Link to video]](https://www.youtube.com/watch?v=sepb6Vs3OMI&feature=youtu.be) 72 | ``` 73 | @Article{Huang19IJRR, 74 | Title = {Kernelized Movement Primitives}, 75 | Author = {Huang, Y. and Rozo, L. and Silv\'erio, J. and Caldwell, D. G.}, 76 | Journal = {International Journal of Robotics Research}, 77 | Year = {2019}, 78 | Volume={38}, 79 | Number={7}, 80 | Pages = {833--852}, 81 | } 82 | ``` 83 | 84 | 85 | #### [3] KMP orientation: learning and adaptation 86 | [[Link to publication]](https://www.researchgate.net/publication/330675655_Generalized_Orientation_Learning_in_Robot_Task_Space) 87 | [[Link to video]](https://www.youtube.com/watch?v=swYJZfAWTHk&feature=youtu.be) 88 | ``` 89 | @InProceedings{Huang19ICRA_2, 90 | Title = {Generalized Orientation Learning in Robot Task Space}, 91 | Author = {Huang, Y. and Abu-Dakka, F. and Silv\'erio, J. and Caldwell, D. G.}, 92 | Booktitle = {Proc. {IEEE} International Conference on Robotics and Automation ({ICRA})},　　　　 93 | Year = {2019}, 94 | Address = {Montreal, Canada}, 95 | Month = {May}, 96 | Pages = {2531--2537} 97 | } 98 | ``` 99 | 100 | #### [4] Uncertainty-aware KMP: uncertainty/covariance prediction and adaptation 101 | [[Link to publication]](https://www.researchgate.net/publication/334884378_Uncertainty-Aware_Imitation_Learning_using_Kernelized_Movement_Primitives) 102 | [[Link to video]](https://www.youtube.com/watch?v=HVk2goCQiaA&feature=youtu.be) 103 | ``` 104 | @InProceedings{silverio2019uncertainty, 105 | Title = {Uncertainty-Aware Imitation Learning using Kernelized Movement Primitives}, 106 | Author = {Silv\'erio, J. and Huang, Y. and Abu-Dakka, Fares J and Rozo, L. and Caldwell, D. G.}, 107 | Booktitle = {Proc. {IEEE/RSJ} International Conference on Intelligent Robots and Systems ({IROS})}, 108 | Year = {2019}, 109 | Pages={90--97} 110 | } 111 | ``` 112 | 113 | 114 | 115 | --------------------------------------------------------------------------------