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


  function [varargout] = dcp(action,varargin) 
 % A discrete movement primitive (DCP) inspired by 
  % Ijspeert A, Nakanishi J, Schaal S (2003) Learning attractor landscapes for  
 % learning motor primitives. In: Becker S, Thrun S, Obermayer K (eds) Advances  
 % in Neural Information Processing Systems 15. MIT Press, Cambridge, MA. 
  % http://www-clmc.usc.edu/publications/I/ijspeert-NIPS2002.pdf. This 
 % version adds several new features, including that the primitive is 
 % formulated as acceleration, and that the canonical system is normalized. 
 % Additinally, a new scale parameter for the nonlinear function allows a larger  
 % spectrum of modeling options with the primitives 
 %  
 % Copyright July, 2007 by 
 %           Stefan Schaal, Auke Ijspeert, and Heiko Hoffmann 
 %            
 % ---------------  Different Actions of the program ------------------------ 
% 
 % Initialize a DCP model: 
 % FORMAT dcp('Init',ID,n_rfs,name,flag) 
 % ID              : desired ID of model 
  % n_rfs           : number of local linear models 
 % name            : a name for the model 
  22 % flag            : flag=1 use 2nd order canonical system, flag=0 use 1st order 
  23 % 
  24 % alternatively, the function is called as 
  25 % 
  26 % FORMAT dcp('Init',ID,d,) 
  27 % d               : a complete data structure of a dcp model 
  28 % 
  29 % returns nothing 
  30 % ------------------------------------------------------------------------- 
  31 % 
  32 % Set the goal state: 
  33 % FORMAT dcp('Set_Goal',ID,g,flag) 
  34 % ID              : ID of model 
  35 % g               : the new goal 
  36 % flag            : flag=1: update x0 with current state, flag=0: don't update x0 
  37 % 
  38 % returns nothing 
  39 % ------------------------------------------------------------------------- 
  40 % 
  41 % Set the scale factor of the movement: 
  42 % FORMAT dcp('Set_Scale',ID,s,flag) 
  43 % ID              : ID of model 
  44 % s               : the new scale 
  45 % 
  46 % returns nothing 
  47 % ------------------------------------------------------------------------- 
  48 % 
  49 % Run the dcps: 
  50 % FORMAT [[y,yd,ydd]=dcp('Run',ID,tau,dt,ct,cc) 
  51 % ID              : ID of model 
  52 % tau             : global time constant to scale speed of system 
  53 % dt              : integration time step 
  54 % ct              : coupling term for transformation system 
  55 % cc              : coupling term for canonical system 
  56 % 
  57 % returns y,yd,ydd, i.e., current pos,vel,acc, of the dcp 
  58 % ------------------------------------------------------------------------- 
  59 % 
  60 % Change values of a dcp: 
  61 % FORMAT dcp('Change',ID,pname,value) 
  62 % ID              : ID of model 
  63 % pname           : parameter name 
  64 % value           : value to be assigned to parameter 
  65 % 
  66 % returns nothing 
  67 % ------------------------------------------------------------------------- 
  68 % 
  69 % Run the dcps: 
  70 % FORMAT dcp('Run',ID,tau,dt,t,td) 
  71 % ID              : ID of model 
  72 % tau             : time constant to scale speed, tau is roughly movement 
  73 %                   time until convergence 
  74 % dt              : integration time step 
  75 % 
  76 % returns y,yd,ydd, i.e., current pos,vel,acc, of the dcp 
  77 % ------------------------------------------------------------------------- 
  78 % 
  79 % Run the dcp and update the weights: 
  80 % FORMAT dcp('Run_Fit',ID,tau,dt,t,td,tdd) 
  81 % ID              : ID of model 
  82 % tau             : time constant to scale speed, tau is roughly movement 
  83 %                   time until convergence 
  84 % dt              : integration time step 
  85 % t               : target for y 
  86 % td              : target for yd 
  87 % tdd             : target for ydd 
  88 % 
  89 % returns y,yd,ydd, i.e., current pos,vel,acc, of the dcp 
  90 % ------------------------------------------------------------------------- 
  91 % 
  92 % Fit the dcp to a complete trajectory in batch mode: 
  93 % FORMAT dcp('Batch_Fit',ID,tau,dt,T,Td,Tdd) 
  94 % ID              : ID of model 
  95 % tau             : time constant to scale speed, tau is roughly movement 
  96 %                   time until convergence the goal 
  97 % dt              : somple time step in given trajectory 
  98 % T               : target trajectory for y 
  99 % Td              : target trajectory for yd (optional, will be generated 
 100 %                   as dT/dt otherwise 
 101 % Tdd             : target trajectory for ydd (optional, will be generated 
 102 %                   as dTd/dt otherwise 
 103 % 
 104 % returns y,yd,ydd, i.e., current pos,vel,acc, of the dcp 
 105 % ------------------------------------------------------------------------- 
 106 % 
 107 % Return the data structure of a dcp model 
 108 % FORMAT [d] = dcps('Structure',ID) 
 109 % ID              : desired ID of model 
 110 % 
 111 % returns the complete data structure of a dcp model, e.g., for saving or 
 112 % inspecting it 
 113 % ------------------------------------------------------------------------- 
 114 % 
 115 % Reset the states of a dcp model to zero 
 116 % FORMAT [d] = dcps('Reset_State',ID) 
 117 % ID              : desired ID of model 
 118 % 
 119 % returns nothing 
 120 % ------------------------------------------------------------------------- 
 121 % 
 122 % Clear the data structure of a LWPR model 
 123 % FORMAT dcps('Clear',ID) 
 124 % ID              : ID of model 
 125 % 
 126 % returns nothing 
 127 % ------------------------------------------------------------------------- 
 128 % 
 129 % Initializes the dcp with a minimum jerk trajectory 
 130 % FORMAT dcps('MinJerk',ID) 
 131; % ID              : ID of model 
 132 % 
 133 % returns nothing 
 134 % ------------------------------------------------------------------------- 
 135  
 136 % the global structure to store all dcps 
 137 global dcps; 
 138 global min_y; 
 139 global max_y; 
 140  
 141 % at least two arguments are needed 
 142 if nargin < 2, 
 143   error('Incorrect call to dcp'); 
 144 end 
 145  
 146 switch lower(action), 
 147 % ......................................................................... 
 148   case 'init' 
 149     if (nargin == 3)  
 150       ID        = varargin{1}; 
 151       dcps(ID)  = varargin{2}; 
 152     else  
 153       % this initialization is good for 0.5 seconds movement for tau=0.5 
 154       ID               = varargin{1}; 
 155       n_rfs            = varargin{2}; 
 156       dcps(ID).name    = varargin{3}; 
 157       dcps(ID).c_order = 0; 
 158       if (nargin == 5) 
 159         dcps(ID).c_order = varargin{4}; 
 160       end 
 161       % the time constants for chosen for critical damping 
 162       dcps(ID).alpha_z = 25; 
 163       dcps(ID).beta_z  = dcps(ID).alpha_z/4; 
 164       dcps(ID).alpha_x = dcps(ID).alpha_z/3; 
 165       dcps(ID).alpha_v = dcps(ID).alpha_z; 
 166       dcps(ID).beta_v  = dcps(ID).beta_z; 
 167       % initialize the state variables 
 168       dcps(ID).z       = 0; 
 169       dcps(ID).y       = 0;  
 170       dcps(ID).x       = 0; 
 171       dcps(ID).v       = 0; 
 172       dcps(ID).zd      = 0; 
 173       dcps(ID).yd      = 0;  
 174       dcps(ID).xd      = 0; 
 175       dcps(ID).vd      = 0; 
 176       dcps(ID).ydd     = 0;  
 177       % the current goal state 
 178       dcps(ID).G       = 0; 
 179;       % the current start state of the primitive 
 180       dcps(ID).y0      = 0; 
 181       % the orginal amplitude (max(y)-min(y)) when the primitive was fit 
 182       dcps(ID).A       = 0; 
 183       % the original goal amplitude (G-y0) when the primitive was fit 
 184       dcps(ID).dG      = 0; 
 185       % the scale factor for the nonlinear function 
 186       dcps(ID).s       = 1; 
 187        
 188       t = (0:1/(n_rfs-1):1)'*0.5; 
 189       if (dcps(ID).c_order == 1) 
 190         % the local models, spaced on a grid in time by applying the 
 191         % anaytical solutions x(t) = 1-(1+alpha/2*t)*exp(-alpha/2*t) 
 192         dcps(ID).c       = (1+dcps(ID).alpha_z/2*t).*exp(-dcps(ID).alpha_z/2*t); 
 193       else 
 194         % the local models, spaced on a grid in time by applying the 
 195         % anaytical solutions x(t) = exp(-alpha*t) 
 196         dcps(ID).c       = exp(-dcps(ID).alpha_x*t); 
 197       end 
 198        
 199       dcps(ID).psi     = zeros(n_rfs,1); 
 200       dcps(ID).w       = zeros(n_rfs,1); 
 201       dcps(ID).sx2     = zeros(n_rfs,1); 
 202       dcps(ID).sxtd    = zeros(n_rfs,1); 
 203       dcps(ID).D       = (diff(dcps(ID).c)*0.55).^2; 
 204       dcps(ID).D       = 1./[dcps(ID).D;dcps(ID).D(end)]; 
 205       dcps(ID).lambda  = 1; 
 206        
 207       dcps.c 
 208       dcps.D 
 209     end 
 210      
 211 % ......................................................................... 
 212   case 'reset_state' 
 213     ID               = varargin{1}; 
 214     % initialize the state variables 
 215     dcps(ID).z       = 0; 
 216     dcps(ID).y       = 0;  
 217     dcps(ID).x       = 0; 
 218     dcps(ID).v       = 0; 
 219     dcps(ID).zd      = 0; 
 220     dcps(ID).yd      = 0;  
 221     dcps(ID).xd      = 0; 
 222     dcps(ID).vd      = 0; 
 223     dcps(ID).ydd     = 0;  
 224     % the goal state 
 225     dcps(ID).G       = 0; 
 226     dcps(ID).y0      = 0; 
 227     dcps(ID).s       = 1; 
 228      
 229; % ......................................................................... 
 230   case 'set_goal' 
 231     ID               = varargin{1}; 
 232     dcps(ID).G       = varargin{2}; 
 233     flag             = varargin{3}; 
 234     if (flag), 
 235       dcps(ID).x     = 1; 
 236       dcps(ID).y0    = dcps(ID).y; 
 237     end 
 238     if (abs(dcps(ID).dG)>0.01) 
 239       dcps(ID).s       = (dcps(ID).G-dcps(ID).y0)/dcps(ID).dG; 
 240     end 
 241      
 242 % ......................................................................... 
 243   case 'set_scale' 
 244     ID               = varargin{1}; 
 245     dcps(ID).s       = varargin{2}; 
 246          
 247 % ......................................................................... 
 248   case 'run' 
 249     ID               = varargin{1}; 
 250     tau              = 0.5/varargin{2}; % tau is relative to 0.5 seconds nominal movement time 
 251     dt               = varargin{3}; 
 252      
 253     if nargin > 4, 
 254       ct  = varargin{4}; 
 255     else 
 256       ct  = 0; 
 257     end 
 258;  
 259     if nargin > 5, 
 260       cc  = varargin{5}; 
 261     else 
 262       cc  = 0; 
 263     end 
 264      
 265     if nargin > 6, 
 266       ct_tau  = varargin{6}; 
 267     else 
 268       ct_tau  = 1; 
 269     end 
 270      
 271     if nargin > 7, 
 272       cc_tau  = varargin{7}; 
 273     else 
 274       cc_tau  = 1; 
 275     end 
 276      
 277     % the weighted sum of the locally weighted regression models 
 278     dcps(ID).psi = exp(-0.5*((dcps(ID).x-dcps(ID).c).^2).*dcps(ID).D); 
 279     amp          = dcps(ID).s; 
 280     if (dcps(ID).c_order == 1) 
 281       in = dcps(ID).v; 
 282     else 
 283       in = dcps(ID).x; 
 284     end 
 285     f            = sum(in*dcps(ID).w.*dcps(ID).psi)/sum(dcps(ID).psi+1.e-10) * amp; 
 286      
 287     if (dcps(ID).c_order == 1), 
 288       dcps(ID).vd = (dcps(ID).alpha_v*(dcps(ID).beta_v*(0-dcps(ID).x)-dcps(ID).v)+cc)*tau*cc_tau; 
 289       dcps(ID).xd = dcps(ID).v*tau*cc_tau; 
 290     else  
 291       dcps(ID).vd = 0; 
 292       dcps(ID).xd = (dcps(ID).alpha_x*(0-dcps(ID).x)+cc)*tau*cc_tau; 
 293     end 
 294      
 295     dcps(ID).zd = (dcps(ID).alpha_z*(dcps(ID).beta_z*(dcps(ID).G-dcps(ID).y)-dcps(ID).z)+f+ct)*tau*ct_tau; 
 296     dcps(ID).yd = dcps(ID).z*tau*ct_tau; 
 297     dcps(ID).ydd= dcps(ID).zd*tau*ct_tau; 
 298      
 299     dcps(ID).x  = dcps(ID).xd*dt+dcps(ID).x; 
 300     dcps(ID).v  = dcps(ID).vd*dt+dcps(ID).v; 
 301      
 302      
 303     dcps(ID).z  = dcps(ID).zd*dt+dcps(ID).z; 
 304     dcps(ID).y  = dcps(ID).yd*dt+dcps(ID).y; 
 305          
 306     varargout(1) = {dcps(ID).y}; 
 307     varargout(2) = {dcps(ID).yd}; 
 308     varargout(3) = {dcps(ID).ydd}; 
 309     %varargout(4) = {dcps(ID).psi*dcps(ID).v/sum(dcps(ID).psi)}; 
 310     varargout(4) = {amp*dcps(ID).psi*dcps(ID).x}; 
 311      
 312 % ......................................................................... 
 313   case 'change' 
 314     ID      = varargin{1}; 
 315     command = sprintf('dcps(%d).%s = varargin{3};',ID,varargin{2}); 
 316     eval(command); 
 317      
 318 % ......................................................................... 
 319   case 'run_fit' 
 320     ID               = varargin{1}; 
 321     tau              = 0.5/varargin{2}; % tau is relative to 0.5 seconds nominal movement time 
 322     dt               = varargin{3}; 
 323     t                = varargin{4}; 
 324     td               = varargin{5}; 
 325     tdd              = varargin{6}; 
 326      
 327     % check whether this is the first time the primitive is fit, and record the 
 328     % amplitude and dG information 
 329     if (dcps(ID).A == 0) 
 330       dcps(ID).dG = dcps(ID).G - dcps(ID).y0; 
 331       if (dcps(ID).x == 1), 
 332         min_y = +1.e10; 
 333         max_y = -1.e10; 
 334         dcps(ID).s = 1; 
 335       end 
 336     end 
 337          
 338     % the regression target 
 339     amp              = dcps(ID).s; 
 340     ft               = (tdd/tau^2-dcps(ID).alpha_z*(dcps(ID).beta_z*(dcps(ID).G-t)-td/tau))/amp; 
 341      
 342     % the weighted sum of the locally weighted regression models 
 343     dcps(ID).psi = exp(-0.5*((dcps(ID).x-dcps(ID).c).^2).*dcps(ID).D); 
 344      
 345     % update the regression 
 346     if (dcps(ID).c_order == 1), 
 347       dcps(ID).sx2  = dcps(ID).sx2*dcps(ID).lambda + dcps(ID).psi*dcps(ID).v^2; 
 348       dcps(ID).sxtd = dcps(ID).sxtd*dcps(ID).lambda + dcps(ID).psi*dcps(ID).v*ft; 
 349       dcps(ID).w    = dcps(ID).sxtd./(dcps(ID).sx2+1.e-10); 
 350     else 
 351       dcps(ID).sx2  = dcps(ID).sx2*dcps(ID).lambda + dcps(ID).psi*dcps(ID).x^2; 
 352       dcps(ID).sxtd = dcps(ID).sxtd*dcps(ID).lambda + dcps(ID).psi*dcps(ID).x*ft; 
 353       dcps(ID).w    = dcps(ID).sxtd./(dcps(ID).sx2+1.e-10); 
 354     end 
 355      
 356     % compute nonlinearity 
 357     if (dcps(ID).c_order == 1) 
 358       in = dcps(ID).v; 
 359     else 
 360       in = dcps(ID).x; 
 361     end 
 362     f     = sum(in*dcps(ID).w.*dcps(ID).psi)/sum(dcps(ID).psi+1.e-10) * amp; 
 363      
 364     % integrate 
 365     if (dcps(ID).c_order == 1), 
 366       dcps(ID).vd = (dcps(ID).alpha_v*(dcps(ID).beta_v*(0-dcps(ID).x)-dcps(ID).v))*tau; 
 367       dcps(ID).xd = dcps(ID).v*tau; 
 368     else  
 369       dcps(ID).vd = 0; 
 370       dcps(ID).xd = dcps(ID).alpha_x*(0-dcps(ID).x)*tau; 
 371     end 
 372      
 373     % note that yd = td = z*tau   ==> z=td/tau; the first equation means 
 374     % simply dcps(ID).zd = tdd 
 375     dcps(ID).zd = (dcps(ID).alpha_z*(dcps(ID).beta_z*(dcps(ID).G-dcps(ID).y)-dcps(ID).z)+f)*tau; 
 376     dcps(ID).yd = dcps(ID).z*tau; 
 377     dcps(ID).ydd= dcps(ID).zd*tau; 
 378      
 379     dcps(ID).x  = dcps(ID).xd*dt+dcps(ID).x; 
 380     dcps(ID).v  = dcps(ID).vd*dt+dcps(ID).v; 
 381      
 382     dcps(ID).z  = dcps(ID).zd*dt+dcps(ID).z; 
 383     dcps(ID).y  = dcps(ID).yd*dt+dcps(ID).y; 
 384      
 385     varargout(1) = {dcps(ID).y}; 
 386     varargout(2) = {dcps(ID).yd}; 
 387     varargout(3) = {dcps(ID).ydd}; 
 388      
 389     if (dcps(ID).A == 0) 
 390       max_y = max(max_y,dcps(ID).y); 
 391       min_y = min(min_y,dcps(ID).y); 
 392       if (dcps(ID).x < 0.0001) 
 393         dcps(ID).A = max_y - min_y; 
 394       end 
 395     end 
 396      
 397 % ......................................................................... 
 398   case 'batch_fit' 
 399      
 400     ID               = varargin{1}; 
 401     tau              = 0.5/varargin{2}; % tau is relative to 0.5 seconds nominal movement time 
 402     dt               = varargin{3}; 
 403     T                = varargin{4}; 
 404     if (nargin > 5)  
 405       Td               = varargin{5}; 
 406     else 
 407       Td               = diffnc(T,dt); 
 408     end 
 409     if (nargin > 6)  
 410       Tdd              = varargin{6}; 
 411     else 
 412       Tdd              = diffnc(Td,dt); 
 413     end 
 414      
 415     % the start state is the first state in the trajectory 
 416     y0 = T(1); 
 417      
 418     % the goal is the last state in the trajectory 
 419     goal = T(end); 
 420      
 421     % the amplitude is the max(T)-min(T) 
 422     A    = range(T); 
 423       
 424     % compute the hidden states 
 425     X = zeros(size(T)); 
 426     V = zeros(size(T)); 
 427     x = 1; 
 428     v = 0; 
 429     
 430     for i=1:length(T), 
 431        
 432       X(i) = x; 
 433       V(i) = v; 
 434       if (dcps(ID).c_order == 1) 
 435         vd   = dcps(ID).alpha_v*(dcps(ID).beta_v*(0-x)-v)*tau; 
 436         xd   = v*tau; 
 437       else 
 438         vd   = 0; 
 439         xd   = dcps(ID).alpha_x*(0-x)*tau; 
 440       end 
 441       x    = xd*dt+x; 
 442       v    = vd*dt+v; 
 443        
 444     end 
 445      
 446     % the regression target 
 447     dcps(ID).dG = goal - y0; 
 448     dcps(ID).A  = max(T)-min(T); 
 449     dcps(ID).s  = 1;  % for fitting a new primitive, the scale factor is always equal to one 
 450  
 451     amp = dcps(ID).s; 
 452     Ft  = (Tdd/tau^2-dcps(ID).alpha_z*(dcps(ID).beta_z*(goal-T)-Td/tau)) / amp; 
 453      
 454     % compute the weights for each local model along the trajectory 
 455     PSI = exp(-0.5*((X*ones(1,length(dcps(ID).c))-ones(length(T),1)*dcps(ID).c').^2).*(ones(length(T),1)*dcps(ID).D')); 
 456  
 457     % compute the regression 
 458     if (dcps(ID).c_order == 1) 
 459       dcps(ID).sx2  = sum(((V.^2)*ones(1,length(dcps(ID).c))).*PSI,1)'; 
 460       dcps(ID).sxtd = sum(((V.*Ft)*ones(1,length(dcps(ID).c))).*PSI,1)'; 
 461       dcps(ID).w    = dcps(ID).sxtd./(dcps(ID).sx2+1.e-10); 
 462     else 
 463       dcps(ID).sx2  = sum(((X.^2)*ones(1,length(dcps(ID).c))).*PSI,1)'; 
 464       dcps(ID).sxtd = sum(((X.*Ft)*ones(1,length(dcps(ID).c))).*PSI,1)'; 
 465       dcps(ID).w    = dcps(ID).sxtd./(dcps(ID).sx2+1.e-10); 
 466     end 
 467      
 468     % compute the prediction 
 469     if (dcps(ID).c_order == 1) 
 470       F     = sum((V*dcps(ID).w').*PSI,2)./sum(PSI,2) * amp;       
 471     else 
 472       F     = sum((X*dcps(ID).w').*PSI,2)./sum(PSI,2) * amp; 
 473     end 
 474     z     = 0; 
 475     zd    = 0; 
 476     y     = y0; 
 477     Y     = zeros(size(T)); 
 478     Yd    = zeros(size(T)); 
 479     Ydd   = zeros(size(T)); 
 480      
 481     for i=1:length(T), 
        
        Ydd(i) = zd*tau; 
        Yd(i)  = z; 
        Y(i)   = y; 
         
        zd   = (dcps(ID).alpha_z*(dcps(ID).beta_z*(goal-y)-z)+F(i))*tau; 
        yd   = z; 
         
        z    = zd*dt+z; 
        y    = yd*dt+y; 
             
      end 
          
      varargout(1) = {Y}; 
     varargout(2) = {Yd}; 
     varargout(3) = {Ydd}; 
       
  % ......................................................................... 
    case 'structure' 
      ID     = varargin{1}; 
      varargout(1) = {dcps(ID)};   
       
  % ......................................................................... 
    case 'clear' 
      ID     = varargin{1}; 
      if exist('dcps') 
        if length(dcps) >= ID, 
          dcps(ID) = []; 
        end 
      end 
       
  % ......................................................................... 
    case 'minjerk' 
      ID     = varargin{1}; 
       
      % generate the minimum jerk trajectory as target to learn from 
      t=0; 
      td=0; 
      tdd=0; 
      goal = 1; 
       
      dcp('reset_state',ID); 
    dcp('set_goal',ID,goal,1); 
     tau = 0.5; 
  dt = 0.001; 
     T=zeros(2*tau/dt,3); 
  
    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; 
   
    % batch fitting 
     i = round(2*tau/dt); % only fit the part of the trajectory with the signal 
   [Yp,Ypd,Ypdd]=dcp('batch_fit',ID,tau,dt,T(1:i,1),T(1:i,2),T(1:i,3)); 
     
 % ......................................................................... 
   otherwise 
    error('unknown action'); 
       
 end