demo_toy_example_2D

 demo_toy_example_2D
 This demo demonstrates a toy 2D problem for the usage of the CCL library.

0001 function demo_toy_example_2D
0002 % demo_toy_example_2D
0003 % This demo demonstrates a toy 2D problem for the usage of the CCL library.
0004 clear all;clc;close all;rng('default');
0005 fprintf('<=========================================================================>\r');
0006 fprintf('<=========================================================================>\r');
0007 fprintf('<===========   Constraint Consistent Learning Library    =================>\r');
0008 fprintf('< This demo will go through a Toy example to demonstrate this CCL toolbox >\r');
0009 fprintf('< The CCL is formulated in the following format:                          > \r');
0010 fprintf('< Consider the set of consistent k-dimensional constraints:               >\r');
0011 fprintf('<                  A(x)U(x) = b(x)                                        >\r');
0012 fprintf('<                      U(x) = pinv(A(x))b(x) + (I-pinv(A(x))A(x))Pi(x)    >\r');
0013 fprintf('<                      U(x) =      U_ts      +         U_ns               >\r');
0014 fprintf('< The task is defined in 2D. The constraints are either random or state   >\r');
0015 fprintf('< dependant parabola. The null space control policies are either linear   >\r');
0016 fprintf('< attractor or limit cycle. This demo will execute section by section and >\r');
0017 fprintf('< allow the user to configure the training parameters.                    >\r');
0018 fprintf('< List of sections:                                                       >\r');
0019 fprintf('< SECTION 1:       PARAMETER CONFIGURATION                                >\r');
0020 fprintf('< SECTION 2:       LEARNING NULL SPACE COMPONENTS                         >\r');
0021 fprintf('< SECTION 3:       LEARNING NULL SPACE CONSTRAINTS                        >\r');
0022 fprintf('< SECTION 4:       LEARNING NULL SPACE POLICY                             >\r');
0023 fprintf('< Configuration options:                                                  >\r');
0024 fprintf('< Constraints:                                                            >\r');
0025 fprintf('<            State independant:                                           >\r');
0026 fprintf('<                              linear (random)                            >\r');
0027 fprintf('<              State dependant:                                           >\r');
0028 fprintf('<                              parabola                                   >\r');
0029 fprintf('< Null space policy:                                                      >\r');
0030 fprintf('<                  linear attractor                                       >\r');
0031 fprintf('<                  linear                                                 >\r');
0032 fprintf('<                  limit cycle                                            >\r');
0033 fprintf('< Task space policy:                                                      >\r');
0034 fprintf('<                  random                                                 >\n');
0035 fprintf('<=========================================================================>\r');
0036 fprintf('<=========================================================================>\r');
0037 fprintf('<=========================================================================>\n\n\n');
0038 fprintf('<=========================================================================>\n');
0039 fprintf('<=========================================================================>\n');
0040 fprintf('<=========================================================================>\n');
0041 fprintf('< SECTION 1:       PARAMETER CONFIGURATION                                >\r');
0042 fprintf('\n< User specified configurations are:                                      >\r');
0043 %% GENERATIVE MODEL PARAMETERS
0044 ctr = .9*ones(1,3); lwtr = 1;                                   % colour, linewidth for training data
0045 cv  = [0  0  0];    lwv  = 1;                                   % colour, linewidth for visualisation data
0046 cpv = [1 .5 .5];    lwpv = 2;                                   % colour, linewidth for visualisation data predictions
0047 settings.dim_x          = 2 ;                                   % dimensionality of the state space
0048 settings.dim_u          = 2 ;                                   % dimensionality of the action space
0049 settings.dim_r          = 2 ;                                   % dimensionality of the task space
0050 settings.dim_k          = 1 ;                                   % dimensionality of the constraint
0051 settings.dt             = 0.1;                                  % time step
0052 settings.null.alpha     = 0.5 ;                                   % null space policy scaling
0053 settings.s2y  = .01;                                                     % noise in output
0054 xmax = ones(settings.dim_x,1); xmin=-xmax;                                % range of data
0055 
0056 settings.projection = 'state_independant';                        % {'state_independant' 'state_dependant'}
0057 settings.task_policy_type = 'random';                           % {'random'}
0058 settings.null_policy_type = 'linear_attractor';                 % {'limit_cycle' 'linear_attractor' 'linear'}
0059 settings.control_space    = 'end_effector';                     % control space in end_effector
0060 
0061 fprintf('< Dim_x             = %d                                                   >\r',settings.dim_x);
0062 fprintf('< Dim_u             = %d                                                   >\r',settings.dim_u);
0063 fprintf('< Dim_r             = %d                                                   >\r',settings.dim_r);
0064 fprintf('< Dim_k             = %d                                                   >\r',settings.dim_k);
0065 fprintf('< Constraint        = %s                                                   >\r',settings.projection);
0066 fprintf('< Null_policy_type  = %s                                                   >\r',settings.null_policy_type);
0067 fprintf('< Task_policy_type  = %s                                                   >\r',settings.task_policy_type);
0068 
0069 %% NULL SPACE POLICY GENERATION
0070 switch settings.null_policy_type
0071     case 'limit_cycle'
0072         radius = 0.75;                                          % radius of attractor
0073         qdot   = 1.0;                                           % angular velocity
0074         w      = 1.0;                                           % time scaling factor
0075         f_n = @(x)(-w*[(radius-x(1,:).^2-x(2,:).^2).*x(1,:) - x(2,:)*qdot;(radius-x(1,:).^2-x(2,:).^2).*x(2,:) + x(1,:)*qdot]);
0076     case 'linear_attractor'
0077         target = [0 0]';
0078         f_n    = @(x) settings.null.alpha .* (target - x);      % nullspace policy
0079     case 'linear'
0080         w    = [1 2;3 4;-1 0];
0081         f_n    = @(x)(([x;ones(1,size(x,2))]'*w)');
0082 end
0083 settings.f_n = f_n;
0084 
0085 %% TASK SPACE POLICY GENERATION
0086 f_b = @(N)(2*rand(1,1,N)-1);
0087 % f_b    = @(x) settings.null.alpha .* (randi([-2,2]) - x);      % nullspace policy
0088 settings.f_b = f_b;
0089 
0090 fprintf('<=========================================================================>\n');
0091 fprintf('<=========================================================================>\n');
0092 fprintf('<=========================================================================>\n');
0093 pause();
0094 
0095 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0096 %%                              LEARN NULL SPACE COMPONENT U_n = U_ts + U_ns                          %%
0097 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0098 fprintf('\n\n\n<=========================================================================>\n');
0099 fprintf('<=========================================================================>\n');
0100 fprintf('<=========================================================================>\n');
0101 fprintf('< SECTION 2:       LEARNING NULL SPACE COMPONENTS                         >\r');
0102 fprintf('< In this section,we are trying to learn U_ns out of the observations(X,U)>\r');
0103 fprintf('< The cost function to be minimised is:                                   >\r');
0104 fprintf('<              E[Pi_ns] = sum(||P*U - Pi_ns)||.^2                         >\r');
0105 fprintf('< In order to achieve good performance, for each constraint, enough data  >\r');
0106 fprintf('< samples and model complexity are necessary                              >\n');
0107 fprintf('\n< For details please refer to:                                            >\r');
0108 fprintf('< Towell, M. Howard, and S. Vijayakumar. IEEE International Conference    >\r');
0109 fprintf('< Intelligent Robots and Systems, 2010.                                   >\n');
0110 fprintf(1,'\n< Start learning Null space component  ...                                > \r');
0111 model = [];
0112 Ntr  = 500;
0113 Nte   = 500;
0114 A_ = orth( rand(2, 1) )';
0115 f_A = @(q)(A_);                       % random constraint
0116 model.num_basis = 16 ;                % define the number of radial basis functions
0117 settings.f_A = f_A;
0118 settings.grid_on = 1;
0119 fprintf('\n< Generating training dataset for learning null space components  ...     >\r');
0120 settings.N = Ntr;
0121 Dtr = ccl_data_gen(settings);
0122 Xtr = Dtr.X; Ytr = Dtr.Y;
0123 Ftr = Dtr.F; Atr = Dtr.A; Ptr = Dtr.P;
0124 NStr = Dtr.NS; TStr = Dtr.TS;
0125 fprintf(1,'#Data (train): %5d, \r',Ntr);
0126 
0127 % generate test data
0128 fprintf('< Generating testing dataset for learning null space components   ...     >\n');
0129 settings.N = Nte;
0130 Dte = ccl_data_gen(settings);
0131 Xte = Dte.X; Yte = Dte.Y;
0132 Fte = Dte.F; Ate = Dte.A; Pte = Dte.P;
0133 TSte = Dte.TS; NSte = Dte.NS;
0134 fprintf(1,'#Data (test): %5d, \n',Nte);
0135 
0136 % generate visualisation data
0137 try
0138     error
0139 catch
0140     Ngp  = 10; Nv = Ngp^settings.dim_x;
0141     [X1v X2v] = ndgrid(linspace(xmin(1),xmax(1),Ngp),linspace(xmin(2),xmax(2),Ngp)); Xv = [X1v(:),X2v(:)]';
0142     Fv = f_n(Xv);
0143     for i = 1:Nv
0144         Av  = f_A(Xv(:,i)) ;
0145         P  = eye(2) - pinv(Av)*Av;
0146         NSv(:,i)= P*Fv(:,i) ;
0147     end
0148 end
0149 
0150 % set up the radial basis functions
0151 model.c     = ccl_math_gc (Xtr, model.num_basis) ;      % generate a grid of basis functions
0152 model.s2    = mean(mean(sqrt(ccl_math_distances(model.c, model.c))))^2 ;   % set the variance as the mean distance between centres
0153 model.phi   = @(x)ccl_basis_rbf ( x, model.c, model.s2 );      % normalised Gaussian rbfs
0154 
0155 % learn the nullspace component
0156 model       = ccl_learnv_ncl (Xtr, Ytr, model) ;                       % learn the model
0157 f_ncl       = @(x) ccl_learnv_pred_ncl ( model, x ) ;                     % set up an inference function
0158 
0159 % predict nullspace components
0160 NSptr = f_ncl (Xtr) ;
0161 NSpte = f_ncl (Xte) ;
0162 NSpv  = f_ncl (Xv)  ;
0163 
0164 % calculate errors
0165 NUPEtr = ccl_error_nupe(NStr, NSptr) ;
0166 NUPEte = ccl_error_nupe(NSte, NSpte) ;
0167 YNSptr = ccl_error_npe (Ytr,  NSptr) ;
0168 YNSpte = ccl_error_npe (Yte,  NSpte) ;
0169 fprintf(1,'NUPE (train) = %5.3e, ', NUPEtr);
0170 fprintf(1,'NNPE (train) = %5.3e, ', YNSptr);
0171 fprintf(1,'\n');
0172 fprintf(1,'NUPE (test)  = %5.3e, ', NUPEte);
0173 fprintf(1,'NNPE (test)  = %5.3e, ', YNSpte);
0174 fprintf(1,'\n');
0175 
0176 % visualisation
0177 figNo = 2;
0178 figure(figNo),clf,hold on,grid on,box on
0179 s = .1;
0180 h(1) = quiver(Xtr(1,:), Xtr(2,:), s*Ytr (1,:), s*Ytr (2,:), 0, 'Color', ctr,'LineWidth',lwtr); % training data
0181 h(2) = quiver(Xv (1,:) , Xv(2,:), s*NSpv(1,:), s*NSpv(2,:), 0, 'Color', cpv,'LineWidth',lwpv); % learnt nullspace component
0182 h(3) = quiver(Xv (1,:) , Xv(2,:), s*NSv (1,:), s*NSv (2,:), 0, 'Color',  cv,'LineWidth', lwv); % true nullspace component
0183 legend(h,'Data','True','Estimated','Location','Best');legend boxoff
0184 title('Learning null space component');
0185 axis tight
0186 clear figNo
0187 fprintf('<=========================================================================>\n');
0188 fprintf('<=========================================================================>\n');
0189 fprintf('<=========================================================================>\n');
0190 pause();
0191 %
0192 
0193 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0194 %%                                            LEARN CONSTRAINTS                                       %%
0195 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0196 fprintf('\n\n\n<=========================================================================>\n');
0197 fprintf('<=========================================================================>\n');
0198 fprintf('<=========================================================================>\n');
0199 fprintf('< SECTION 3:       LEARNING NULL SPACE CONSTRAINTS                        >\r');
0200 fprintf('< In this section,we will start addressing state independant constraint A >\r');
0201 fprintf('< and then state dependant constraint A(x). The exploration policy will   >\r');
0202 fprintf('< change the performance of the learnt constraint                         >\r');
0203 fprintf('< The cost function to be minimised is:                                   >\r');
0204 fprintf('<                           E[A] = min(sum||A*Un||^2)                     >\n');
0205 fprintf('\n< For details please refer to:                                            >\r');
0206 fprintf('< H.-C. Lin, M. Howard, and S. Vijayakumar. IEEE International Conference >\r');
0207 fprintf('< Robotics and Automation, 2015                                           >\n');
0208 settings.grid_on = 0;
0209 settings.task_policy_type = ' ';
0210 settings.learn_nc       = 0;
0211 if strcmp(settings.projection,'state_independant')
0212     fprintf(1,'\n< Start learning state independant null space projection   ...            > \n');
0213     % generate visualisation data
0214     settings.dim_n          = 20 ;                          % number of steps in each trajactory
0215     settings.nTraj          = 50 ;                          % number of trajectories
0216     settings.dim_exp        = 1 ;                           % number of experiment to repeat
0217     settings.learn_alpha    = 1;
0218     A_                      = orth( rand(2, 1) )';
0219     P                       = eye(2) - pinv(A_)*A_;
0220     f_alpha                 = @(q)(A_);                     % random constraint
0221     settings.f_alpha        = f_alpha;
0222     Dtr                     = [];
0223     Dte                     = [];
0224     for i = 1:settings.dim_exp
0225         % generating training dataset
0226         fprintf('\n< Generating training dataset for learning constraints    ...             >\r');
0227         Dtr = ccl_data_gen(settings);
0228         Xtr = Dtr.X ; Ytr = Dtr.U ; TStr = Dtr.TS ; NStr = Dtr.NS ; Ftr = Dtr.F ;
0229         fprintf(1,'#Data (train): %5d, \r',Ntr);
0230         % generating testing data
0231         fprintf('< Generating testing dataset for learning constraints     ...             >\r');
0232         Dte = ccl_data_gen (settings) ;
0233         Xte = Dte.X ; Yte = Dte.U ; TSte = Dtr.TS ; NSte = Dte.NS ; Fte = Dte.F ;
0234         fprintf(1,'#Data (test): %5d, \n',Nte);
0235         fprintf(1,'\n< Experiment %d \n', i) ;
0236         fprintf(1,'\t Dimensionality of action space: %d \n', settings.dim_u) ;
0237         fprintf(1,'\t Dimensionality of task space:   %d \n', settings.dim_k) ;
0238         fprintf(1,'\t Dimensionality of null space:   %d \n', settings.dim_u-settings.dim_k) ;
0239         fprintf(1,'\t Size of the training data:      %d \n', size(Xtr,2)) ;
0240         
0241         model  = ccl_learna_nhat (NStr);
0242         
0243         fprintf(1,'True projection:\n w =\n'),      disp(P);
0244         fprintf(1,'Estimated projection:\n wp =\n'),disp(model.P)
0245         
0246         % make prediction
0247         fp   = @(f)ccl_learna_pred_proj (f,model);
0248         Yptr = fp(Ftr);
0249         Ypte = fp(Fte);
0250         Ypv  = fp(Fv) ;
0251         
0252         fprintf(1,'\n Result %d \n') ;
0253         fprintf(1,'\t ===============================\n' ) ;
0254         fprintf(1,'\t       |    NPPE   |   NPOE \n' ) ;
0255         fprintf(1,'\t -------------------------------\n' ) ;
0256         nPPE = ccl_error_ppe(Ytr, model.P, Ftr) ;
0257         nPOE = ccl_error_poe(Ytr, model.P, Ftr) ;
0258         fprintf(1,'\t Train |  %4.2e | %4.2e  \n', nPPE,  nPOE ) ;
0259         nPPE = ccl_error_ppe(Yte, model.P, Fte) ;
0260         nPOE = ccl_error_poe(Yte, model.P, Fte) ;
0261         fprintf(1,'\t Test  |  %4.2e | %4.2e  \n', nPPE,  nPOE ) ;
0262         fprintf(1,'\t ===============================\n' ) ;
0263     end
0264     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0265 elseif strcmp(settings.projection, 'state_dependant')
0266     
0267     fprintf(1,'\n< Start learning state dependant null space projection   ...              > \n');
0268     settings.dim_b          = 10 ;                          % dimensionality of the kernel function. 16 is normally enough for a 2D problem
0269     settings.dim_n          = 20 ;                          % number of steps in each trajactory
0270     settings.nTraj          = 50 ;                          % number of trajectories
0271     settings.dim_exp        = 1 ;                           % number of experiment to repeat
0272     settings.learn_alpha    = 1;
0273     Ntr                     = settings.dim_n*settings.nTraj;
0274     Nte                     = Ntr;
0275     Dtr                     = [];
0276     Dte                     = [];
0277     for i = 1:settings.dim_exp
0278         fprintf('< Generating training dataset for learning constraints     ...             >\r');
0279         Dtr = ccl_data_gen(settings);
0280         Xtr = Dtr.X ; Ytr = Dtr.U ; TStr = Dtr.TS ; NStr = Dtr.NS ; Ftr = Dtr.F ;
0281         fprintf(1,'#Data (train): %5d, \r',Ntr);
0282         % generating testing data
0283         fprintf('< Generating testing dataset learning constraints      ...                 >\r');
0284         Dte = ccl_data_gen (settings) ;
0285         Xte = Dte.X ; Yte = Dte.U ; TSte = Dte.TS ; NSte = Dte.NS ; Fte = Dte.F ;
0286         fprintf(1,'#Data (test): %5d, \n',Nte);
0287         fprintf(1,'\n Experiment %d \n', i) ;
0288         fprintf(1,'\t Dimensionality of action space: %d \n', settings.dim_u) ;
0289         fprintf(1,'\t Dimensionality of task space:   %d \n', settings.dim_k) ;
0290         fprintf(1,'\t Dimensionality of null space:   %d \n', settings.dim_u-settings.dim_k) ;
0291         fprintf(1,'\t Dimensionality of kernel:       %d \n', settings.dim_b) ;
0292         fprintf(1,'\t Size of the training data:      %d \n', size(Xtr,2)) ;
0293         %         fprintf(1,'\t Random constraint:  [%4.2f, %4.2f] \n', settings.alpha(1), settings.alpha(2)) ;
0294         fprintf(1,'\n Learning state-dependent constraint vectors... \n') ;
0295         
0296         model_alpha_ccl  = ccl_learna_alpha (NStr, Xtr, settings);
0297         
0298         ppe.alpha_ccl.tr = ccl_error_ppe_alpha (model_alpha_ccl.f_proj, Xtr, Ftr, NStr) ;
0299         ppe.alpha_ccl.te = ccl_error_ppe_alpha (model_alpha_ccl.f_proj, Xte, Fte, NSte) ;
0300         poe.alpha_ccl.tr = ccl_error_poe_alpha (model_alpha_ccl.f_proj, Xtr, NStr) ;
0301         poe.alpha_ccl.te = ccl_error_poe_alpha (model_alpha_ccl.f_proj, Xte, NSte) ;
0302         
0303         fprintf(1,'\n Result %d \n') ;
0304         fprintf(1,'\t ===============================\n' ) ;
0305         fprintf(1,'\t       |    NPPE   |   NPOE \n' ) ;
0306         fprintf(1,'\t -------------------------------\n' ) ;
0307         fprintf(1,'\t Train |  %4.2e | %4.2e  \n', ppe.alpha_ccl.tr,  poe.alpha_ccl.tr ) ;
0308         fprintf(1,'\t Test  |  %4.2e | %4.2e  \n', ppe.alpha_ccl.te,  poe.alpha_ccl.te ) ;
0309         fprintf(1,'\t ===============================\n' ) ;
0310     end
0311 end
0312 fprintf('<=========================================================================>\n');
0313 fprintf('<=========================================================================>\n');
0314 fprintf('<=========================================================================>\n');
0315 pause();
0316 
0317 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0318 %%                              LEARN PARAMETRIC NULL SPACE POLICY MODEL                              %%
0319 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0320 %% Learn null space policy %%
0321 fprintf('\n\n\n<=========================================================================>\n');
0322 fprintf('<=========================================================================>\n');
0323 fprintf('<=========================================================================>\n');
0324 fprintf('< SECTION 4:       LEARNING NULL SPACE POLICY                             >\r');
0325 fprintf('< This section is for learning null space policy models. The methods we   >\r');
0326 fprintf('< use are parametric model and locally weighted parametric model          >\r');
0327 fprintf('< The cost function to be minimised is:                                   >\r');
0328 fprintf('<                  E[Pi] = min(sim||U_ns - U_ns^2*Pi||^2)                 >\r');
0329 fprintf('< To achiev a better unconstrained null space policy, more constraints    >\r');
0330 fprintf('< need to be demonstrated in the training dataset                         >\n');
0331 fprintf('\n< For details please refer to:                                            >\r');
0332 fprintf('< Howard, Matthew, et al. "A novel method for learning policies from      >\r');
0333 fprintf('< variable constraint data." Autonomous Robots 27.2 (2009): 105-121.      >\n');
0334 fprintf('\n< Start learning null space policy   ...                                 > \n');
0335 Dtr     = [];
0336 Dte     = [];
0337 settings.grid_on = 1;
0338 if strcmp(settings.projection, 'state_independant')
0339     f_A = @(q)(orth( rand(2, 1) )');                       % random constraint
0340 elseif strcmp(settings.projection, 'state_dependant')
0341     f_A = @(q)([2*randi([-10,10])*q(1),-1]);            % parabola constraint
0342 end
0343 settings.f_A = f_A;
0344 Ntr          = 500;
0345 Nte          = 500;
0346 fprintf('\n< Generating training dataset for learning null space constraints  ...    >\n');
0347 settings.N = Ntr;
0348 Dtr = ccl_data_gen(settings);
0349 Xtr = Dtr.X; Ytr = Dtr.Y;
0350 Ftr = Dtr.F; Atr = Dtr.A; Ptr = Dtr.P;
0351 NStr = Dtr.NS; TStr = Dtr.TS;
0352 fprintf(1,'#Data (train): %5d, \r',Ntr);
0353 
0354 % generate test data
0355 fprintf('\n< Generating testing dataset for learning null space constraints   ...    >\n');
0356 settings.N = Nte;
0357 Dte = ccl_data_gen(settings);
0358 Xte = Dte.X; Yte = Dte.Y;
0359 Fte = Dte.F; Ate = Dte.A; Pte = Dte.P;
0360 TSte = Dte.TS; NSte = Dte.NS;
0361 
0362 fprintf(1,'#Data (test): %5d, \n',Nte);
0363 
0364 try
0365     error
0366 catch
0367     fprintf(1,'\n< Start learning Null space policy with parametric model  ...             > \n');
0368     Ngp  = 10; Nv = Ngp^settings.dim_x;
0369     [X1v X2v] = ndgrid(linspace(xmin(1),xmax(1),Ngp),linspace(xmin(2),xmax(2),Ngp)); Xv = [X1v(:),X2v(:)]';
0370     Fv = f_n(Xv);
0371 end
0372 % set up regression model
0373 model = [];
0374 model.w = [];
0375 cmax = xmax+.1;
0376 cmin = xmin-.1;
0377 Ngp  = 10; Nc = Ngp^settings.dim_x;
0378 [c1,c2] = ndgrid(linspace(cmin(1),cmax(1),Ngp),linspace(cmin(2),cmax(2),Ngp)); c = [c1(:),c2(:)]';
0379 s2   = 0.5;
0380 model.phi = @(x)ccl_basis_rbf ( x, c, s2 );
0381 %     model.phi = @(x)phi_linear ( x );
0382 
0383 % train parametric model
0384 model = ccl_learnp_pi(Xtr,NStr,model); fp = @(x)ccl_learnp_pred_linear(x,model);
0385 
0386 % predict training data
0387 Fptr = fp(Xtr);
0388 
0389 % compute training error
0390 NUPEtr = ccl_error_nupe(Ftr,Fptr);
0391 fprintf(1,'NUPE (train) = %5.2e, ',NUPEtr);
0392 NCPEtr = ccl_error_ncpe(NStr,Fptr,Ptr);
0393 fprintf(1,'NCPE (train) = %5.2e, ',NCPEtr);
0394 
0395 % predict test data
0396 Fpte = fp(Xte);
0397 
0398 % compute test error
0399 NUPEte = ccl_error_nupe(Fte,Fpte);
0400 fprintf(1,'NUPE (test) = %5.2e, ',NUPEte);
0401 NCPEte = ccl_error_ncpe(NSte,Fpte,Pte);
0402 fprintf(1,'NCPE (test) = %5.2e',NCPEte);
0403 fprintf(1,'\n');
0404 
0405 % visualisation
0406 figNo = 4;
0407 figure(figNo),clf,hold on,grid on,box on
0408 % visualise the training data
0409 s = .1;
0410 h(1)=quiver(Xtr(1,:),Xtr(2,:),s* NStr(1,:),s* NStr(2,:),0,'Color', ctr,'LineWidth', lwtr);
0411 % visualise the fit
0412 % predict visualisation data
0413 Fpv = fp(Xv);
0414 % visualise the fit
0415 h(2)=quiver(Xv(1,:),Xv(2,:),s*Fpv(1,:),s*Fpv(2,:),0,'Color',cpv,'LineWidth',lwpv);
0416 h(3)=quiver(Xv(1,:),Xv(2,:),s* Fv(1,:),s* Fv(2,:),0,'Color', cv,'LineWidth', lwv);
0417 legend(h,'Data','Estimated','True','Location','Best');legend boxoff
0418 title('Learning null space policy with linear parametric model')
0419 axis tight
0420 pause();
0421 
0422 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0423 %%                              LEARN LOCALLY WEIGHTED LINEAR POLICY MODEL                            %%
0424 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0425 fprintf(1,'\n< Start learning Null space policy with locally weighted  model   ...     > \n');
0426 model = [];
0427 Ngp  = 10; Nc = Ngp^settings.dim_x;
0428 [c1,c2] = ndgrid(linspace(cmin(1),cmax(1),Ngp),linspace(cmin(2),cmax(2),Ngp)); c = [c1(:),c2(:)]';
0429 s2 = 0.005;
0430 model = [];
0431 model.W   = @(x)ccl_basis_rbf( x, c, s2 );
0432  model.phi = @(x)ccl_basis_linear( x );
0433 % model.phi = @(x)ccl_basis_rbf ( x, c, s2 );
0434 
0435 % train the model
0436 model = ccl_learnp_lwpi(Xtr,NStr,model); fp = @(x)ccl_learnp_pred_lwlinear(x,model);
0437 
0438 % predict training data
0439 Fptr = fp(Xtr);
0440 
0441 % compute training error
0442 NUPEtr = ccl_error_nupe(Ftr,Fptr);
0443 fprintf(1,'NUPE (train) = %5.2e, ',NUPEtr);
0444 NCPEtr = ccl_error_ncpe(NStr,Fptr,Ptr);
0445 fprintf(1,'NCPE (train) = %5.2e, ',NCPEtr);
0446 
0447 % predict test data
0448 Fpte = fp(Xte);
0449 
0450 % compute test error
0451 NUPEte = ccl_error_nupe(Fte,Fpte);
0452 fprintf(1,'NUPE (test) = %5.2e, ',NUPEte);
0453 NCPEte = ccl_error_ncpe(NSte,Fpte,Pte);
0454 fprintf(1,'NCPE (test) = %5.2e',NCPEte);
0455 fprintf(1,'\n');
0456 
0457 % visualisation
0458 figNo = 5;
0459 figure(figNo),clf,hold on,grid on,box on
0460 % visualise the training data
0461 s = .1;
0462 h(1)=quiver(Xtr(1,:),Xtr(2,:),s* NStr(1,:),s* NStr(2,:),0,'Color', ctr,'LineWidth', lwtr);
0463 % visualise the fit
0464 % predict visualisation data
0465 Fpv = fp(Xv);
0466 % visualise the fit
0467 h(2)=quiver(Xv(1,:),Xv(2,:),s*Fpv(1,:),s*Fpv(2,:),0,'Color',cpv,'LineWidth',lwpv);
0468 h(3)=quiver(Xv(1,:),Xv(2,:),s* Fv(1,:),s* Fv(2,:),0,'Color', cv,'LineWidth', lwv);
0469 legend(h,'Data','Estimated','True','Location','Best');legend boxoff
0470 title('Learning null space policy with locally weighted parametric model')
0471 axis tight
0472 fprintf('<=========================================================================>\n');
0473 fprintf('<=========================================================================>\n');
0474 fprintf('<=========================================================================>\n');
0475 pause();
0476 close all;
0477 end