ccl_rob_vis_sbs

 ccl_rob_vis_sbs (settings)

 Side by side visualisation of the learnt constraint

 Input:

   settings                       Parameters
   f_project                      Learnt constraint

0001 function ccl_rob_vis_sbs (settings,f_project)
0002 % ccl_rob_vis_sbs (settings)
0003 %
0004 % Side by side visualisation of the learnt constraint
0005 %
0006 % Input:
0007 %
0008 %   settings                       Parameters
0009 %   f_project                      Learnt constraint
0010 
0011 Iu  = eye(settings.dim_u) ;
0012 L   = settings.link.length ;
0013 
0014 % null space policy
0015 policy_ns = @(q) settings.null.alpha .* (settings.null.target - q) ;
0016 % policy_ns = @(q) settings.null.alpha .* (q-1.*pi/180) ;
0017 
0018 x = (pi/180)*[0 90]' + (pi/180)*[10 10]'.*(rand(2,1));
0019 x1 = x;
0020 if strcmp(settings.projection,'state_independant')
0021     nhat = @(x)(settings.lambda) ;
0022 elseif strcmp(settings.projection,'state_dependant')
0023     a = 2;
0024     nhat = @(x)([2*a.*[1,0]*ccl_rob_forward(x,settings.link.length), -1]) ;
0025 end
0026 
0027 for n = 1 : settings.dim_n+1
0028     J   = ccl_rob_jacobian (x, L) ;
0029     r   = ccl_rob_forward(x, L) ;       r_   = ccl_rob_forward(x1, L) ;
0030     A   = nhat(x) * J ;
0031     invA= pinv(A) ;
0032     P_  = Iu - invA*A;
0033     f   = policy_ns(x);         f_   = policy_ns(x1);
0034     ns  = P_*f;                 ns_  = real(f_project(x1))*f_;
0035     ts  = zeros(2,1) ;
0036     u   = ns ;                  u_   = ns_ ;
0037     x   = x + u * settings.dt;
0038     x1  = x1 + u_ * settings.dt;
0039     
0040     if ts'*ns > 1e-6
0041         fprintf('ts and ns are not orthogonal') ;
0042         return ;
0043     elseif norm(ns) < 1e-3 %norm(ts) < 1e-3 || norm(ns) < 1e-3
0044         break ;
0045     end
0046     R(:,n) = r;
0047     R_(:,n) = r_;
0048     X(:,n) = x;
0049     X_(:,n) = x1;
0050 end % end n loop
0051 figh = figure;
0052 ccl_rob_vis_move (figh,R, X, L,'Movement under true constraint',[500,200,500,500]) ;
0053 figh = figure;
0054 ccl_rob_vis_move (figh,R_, X_, L,'Movement under Learnt constraint',[1000,200,500,500]) ;
0055 end