Description of ccl_data

0001 function data = ccl_data_gen (settings)
0002 % data = ccl_data_gen (settings)
0003 %
0004 % Generate a two link arm robot for simulation purpose by changing
0005 % setting parameters for each constraint.
0006 %
0007 % Input:
0008 %
0009 %   settings:
0010 %       settings.dim_u            Dimensionality of the action space
0011 %       settings.dim_n            Length of the trajectories
0012 %       settings.dim_k            Dimensionality of the constraint
0013 %       settings.dim_r            Dimensionality of the task space x, z, orientation
0014 %       settings.dt               Time interval
0015 %       settings.nTraj            Dimensionality of the trajectories
0016 %       settings.link.length      Length of robot arm links
0017 %       settings.lambda           Ground truth of the selection matrix
0018 %       settings.null.alpha       Null space  policy scaling
0019 %       settings.null.target      Null space  policy target
0020 %       settings.output.show_traj Use 1 to display generated data
0021 %
0022 % Output:
0023 %
0024 %   data:
0025 %       data.X                    State space data
0026 %       data.U                    Action space data
0027 %       data.F                    Null space policy function handle
0028 %       data.R                    Task space data
0029 %       data.Ts                   Task space components data
0030 %       data.Ns                   Null space components data
0031 
0032 Iu  = eye(settings.dim_u) ;
0033 
0034 % null space policy
0035 policy_ns = settings.f_n;
0036 % policy_ns = @(q) settings.null.alpha .* (settings.null.target - q) ;
0037 % policy_ns = @(q) settings.null.alpha .* (q-1.*pi/180) ;
0038 
0039 X = cell(settings.dim_k,1);
0040 F = cell(settings.dim_k,1);
0041 U = cell(settings.dim_k,1);
0042 R = cell(settings.dim_k,1);
0043 B = cell(settings.dim_k,1);
0044 TS = cell(settings.dim_k,1);
0045 NS = cell(settings.dim_k,1);
0046 data.P = [];
0047 if settings.grid_on == 1
0048     N = settings.N;
0049     xmax = ones(settings.dim_x,1); xmin=-xmax;                                % range of data
0050     Xtr  = repmat(xmax-xmin,1,N).*rand(settings.dim_x,N)+repmat(xmin,1,N);
0051     Ftr  = settings.f_n(Xtr);
0052     Btr  = settings.f_b(N);
0053     f_A  = settings.f_A;
0054     for n=1:N
0055         Atr(:,:,n)  = f_A(Xtr(:,n)) ;
0056         P   = eye(2) - pinv(Atr(:,:,n))*Atr(:,:,n) ;
0057         Ptr(:,:,n)  = P ;
0058         NStr(:,n)   = Ptr(:,:,n)*Ftr(:,n) ;
0059         TStr(:,n)   = pinv(Atr(:,:,n))*Btr(:,n) ;
0060         Ytr(:,n)    = TStr(:,n) + NStr(:,n) + settings.s2y*randn(settings.dim_u,1);
0061     end
0062     data.X = Xtr; data.Y = Ytr; data.N = N;
0063     data.F = Ftr; data.A = Atr; data.P = Ptr;
0064     data.NS = NStr ; data.TS = TStr ;
0065 else
0066 
0067     for k = 1: settings.nTraj
0068         if strcmp(settings.task_policy_type,' ')
0069             policy_ts = @(x)zeros(settings.dim_k,1);
0070         else
0071             % task space policy
0072             target = settings.task.target(k);
0073             policy_ts = @(x) settings.null.alpha .* (target - x) ;
0074         end
0075         X{k}        = zeros(settings.dim_u, settings.dim_n);
0076         F{k}        = zeros(settings.dim_u, settings.dim_n);
0077         U{k}        = zeros(settings.dim_u, settings.dim_n);
0078         R{k}        = zeros(settings.dim_r, settings.dim_n);
0079         B{k}        = zeros(settings.dim_r, settings.dim_n);
0080         TS{k}       = zeros(settings.dim_u, settings.dim_n);
0081         NS{k}       = zeros(settings.dim_u, settings.dim_n);
0082         rnd = [1,-1];
0083         if isfield(settings,'learn_alpha')
0084             if strcmp(settings.null_policy_type,'linear_attractor')
0085                 % initial state:
0086                 x = ([1,2]'+rand(2,1)*0.1).*[rnd(randi(2)),rnd(randi(2))]';
0087             elseif strcmp(settings.null_policy_type,'limit_cycle')
0088                 % initial state:
0089                 x = ([0,0]'+rand(2,1)*0.1).*[rnd(randi(2)),rnd(randi(2))]';
0090             elseif strcmp(settings.null_policy_type,'linear')
0091                 % initial state:
0092                 x = ([1,2]'+rand(2,1)*0.1).*[rnd(randi(2)),rnd(randi(2))]';
0093             end
0094             
0095             if strcmp(settings.projection, 'state_independant')
0096                 generate_A    = settings.f_alpha;                       % random constraint
0097             elseif strcmp(settings.projection, 'state_dependant')
0098                 a = 2;
0099                 f_alpha = @(x)([2*a*x(1),-1]);
0100                 generate_A = @(x)(f_alpha(x));
0101             end
0102         end
0103         
0104         if isfield(settings,'learn_lambda')
0105             L   = settings.link.length ;
0106             x = (pi/180)*[0 90]' + (pi/180)*[10 10]'.*(rand(2,1));
0107             
0108             if strcmp(settings.projection, 'state_independant')
0109                 generate_A = @(q)(settings.lambda * ccl_rob_jacobian (q, L));
0110             elseif strcmp(settings.projection, 'state_dependant')
0111                 generate_A = @(q)(settings.lambda(q) * ccl_rob_jacobian (q, L));
0112             end
0113         end
0114         
0115         if settings.learn_nc == 1
0116             L   = settings.link.length ;
0117             x = (pi/180)*[0 90]' + (pi/180)*[10 10]'.*(rand(2,1));
0118             f_lambda = [0,1];                       % random constraint
0119             generate_A = @(q)(f_lambda * ccl_rob_jacobian (q, L));
0120         end
0121         
0122         if isfield(settings,'learn_pi')
0123             L   = settings.link.length ;
0124             x = (pi/180)*[0 90]' + (pi/180)*[10 10]'.*(rand(2,1));
0125             generate_A = settings.A;
0126         end
0127 
0128         
0129         for n = 1 : settings.dim_n+1
0130             A   = generate_A(x) ;
0131             invA= pinv(A) ;
0132             P_   = Iu - invA*A;
0133             f   = policy_ns(x);
0134             ns  = P_*f;
0135             if strcmp(settings.control_space,'joint')
0136                 r   = ccl_rob_forward(x, L) ;
0137                 ts  = pinv(A)*policy_ts(r(settings.fix_joint)) ;
0138                 u   = ts + ns ;
0139                 R{k}(:,n)   = r ;
0140                 B{k} = diff(R{k}')' ;
0141                 B{k} = B{k}(:,1:n-1) ;
0142             end
0143             if strcmp(settings.control_space,'end_effector')
0144                 ts  = policy_ts(x(1)) ;
0145                 u   = ns ;
0146             end
0147             X{k}(:,n)   = x ;
0148             F{k}(:,n)   = f ;
0149             U{k}(:,n)   = u ;
0150             TS{k}(:,n)  = ts ;
0151             NS{k}(:,n)  = ns ;
0152             P{k}(:,:,n) = P_;
0153             x           = x + u * settings.dt +settings.s2y*randn(settings.dim_u,1);
0154             
0155             if ts'*ns > 1e-6
0156                 fprintf('ts and ns are not orthogonal') ;
0157                 return ;
0158             elseif norm(ns) < 1e-3 %norm(ts) < 1e-3 || norm(ns) < 1e-3
0159                 break ;
0160             end
0161         end % end n loop
0162         
0163         X{k} = X{k}(:,1:n-1) ;
0164         R{k} = R{k}(:,1:n-1) ;
0165         F{k} = F{k}(:,1:n-1) ;
0166         U{k} = U{k}(:,1:n-1) ;
0167         TS{k}= TS{k}(:,1:n-1) ;
0168         NS{k}= NS{k}(:,1:n-1) ;
0169         P{k} = P{k}(:,:,1:n-1) ;
0170         
0171         data.P = cat(3,data.P,P{k});
0172     end % end k loop
0173     if  strcmp(settings.control_space,'joint')
0174         if settings.output.show_traj
0175             ccl_rob_vis_move (R{end}, X{end}, L) ;
0176         end
0177     end
0178     data.X = [X{:}];
0179     data.U = [U{:}];
0180     data.F = [F{:}];
0181     data.R = [R{:}];
0182     data.TS = [TS{:}];
0183     data.NS = [NS{:}];
0184 end
ccl_data_gen

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
