% solution of the optimization task 
% consisting of three moment equations
% the number of the muscles is N
% the objective functions is a quadratic function 
% of the type Z=sum(c(i)F(i)^2) and the sumation
% is over all muscles (i=1,2,   ...N)
% c(i) can be different quantities
% for current calculations c(i)=1/PCSA(i)^2

% first moment equation
% d1(1)*F(1)+d1(2)*F(2)+d1(3)*F(3)+ .....d1(N)*F(N)=M1
% second moment equation
% d2(1)*F(1)+d2(2)*F(2)+d2(3)*F(3)+ .....d2(N)*F(N)=M2
% third moment equation
% d3(1)*F(1)+d3(2)*F(2)+d3(3)*F(3)+ .....d3(N)*F(N)=M3
% if some of the forces does not act in some of the joint 
% its respective moment arm is zero 
% the program can be used for one or two degrees of freedom also
% then some of the moment equations must contains only one force
% that does not appear in the other equations
% for help please write an E-mail to: Rosi.Raikova@clbme.bas.bg

% for an example N=9, everyone can change this number bellow

clear all
disp ('number of the muscles')

N=9 % number of the muscles, you may change this value

PCSA=zeros(N);c=zeros(N);d1=zeros(N);d2=zeros(N);d3=zeros(N);



% for an example joint moments have the values given below
% everyone can change these constants according to his model

M1=-19; % first joint moment
M2=-45; % second joint moment
M3=-70; % third joint moment


% phisiological cross-sectional areas of the muscles

PCSA(1)=11.5;
PCSA(2)=92.5;
PCSA(3)=44.3;
PCSA(4)=98.1;
PCSA(5)=20.1;
PCSA(6)=6.1;
PCSA(7)=45.5;
PCSA(8)=31.0;
PCSA(9)=44.3;

% weight coefficients in the objective function are 1/(PCSA(i))^2
for i=1:N;
c(i)=1/(PCSA(i)^2);
end


% muscle lever arms with respect to the first joint 
% everyone can change these data - only the nonzero arms can be written 
% the example values are taken from Raikova and Prilutsky,
% J.Biomechanics (2001) vol. 34, 1243-1255

d1(1)=0.0298;
d1(2)=-0.044;
d1(3)=-0.044;


% muscle lever arms with respect to the second joint 
d2(3)=-0.0138;
d2(4)=0.0329;
d2(5)=0.0329;
d2(6)=-0.025;
d2(7)=-0.025;


% muscle lever arms with respect to the third joint 
d3(5)=0.0279;
d3(7)=-0.0619;
d3(8)=0.0317;
d3(9)=-0.0368;

% computations
% in the matrix m all combinations of 0 and 1 in a vector with N components are stored

zmin=10^20;
Fmin=zeros(N);
landa1min=0; landa2min=0.0; landa3min=0.0;
m = zeros(1,N);
p = 0;
while p==0, 
 p=1;
 for i=N:-1:1
  digit = m(i);
  m(i) = rem(m(i)+p,2);
  if (digit+p)==2
   p = 1;
  else
   p = 0;
  end;
 end;
 m;

 numpossol=0;

 for j=1:N;
  d1loop(j)=d1(j)*m(j);
  d2loop(j)=d2(j)*m(j);
  d3loop(j)=d3(j)*m(j);
 end

 s1=0.0; s2=0.0; s3=0.0; s4=0.0; s5=0.0; s6=0.0;

 for j=1:N;
  s1=s1+d1loop(j)*d1loop(j)/c(j);
  s2=s2+d1loop(j)*d2loop(j)/c(j);
  s3=s3+d1loop(j)*d3loop(j)/c(j);
  s4=s4+d2loop(j)*d2loop(j)/c(j);
  s5=s5+d2loop(j)*d3loop(j)/c(j);
  s6=s6+d3loop(j)*d3loop(j)/c(j);
 end

% the system for obtaining landai is:
% landa1*s1+landa2*s2+landa3*s3=2M1
% landa1*s2+landa2*s4+landa3*s5=2M2
% landa1*s3+landa2*s5+landa3*s6=2M3

 if s1~=0

  p1=s4-s2*s2/s1;

  if p1~=0

   p2=s5-s3*s2/s1;
   p3=2*M2-2*M1*s2/s1;
   p4=s6-s3*s3/s1;
   p5=2*M3-2*M1*s3/s1;
   landa3=(p5-p2*p3/p1);

   if (p4-p2*p2/p1)~=0

    landa3=landa3/(p4-p2*p2/p1);
    landa2=(p3-landa3*p2)/p1;
    landa1=(2*M1-landa2*s2-landa3*s3)/s1;


    for j=1:N;
     F(j)=(landa1*d1loop(j)+landa2*d2loop(j)+landa3*d3loop(j))/(2*c(j));
     if F(j)<0
      numpossol=1;
     end
    end 

% check of the moment equations
    jointmom1=0.0; jointmom2=0.0; jointmom3=0.0;

    for j=1:N;
     jointmom1=jointmom1+d1(j)*F(j);
     jointmom2=jointmom2+d2(j)*F(j);
     jointmom3=jointmom3+d3(j)*F(j);
    end

    if (abs(jointmom1-M1))>0.0001
     numpossol=numpossol+1;
    end;
    if (abs(jointmom2-M2))>0.0001
     numpossol=numpossol+1;
    end;
    if (abs(jointmom3-M3))>0.0001
     numpossol=numpossol+1;
    end;
 
   
    if numpossol==0
     % disp ('posible solution')
  
      F';
     jointmom1;
     jointmom2;
     jointmom3;
% if one is interested to see all possible solutions which satisfied
% eguality constraint (joint moment equations) and inequality constraints
% for positiveness of all muscle forces  remove ; in the F' above 
     z=0;
     for j=1:N
      z=z+c(j)*F(j)*F(j);
     end
     z;

     landa1;
     landa2;
     landa3;

     if z<zmin
      zmin=z;
      Fmin=F;
      landa1min=landa1; landa2min=landa2; landa3min=landa3;
     end

    end; %posible solution
% pause;
   end; % p4-p2*p2/p1
  end; % p1~=0
 end; % s1~=0
end; %while p==0 

disp ('     OPTIMAL SOLUTION')
disp ('   OPTIMAL MUSCLE FORCES')
Fmin'

disp ('THE VALUE OF THE OBJECTIVE FUNCTION')
Zminimum=zmin

disp ('Lagrange multipliers')
landa1=landa1min
landa2=landa2min
landa3=landa3min

