www.pudn.com > Online_LGP.rar > Untitled.m, change:2014-04-08,size:2389b


global dcps; 
 
dt        = 0.001; 
goal      = 1; 
tau       = 0.5; 
n_rfs     = 10; 
ID        = 1; 
 
dcp('clear',ID); 
dcp('init',ID,n_rfs,'minJerk_dcp',0); 
 
% initialize some variables for plotting 
Z=zeros(floor(2*tau/dt+1),2); 
X=Z; 
V=Z; 
T=zeros(floor(2*tau/dt+1),3); 
Y=T; 
PSI=zeros(floor(2*tau/dt+1),n_rfs); 
W=PSI; 
 
% generate the minimum jerk trajectory 
t=0; 
td=0; 
tdd=0; 
 
for i=0:2*tau/dt, 
  [t,td,tdd]=min_jerk_step(t,td,tdd,goal,tau-i*dt,dt); 
  T(i+1,:)   = [t td tdd]; 
end; 
 
% use the inbuilt function to initialize the dcp with minjerk 
%dcp('minjerk',ID); 
dcp('Batch_Fit',ID,tau,dt,T(:,1),T(:,2),T(:,3)); 
 
% test the fit 
dcp('reset_state',ID); 
dcp('set_goal',ID,goal,1); 
 
for i=0:2*tau/dt, 
   
  [y,yd,ydd]=dcp('run',ID,tau,dt); 
   
  Z(i+1,:)   = [dcps(ID).z dcps(ID).zd]; 
  Y(i+1,:)   = [y yd ydd]; 
  X(i+1,:)   = [dcps(ID).x dcps(ID).xd]; 
  V(i+1,:)   = [dcps(ID).v dcps(ID).vd]; 
  PSI(i+1,:) = dcps(ID).psi'; 
  W(i+1,:)   = dcps(ID).w'; 
   
end; 
   
% plotting 
time = (0:dt:tau*2)'; 
 
figure(1); 
clf; 
 
% plot position, velocity, acceleration vs. target 
subplot(431); 
plot(time,[Y(:,1) T(:,1)]); 
title('y'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(432); 
plot(time,[Y(:,2) T(:,2)]); 
title('yd'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(433); 
plot(time,[Y(:,3) T(:,3)]); 
title('ydd'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
% plot internal states 
subplot(434); 
plot(time,Z(:,1)); 
title('z'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(435); 
plot(time,Z(:,2)); 
title('zd'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(436); 
plot(time,PSI); 
title('Weighting Kernels'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(437); 
plot(time,V(:,1)); 
title('v'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(438); 
plot(time,V(:,2)); 
title('vd'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(439); 
plot(time,W); 
title('Linear Model Weights over Time'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(4,3,10); 
plot(time,X(:,1)); 
title('x'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(4,3,11); 
plot(time,X(:,2)); 
title('xd'); 
aa=axis; 
axis([min(time) max(time) aa(3:4)]); 
 
subplot(4,3,12); 
plot(W(end,:)); 
title('Weights'); 
xlabel(sprintf('tau=%f',tau)); 
 
drawnow;