[ return ]

grad.m



%
%
%  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 the  vector
%	 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);


[ return ]