0001 function demo_with_real_data
0002
0003
0004
0005
0006
0007 clear all;clc;close all; rng('default');
0008 fprintf('<=========================================================================>\r');
0009 fprintf('<=========================================================================>\r');
0010 fprintf('<=========== Constraint Consistent Learning Library =================>\r');
0011 fprintf('< This demo will demonstrate the CCL toolbox using real data from Trakstar>\r');
0012 fprintf('< The CCL is formulated in the following format: > \r');
0013 fprintf('< Consider the set of consistent k-dimensional constraints: >\r');
0014 fprintf('< A(x)U(x) = b(x) >\r');
0015 fprintf('< U(x) = pinv(A(x))b(x) + (I-pinv(A(x))A(x))Pi(x) >\r');
0016 fprintf('< U(x) = U_ts + U_ns >\r');
0017 fprintf('< The task is defined in 2D. The constraints are either random or state >\r');
0018 fprintf('< dependant parabola. The null space control policies are either linear >\r');
0019 fprintf('< attractor or limit cycle. This demo will execute section by section and >\r');
0020 fprintf('< allow the user to configure the training parameters. >\r');
0021 fprintf('< List of sections: >\r');
0022 fprintf('< SECTION 1: PARAMETER CONFIGURATION >\r');
0023 fprintf('< SECTION 2: LEARNING NULL SPACE CONSTRAINTS >\r');
0024 fprintf('< Configuration options: >\r');
0025 fprintf('< Constraints: >\r');
0026 fprintf('< State independant: >\r');
0027 fprintf('< linear(random) >\r');
0028 fprintf('< State dependant: >\r');
0029 fprintf('< parabola >\r');
0030 fprintf('< Null space policy: >\r');
0031 fprintf('< circular policy demonstrated by human >\r');
0032 fprintf('<=========================================================================>\r');
0033 fprintf('<=========================================================================>\r');
0034 fprintf('<=========================================================================>\n\n\n');
0035 fprintf('<=========================================================================>\n');
0036 fprintf('<=========================================================================>\n');
0037 fprintf('<=========================================================================>\n');
0038 fprintf('< SECTION 1: PARAMETER CONFIGURATION >\r');
0039 fprintf('\n< User specified configurations are: >\r');
0040
0041 ctr = 1*ones(1,3);
0042 cte = [0 0 0];
0043 settings.dim_x = 3 ;
0044 settings.dim_u = 3 ;
0045 settings.dim_r = 3 ;
0046 settings.dim_k = 1 ;
0047 settings.dt = 0.02;
0048 settings.projection = 'state_dependant';
0049 settings.null_policy_type = 'circle';
0050
0051 fprintf('< Dim_x = %d >\r',settings.dim_x);
0052 fprintf('< Dim_u = %d >\r',settings.dim_u);
0053 fprintf('< Dim_r = %d >\r',settings.dim_r);
0054 fprintf('< Dim_k = %d >\r',settings.dim_k);
0055 fprintf('< Constraint = %s >\r',settings.projection);
0056 fprintf('< Null_policy_type = %s >\r',settings.null_policy_type);
0057
0058 fprintf('<=========================================================================>\n');
0059 fprintf('<=========================================================================>\n');
0060 fprintf('<=========================================================================>\n');
0061 pause();
0062
0063
0064
0065
0066 fprintf('\n\n\n<=========================================================================>\n');
0067 fprintf('<=========================================================================>\n');
0068 fprintf('<=========================================================================>\n');
0069 fprintf('< SECTION 2: LEARNING NULL SPACE CONSTRAINTS >\r');
0070 fprintf('< In this section,we will start addressing state independant constraint A >\r');
0071 fprintf('< and then state dependant constraint A(x). The exploration policy will >\r');
0072 fprintf('< change the performance of the learnt constraint >\r');
0073 fprintf('< The cost function to be minimised is: >\r');
0074 fprintf('< E[A] = min(sum||A*Un||^2) >\n');
0075 fprintf('\n< For details please refer to: >\r');
0076 fprintf('< H.-C. Lin, M. Howard, and S. Vijayakumar. IEEE International Conference >\r');
0077 fprintf('< Robotics and Automation, 2015 >\n');
0078
0079 if strcmp(settings.projection,'state_independant')
0080 fprintf(1,'\n< Start learning state independant null space projection ... > \n');
0081 N_tr = 0.7;
0082 N_te = 0.3;
0083 options.dim_b = 5;
0084 options.dim_r = 3;
0085
0086 fprintf('< Generating training and testingdataset for learning constraints ... >\r');
0087 Data = ccl_data_load('planer_circle');
0088 [ind_tr,ind_te] = dividerand(Data.N,N_tr,N_te,0) ;
0089 Xtr = Data.X(:,ind_tr);Ytr = Data.Y(:,ind_tr);
0090 Xte = Data.X(:,ind_te);Yte = Data.Y(:,ind_te);
0091 fprintf(1,'#Data (train): %5d, \r',size(Xtr,2));
0092 fprintf(1,'#Data (test): %5d, \n',size(Xte,2));
0093 fprintf(1,'\t Dimensionality of action space: %d \n', settings.dim_u) ;
0094 fprintf(1,'\t Dimensionality of task space: %d \n', settings.dim_k) ;
0095 fprintf(1,'\t Dimensionality of null space: %d \n', settings.dim_u-settings.dim_k) ;
0096 fprintf(1,'\t Size of the training data: %d \n', size(Xtr,2)) ;
0097 fprintf(1,'\n Learning state-independent constraint vectors ... \n') ;
0098
0099 model = ccl_learna_alpha (Ytr,Xtr,options);
0100 fprintf(1,'\n Result %d \n') ;
0101 fprintf(1,'\t ===============================\n' ) ;
0102 fprintf(1,'\t | NPOE VPOE UPOE\n' ) ;
0103 fprintf(1,'\t -------------------------------\n' ) ;
0104 [nPOE,vPOE,uPOE] = ccl_error_poe_alpha (model.f_proj, Xtr, Ytr) ;
0105 fprintf(1,'\t Train | %4.2e %4.2e %4.2e \n', nPOE, sum(vPOE), uPOE ) ;
0106 nPOE = ccl_error_poe_alpha (model.f_proj, Xte, Yte) ;
0107 fprintf(1,'\t Test | %4.2e %4.2e %4.2e \n', nPOE, sum(vPOE), uPOE ) ;
0108 fprintf(1,'\t ===============================\n' ) ;
0109 figure;scatter3(Xtr(1,:),Xtr(2,:),Xtr(3,:),'filled','MarkerEdgeColor','k',...
0110 'MarkerFaceColor',ctr);hold on;
0111 scatter3(Xte(1,:),Xte(2,:),Xte(3,:),'MarkerEdgeColor','k',...
0112 'MarkerFaceColor',cte);
0113 zlim([0,1]);xlabel('x');ylabel('y');zlabel('z');title('Training (black) & Testing (white) data visualisation');
0114 for n = 1:size(Yte,2)
0115 NS_p(:,n) = model.f_proj(Xte(:,n)) * Yte(:,n) ;
0116 end
0117 figure;plot(real(NS_p(1,:)));hold on;plot(Yte(1,:)); legend('prediction','true observation');title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y1');
0118 figure;plot(real(NS_p(2,:)));hold on;plot(Yte(2,:)); legend('prediction','true observation');title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y2');
0119 figure;plot(real(NS_p(3,:)));hold on;plot(Yte(3,:)); legend('prediction','true observation');title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y3');
0120
0121 elseif strcmp(settings.projection, 'state_dependant')
0122 fprintf(1,'\n< Start learning state dependant null space projection ... > \n');
0123 N_tr = 0.7;
0124 N_te = 0.3;
0125 options.dim_b = 5;
0126 options.dim_r = 3;
0127
0128 fprintf('< Generating training and testingdataset for learning constraints ... >\r');
0129 Data = ccl_data_load('curve_circle');
0130 [ind_tr,ind_te] = dividerand(Data.N,N_tr,N_te,0) ;
0131 Xtr = Data.X(:,ind_tr);Ytr = Data.Y(:,ind_tr);
0132 Xte = Data.X(:,ind_te);Yte = Data.Y(:,ind_te);
0133 fprintf(1,'#Data (train): %5d, \r',size(Xtr,2));
0134 fprintf(1,'#Data (test): %5d, \n',size(Xte,2));
0135 fprintf(1,'\t Dimensionality of action space: %d \n', settings.dim_u) ;
0136 fprintf(1,'\t Dimensionality of task space: %d \n', settings.dim_k) ;
0137 fprintf(1,'\t Dimensionality of null space: %d \n', settings.dim_u-settings.dim_k) ;
0138 fprintf(1,'\t Size of the training data: %d \n', size(Xtr,2)) ;
0139 fprintf(1,'\n Learning state-dependent constraint vectors... \n') ;
0140
0141 model = ccl_learna_alpha (Ytr,Xtr,options);
0142
0143 fprintf(1,'\n Result %d \n') ;
0144 fprintf(1,'\t ===============================\n' ) ;
0145 fprintf(1,'\t | NPOE VPOE UPOE\n' ) ;
0146 fprintf(1,'\t -------------------------------\n' ) ;
0147 [nPOE,vPOE,uPOE] = ccl_error_poe_alpha (model.f_proj, Xtr, Ytr) ;
0148 fprintf(1,'\t Train | %4.2e %4.2e %4.2e \n', nPOE, sum(vPOE), uPOE ) ;
0149 nPOE = ccl_error_poe_alpha (model.f_proj, Xte, Yte) ;
0150 fprintf(1,'\t Test | %4.2e %4.2e %4.2e \n', nPOE, sum(vPOE), uPOE ) ;
0151 fprintf(1,'\t ===============================\n' ) ;
0152 figure;scatter3(Xtr(1,:),Xtr(2,:),Xtr(3,:),'filled','MarkerEdgeColor','k',...
0153 'MarkerFaceColor',ctr);hold on;
0154 scatter3(Xte(1,:),Xte(2,:),Xte(3,:),'MarkerEdgeColor','k',...
0155 'MarkerFaceColor',cte);
0156 xlabel('x');ylabel('y');zlabel('z');title('Training (black) & Testing (white) data visualisation');
0157 for n = 1:size(Yte,2)
0158 NS_p(:,n) = model.f_proj(Xte(:,n)) * Yte(:,n) ;
0159 end
0160 figure;plot(real(NS_p(1,:)));hold on;plot(Yte(1,:)); legend(['prediction','true observation']);title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y1');
0161 figure;plot(real(NS_p(2,:)));hold on;plot(Yte(2,:)); legend(['prediction','true observation']);title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y2');
0162 figure;plot(real(NS_p(3,:)));hold on;plot(Yte(3,:)); legend(['prediction','true observation']);title('Prediciton VS True observation'); xlabel('Time step'); ylabel('Y3');
0163 end
0164 fprintf('<=========================================================================>\n');
0165 fprintf('<=========================================================================>\n');
0166 fprintf('<=========================================================================>\n');
0167 pause();
0168 close all;
0169 end