function [A,B,ur] = linpendu(Theta,q1,q2)
%
%  Format: [A,B,ur] = linpendu(Theta,q1,q2)
%
%  This function finds the linear model for the pendubot about the operating
%  point {q1,0,q2,0,Theta(4)*g*cos(q1)}.  This function uses SI units
%  so g is equal to 9.804 m/s^2.
%
% Inputs:
%    1. Theta:  vector containing pendubots parameters:
%                Theta(1) =  m1*lc1^2 + m2*l1^2 + I1
%                Theta(2) =  m2*lc2^2 + I2
%                Theta(3) =  m2*l1*lc2
%                Theta(4) =  m1*lc1 + m2*l1
%                Theta(5) =  m2*lc2
%
%    2. q1 and q2 are the desired operating points and their sum should follow:
%       |q1 + q2| = pi/2
%
% Outputs:
%    1. A and B the matrices in the linear equation for the Pendubot
%          delta_xdot = A*delta_x + B*delta_tau;
%    2. ur: Torque needed to hold linkage at operating point

allok = 1;
if nargin ~= 3
   allok = 0;
   disp('linpendu needs 3 input arguments');
end
if nargout ~= 3
   allok = 0;
   disp('linpendu needs 3 output variables');
end
if allok == 1
  [m,n] = size(Theta);
  if ((m ~= 5 | n ~= 1) & (n ~= 5 | m ~= 1))
     allok = 0;
     disp('The dimension of Theta is incorrect; Theta must have 5 elements');
  end
end
if allok == 1
  [m,n] = size(q1);
  if (m ~= 1 | n ~= 1)
     allok = 0;
     disp('The dimension of q1 is incorrect; q1 must be a scaler');
  end
end
if allok == 1
  [m,n] = size(q2);
  if (n ~= 1 | m ~= 1)
     allok = 0;
     disp('The dimension of q2 is incorrect; q2 must be a scaler');
  end
end
if allok == 1
  if (abs(abs(q1 + q2) - pi/2) > .1)
    allok = 0;
    disp('Your operating point does not follow the rule |q1 + q2| = pi/2');
  end
end
if allok == 1
   g=9.804;
   X1 = Theta(1);
   X2 = Theta(2);
   X3 = Theta(3);
   X4 = Theta(4);
   X5 = Theta(5);
   ur=X4*g*cos(q1);
   den1=2*X1*X2-X3^2-X3^2*cos(2*q2);
   df2dx1=(g*(2*X2*X4*sin(q1)-X3*X5*sin(q1)-X3*X5*sin(q1+2*q2)))/den1;
   equ1=X1*X2-(X3^2)/2-(X3^2*cos(2*q2))/2;
   equ2=X2*ur-g*(X2*X4*cos(q1)+(X3*X5*cos(q1))/2+(X3*X5*cos(q1+2*q2))/2);
   df2dx3=-((X3^2*equ2*sin(2*q2))/equ1^2) + (2*g*X3*X5*sin(q1+2*q2))/(-den1);
   df4dx1=(g*X5*(X1+X2+2*X3*cos(q2))*sin(q1+q2)-g*(X2+X3*cos(q2))*(X4*sin(q1)+X5*sin(q1+q2)))/equ1;
   equ3=g*X5*(X1+X2+2*X3*cos(q2))*cos(q1+q2);
   equ4=(X2+X3*cos(q2))*(-ur+g*X4*cos(q1)+g*X5*cos(q1+q2));
   equ5=(g*X3*X4*sin(q1-q2))/2+X3*ur*sin(q2)-(g*X3*X4*sin(q1+q2))/2;
   equ6=g*(X1*X5*sin(q1+q2)+X3*X5*sin(q1+2*q2));
   df4dx3=-(X3^2*(-equ3+equ4)*sin(2*q2))/equ1^2+(equ5+equ6)/equ1;
   df2du=X2/equ1;
   df4du=(-X2-X3*cos(q2))/equ1;
   A(2,1) = df2dx1;
   A(2,3) = df2dx3;
   A(4,1) = df4dx1;
   A(4,3) = df4dx3;
   B(2,1) = df2du;
   B(4,1) = df4du;
   A(1,2) = 1;
   A(3,4) = 1;
else
   disp('Error in variables entered type "help linpendu" for more info');
end
