ccl_data_genlwr

 dataset = ccl_data_genlwr (settings)

 Generate Constraint Consistent Learning (CCL) data for a 7 Dof simulated robot arm

 Input:

   settings                    Task related parameters (details see comments)

 Output:

   dataset                     Generated dataset

0001 function dataset = ccl_data_genlwr (settings)
0002 % dataset = ccl_data_genlwr (settings)
0003 %
0004 % Generate Constraint Consistent Learning (CCL) data for a 7 Dof simulated robot arm
0005 %
0006 % Input:
0007 %
0008 %   settings                    Task related parameters (details see comments)
0009 %
0010 % Output:
0011 %
0012 %   dataset                     Generated dataset
0013 
0014 
0015 
0016 
0017 % CCL: A MATLAB library for Constraint Consistent Learning
0018 % Copyright (C) 2007  Matthew Howard
0019 % Contact: matthew.j.howard@kcl.ac.uk
0020 %
0021 % This library is free software; you can redistribute it and/or
0022 % modify it under the terms of the GNU Lesser General Public
0023 % License as published by the Free Software Foundation; either
0024 % version 2.1 of the License, or (at your option) any later version.
0025 %
0026 % This library is distributed in the hope that it will be useful,
0027 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0028 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
0029 % Lesser General Public License for more details.
0030 %
0031 % You should have received a copy of the GNU Library General Public
0032 % License along with this library; if not, write to the Free
0033 % Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
0034 
0035 settings.dim_hand       = 6 ;                                                   %Dimensionality of the task space
0036 settings.dim_joint      = 7 ;                                                   %Dimensionality of the robot joints
0037 settings.joint.free     = setdiff(1:settings.dim_joint, settings.joint.fixed);  %Labels of the free joints
0038 settings.joint.target   = pi/180 * zeros(7,1) ;                                 %Target of the linear policy in joint space
0039 settings.joint.limit    = pi/180 * [170, 120, 170, 120, 170, 120, 170]';        %Limits of each jiont
0040 
0041 settings.end.free       = setdiff(1:settings.dim_hand, settings.end.fixed) ;    %Lables of the free dimensionality of the end effector/task space.
0042 
0043 settings.dim_u          = size(settings.joint.free,2) ;                         %Dimensionality of the control commands
0044 settings.dim_x          = size(settings.joint.free,2) ;                         %Dimensionality of the joint space
0045 settings.dim_r          = size(settings.end.free,2) ;                           %Dmiensionaliry of the task space
0046 
0047 %% set up the nullspace policy
0048 switch (settings.null.type)                                                     %Null space policy types
0049     case 'linear'
0050         settings.null.alpha   = 1 ;                                             %Null space policy scaling
0051         settings.null.target  = settings.joint.target(settings.joint.free) ;
0052         policy_ns             = @(x) policy_linear(x, settings.null) ;
0053     case 'avoidance'
0054         settings.null.alpha   = 1 ;
0055         settings.null.target  = settings.joint.target(settings.joint.free) ;
0056         policy_ns             = @(x) policy_avoidance ( x, settings.null ) ;
0057     case 'learnt'
0058         policy_ns             = settings.null.func ;
0059     otherwise
0060         fprintf('Unkown null-space policy\n') ;
0061 end
0062 
0063 %% set-up the selection matrix for constraints
0064 Lambda = settings.Lambda ;
0065 
0066 rob = dlr_7dof ;
0067 
0068 J   = @(q) jacob0(rob,q) ;
0069 Jx  = @(x) get_jacobian (x, rob, settings) ;
0070 dt  = settings.dt;
0071 Iu  = eye(settings.dim_u) ;
0072 
0073 X = cell(settings.dim_traj,1) ;
0074 Pi= cell(settings.dim_traj,1) ;
0075 U = cell(settings.dim_traj,1) ;
0076 R = cell(settings.dim_traj,1) ;
0077 V = cell(settings.dim_traj,1) ;
0078 
0079 for k=1: settings.dim_traj
0080     
0081     %% get initial posture
0082     q = settings.joint.target ;
0083     q(settings.joint.free) = settings.joint.limit(settings.joint.free).*rand(settings.dim_u,1)- (settings.joint.limit(settings.joint.free)/2) ;
0084     x = q(settings.joint.free);
0085     
0086     X{k}    = zeros(settings.dim_u, settings.dim_step);
0087     U{k}    = zeros(settings.dim_u, settings.dim_step);
0088     Pi{k}   = zeros(settings.dim_u, settings.dim_step);
0089     
0090     for n = 1 : settings.dim_step+1
0091         q(settings.joint.free) = x ;
0092         Jn  = J(q) ;
0093         A   = Lambda * Jn(settings.end.free,settings.joint.free) ;
0094         invA= pinv(A) ;
0095         
0096         P   = Iu - invA*A ;
0097         f   = policy_ns(x) ;
0098         u   = P * f ;
0099         
0100         r   = fkine(rob,q) ;
0101         %             r   = tr2diff(r) ;
0102         r   = r(settings.end.free) ;
0103         
0104         X{k}(:,n)   = x ;
0105         U{k}(:,n)   = u ;
0106         R{k}(:,n)   = r ;
0107         Pi{k}(:,n)  = f ;
0108         x           = x + dt*u;
0109         
0110         if norm(u) < 1e-3
0111             break ;
0112         end
0113     end % end t loop
0114     V{k} = diff(R{k}')' ;
0115     V{k} = V{k}(:,1:n-1) ;
0116     X{k} = X{k}(:,1:n-1);
0117     R{k} = R{k}(:,1:n-1);
0118     U{k} = U{k}(:,1:n-1);
0119     Pi{k}= Pi{k}(:,1:n-1);
0120 end % end k loop
0121 
0122 dataset.X = [X{:}] ;
0123 dataset.U = [U{:}] ;
0124 dataset.Pi= [Pi{:}];
0125 dataset.R = [R{:}] ;
0126 dataset.V = [V{:}] ;
0127 dataset.rob=rob ;
0128 dataset.J = Jx ;
0129 dataset.Lambda= Lambda ;
0130 dataset.settings = settings ;
0131 dataset.settings.dim_n = size(X,2) ;
0132 end
0133 
0134 function Jxn = get_jacobian (x, rob, settings)
0135 % Jxn = get_jacobian (x, rob, settings)
0136 %
0137 % Return a function handle of jacobian calculation
0138 %
0139 % Input:
0140 %   x                   Joint state variables
0141 %   rob                 Robot object
0142 %   settings            Task related parameters
0143 %
0144 % Output:
0145 %   Jxn                 Jacobian function handle
0146 
0147 q = settings.joint.target ;
0148 q(settings.joint.free) = x ;
0149 Jxn = jacob0(rob,q) ;
0150 Jxn = Jxn (settings.end.free, settings.joint.free) ;
0151 end
0152 
0153 function ROBOT=dlr_7dof()
0154 % ROBOT=dlr_7dof()
0155 %
0156 % Create a 7 Dof robot arm
0157 %
0158 % Output:
0159 %   ROBOT                Robot object
0160 
0161 L1 = Link([ 0 0 0 pi/2],   'standard');
0162 L2 = Link([0 0 0.3 -pi/2],'standard');
0163 L3 = Link([0 0 0.4 -pi/2],'standard');
0164 L4 = Link([0 0 0.5 pi/2],'standard');
0165 L5 = Link([0 0 0.39 pi/2],'standard');
0166 L6 = Link([0 0 0 -pi/2],'standard');
0167 L7 = Link([0 0 0.2 0],'standard');
0168 ROBOT = SerialLink([L1,L2,L3,L4,L5,L6,L7], 'name', 'DLR/KUKA');
0169 end
0170 function U = policy_linear(x, null_settings)
0171 % U = policy_linear(x, null_settings)
0172 %
0173 % A linear null space policy
0174 %
0175 % Input:
0176 %   x                   Joint state variable
0177 %   null_settings       Setting parameters for null space policy
0178 % Output:
0179 %   U                   Null space policy function handle
0180 U = null_settings.alpha .* ((null_settings.target - x));
0181 end