% % % file: grad.m % % Given an mcx specification and a desired output, supply % a set of weights to the different mcx units that produce % the desired output. % % Step 1: remove all mcx units that can't possibly participate. % These are units that send outputs to joints that we do % not want to specify a value for. This is accomplished % by stripping out the rows from both thevector % and the matrix that correspond to joints that we % do not want to specify a value for, and the columns of % that correspond to the mcx units that can't participate. % % Step 2: The pseudo-inverse of the m_new provides an initial % approximation to the output weights. However, some % of the weights might be negative. % % Step 3: Repair the negative weights by doing a pseudo gradient % search in the null-space, using the cost function: % cost = sum (ramp(-weight_i)). % where ramp is 0 for non-positive inputs % and equal to the input value otherwise % % Because ramp is not differentiable, we run the risk % of going in circles. However, the psuedo-inverse gives % us a good enough starting point that this is not a % signification problem in practice. % % Step 4: Generate a weight vector that corresponds to the full % set of mcx units. % clear all; getmcx; % Motor cortex matrix getdesired; % Desired position threshold = -0.001; step = 0.1; max_iterations = 1000; % Maximum number of steps in the null-space search max_attempts = 10; % Maximum number of tries at randomly selecting % a set of mcx units (columns of ). output_tolerance = 0.01;% Max difference between desired and actual output. % This message indicates no solution found. % We do this here just in case matlab pukes in the middle. [fid, message] = fopen('f5.w', 'w'); % Check status if(~strcmp(message,'')) disp('Error opening output file.'); return; end; fprintf(fid, 'w no solution\n'); fclose(fid); rand('seed', random_seed); % Random seed is supplied by getdesired.m % Check to make sure the mcx matrix has the right rank. if rank(m) ~= (numjoints * 2) disp(['Mcx matrix rank not ', num2str(numjoints*2), ' (is only ', num2str(rank(m)), ')']); return; end; attempts = 0; while(attempts < max_attempts) % Step 1 % Strip out the 'degree' element for each joint % (desired vector). desired_degree = desired([2:2:size(desired,1)]); ind = [1:size(desired_degree, 1)]; % List (indexes of) of degree elements that are 0. desired_zeroed = ind(desired_degree == 0); % List (indexes of) of degree elements that are non-0. desired_other = ind(desired_degree ~= 0); % Pull out degrees for each cell m_degree = m([2:2:size(desired,1)],:); % Take out rows that correspond to zero degree (desired vec) m_zeroed = m_degree(desired_zeroed,:); ind = [1:size(m, 2)]; error_flag = 0; % Will be set if an error occurs. attempts = attempts + 1; % List of columns that may be used. % We are filtering both infeasible columns, as well % as randomly pruning some columns. rn = rand(1,size(m_zeroed,2)); tmp_rn = (all(m_zeroed == 0) == 1) & (rn < connect_prob); if(all(tmp_rn == 0)) error_flag = 1; % This will force the following % while loop to fail. Sorry about % the funky blocking. disp('Error: all columns selected out'); break; else cols = ind(tmp_rn); % Now construct a new 'desired' and a new 'm' with % offending rows and columns stripped out from the % originals. exp_desired_other = zeros(1,length(desired_other)*2); exp_desired_other([1:2:length(desired_other)*2]) = ... desired_other*2-1; exp_desired_other([2:2:length(desired_other)*2]) = ... desired_other*2; % New m matrix with rows and columns stripped m_new = m(exp_desired_other,cols); % New desired vector with rows stripped. desired_new = desired(exp_desired_other); % Step 2 % Compute pseudo-inverse and the corresponding solution in = pinv(m_new); % Initial position weights = in * desired_new; % Null space. n = null(m_new); % Which weight elements are non-negative. cmp = (weights >= threshold); % Loop as long as there are some negative weights iter = 0; end; while ~error_flag & ~all(cmp) % disp('ITERATION'); % disp(weights'); % If no null space to search, then terminate. if(n == []) error_flag = 1; disp('Null space is nil'); break; end; c = 1-cmp; % Current 'cost' % disp(sum(weights(c'>0))); ind = [1:size(n,1)]; % Indices of elements that are negative sub = ind(c'>0); % Pull out null-space rows grd = n(sub,:); % Compute the gradient in the null space. if size(grd,1) == 1 y = grd; else y = sum(grd); end % Now update the weights weights = weights + step * n * y'; cmp = (weights >= threshold); iter = iter+1; if(iter == max_iterations) disp('TOO MANY ITERATIONS'); disp(['Final Error: ', sum(weights(c'>0))]); error_flag = 1; break; end; end; if(~error_flag) % No errors % Now reconstruct actual weight vector weights_out = zeros(size(m,2),1); weights_out(cols) = weights; % Zero out weights that are still a little negative ind = [1:size(weights_out,1)]; rows = ind(weights_out < 0); weights_out(rows,:) = zeros(length(rows),1); disp('CHECK'); disp(m * weights_out); % Check to see if final solution is good enough if(~all(abs(desired - m * weights_out) < output_tolerance)) disp('Too much error in final solution.'); error_flag = 1; else % Generate the output and quit. disp(['DONE, ', num2str(iter), ' iterations']); disp(weights_out); % Open output file [fid, message] = fopen('f5.w', 'w'); % Check status if(~strcmp(message,'')) disp('Error opening output file.'); return; end; % File header fprintf(fid, 'w solution\n'); % Write weight matrix for i = 1:size(weights_out,1) fprintf(fid, '%f\n', weights_out(i,1)); end; fclose(fid); return; end; end; end; disp('NO SOLUTION FOUND - exiting'); % Could not find a solution - report this [fid, message] = fopen('f5.w', 'w'); % Check status if(~strcmp(message,'')) disp('Error opening output file.'); return; end; fprintf(fid, 'w no solution\n'); fclose(fid);