robotxy.m

very excellent 2 degree of freedom two link robot inverse kinematic. its graphically good.

%solve('(q1-xs)^2+(q2-ys)^2=l1^2','(xo-xs)^2+(yo-ys)^2=l2^2','xs','ys')
clc
disp('Automatic Optimal Control Equation Driver.');
disp('Defining Parameters...');
syms xo yo l1 l2 I1 I2 m1 m2 r1 g
syms q1 q2 qd1 qd2 u1 u2
xs1=-1/2*(-yo*(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2)*(l2^2-2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2))^(1/2)-q1^2*q2^2-q1^2*yo^2+xo^2*q2^2+xo^2*yo^2-2*yo*xo^2*q2+q2*(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2)*(l2^2-2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2))^(1/2)-q1^4+xo^4+2*xo*q1^3-2*xo^3*q1+q1^2*l1^2-q1^2*l2^2+xo^2*l1^2-xo^2*l2^2-2*xo*q1*l1^2+2*xo*q1*l2^2+2*q1^2*q2*yo)/(q2^2+xo^2+yo^2-2*q2*yo-2*xo*q1+q1^2)/(-xo+q1);
xs2=-1/2*(yo*(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2)*(l2^2-2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2))^(1/2)-q1^2*q2^2-q1^2*yo^2+xo^2*q2^2+xo^2*yo^2-2*yo*xo^2*q2-q2*(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2)*(l2^2-2*l1*l2+l1^2-yo^2+2*q2*yo-q2^2-q1^2+2*xo*q1-xo^2))^(1/2)-q1^4+xo^4+2*xo*q1^3-2*xo^3*q1+q1^2*l1^2-q1^2*l2^2+xo^2*l1^2-xo^2*l2^2-2*xo*q1*l1^2+2*xo*q1*l2^2+2*q1^2*q2*yo)/(q2^2+xo^2+yo^2-2*q2*yo-2*xo*q1+q1^2)/(-xo+q1);
ys1=-1/2*(l2^2*yo-q2^3-q1^2*yo-yo^3+q2^2*yo+2*xo*q1*q2-xo^2*q2-l1^2*yo-xo^2*yo-q1^2*q2+q2*yo^2+2*xo*q1*yo+q2*l1^2-q2*l2^2-(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-xo^2-yo^2+2*xo*q1-q1^2+2*q2*yo-q2^2)*(l2^2-2*l1*l2+l1^2-xo^2-yo^2+2*xo*q1-q1^2+2*q2*yo-q2^2))^(1/2))/(q2^2+xo^2+yo^2-2*q2*yo-2*xo*q1+q1^2);
ys2=-1/2*(l2^2*yo-q2^3-q1^2*yo-yo^3+q2^2*yo+2*xo*q1*q2-xo^2*q2-l1^2*yo-xo^2*yo-q1^2*q2+q2*yo^2+2*xo*q1*yo+q2*l1^2-q2*l2^2+(-(-xo+q1)^2*(l2^2+2*l1*l2+l1^2-xo^2-yo^2+2*xo*q1-q1^2+2*q2*yo-q2^2)*(l2^2-2*l1*l2+l1^2-xo^2-yo^2+2*xo*q1-q1^2+2*q2*yo-q2^2))^(1/2))/(q2^2+xo^2+yo^2-2*q2*yo-2*xo*q1+q1^2);
xs=xs1; ys=ys1;

xsd=simplify(diff(xs,q1)*qd1+diff(xs,q2)*qd2);
ysd=simplify(diff(ys,q1)*qd1+diff(ys,q2)*qd2);
K=1/2*I1*(xsd^2+ysd^2)/l1^2+1/2*I2*((qd1-xsd)^2+(qd2-ysd)^2)/l2^2+1/2*m2*((qd1+xsd)^2+(qd2+ysd)^2);
P=m1*g*ys/2+m2*g*(q2+ys)/2;
L=K-P;
DOF=2;
H='u1^2+u2^2';
 

% if you don't want symbolic set this parameter to zero and then define
% your parameters numerically.
%this menu enable us to improve speed in calculation.
Torquesubs=1;
%in this section you should put value of parameters
for i=1:DOF
  ii=int2str(i);jj=int2str(i+DOF);
  eval(strcat('syms q',ii,' qd',ii,' qdd',ii,' p',ii,' p',jj));
end

%definition of inputs
disp('Defined.');
disp('Extracting Dynamic Equations...');
Ls='';
qdds='';
for i=1:DOF
   ii=int2str(i);
   disp(strcat('Degree:',ii));
   disp('Differentiating 1');
   eval(strcat('A',ii,'=diff(L,qd',ii,');'));
   disp('Differentiating 2');
   eval(strcat('B',ii,'=diff(L,q',ii,');'));
   eval(strcat('Ap',ii,'=0;'));
   for j=1:DOF
     jj=int2str(j);
     disp(strcat('SubDegree:',jj));
     disp('Differentiating 1');
     eval(strcat('Ap',ii,'=Ap',ii,'+diff(A',ii,',q',jj,')*qd',jj,';'));
     disp('Differentiating 2');
     eval(strcat('Ap',ii,'=Ap',ii,'+diff(A',ii,',qd',jj,')*qdd',jj,';'));
   end
   disp('Simpling...');
%    eval(strcat('L',ii,'=simplify(u',ii,'-Ap',ii,'+B',ii,');'));
   eval(strcat('L',ii,'=(u',ii,'-Ap',ii,'+B',ii,');'));
   disp('Done.');
   Ls=strcat(Ls,'L',ii',',');
   qdds=strcat(qdds,'qdd',ii);
   if i<DOF
    qdds=strcat(qdds,',');
   end
end
eval(strcat('Qdd=solve(',Ls,qdds,');'));
disp('Equations Extracted.');
disp('Simpling...');
Us='';
if DOF>1
 for i=1:DOF
  ii=int2str(i);
  eval(strcat('Qdd.qdd',ii,'=simplify(Qdd.qdd',ii,');'));
  Us=strcat(Us,'u',ii);
  if i<DOF
    Us=strcat(Us,',');
  end
 end
else
 Us=u1;
 Qdd=simplify(Qdd);
end
disp('Making Hamiltonian Equation...');
%is now time to define a Hamiltonian.
if DOF>1
 for i=1:DOF
  ii=int2str(i);
  jj=int2str(i+DOF);
  eval(strcat('H=H+p',ii,'*q',ii,'+p',jj,'*Qdd.qdd',ii,';'));
 end
else
 H=H+p1*qd1+p2*Qdd;
end;
disp('Done. DO Euler... '); 
%actify the Euler-Lagrange Formula on Hamiltonian
if DOF>1
 for i=1:DOF
  ii=int2str(i);        %number of qi
  jj=int2str(i+DOF);    %number of qdi
  eval(strcat('pd',ii,'=-diff(H,q',ii,');'));
  eval(strcat('pd',jj,'=-diff(H,qd',ii,');'));
  eval(strcat('u',ii,'=solve(diff(H,u',ii,'),u',ii,');'));
  if Torquesubs
   eval(strcat('pd',ii,'=subs(pd',ii,',''[',Us,']'',[',Us,']);'));
   eval(strcat('pd',jj,'=subs(pd',jj,',''[',Us,']'',[',Us,']);'));
   eval(strcat('Qdd.qdd',ii,'=subs(pd',ii,',''[',Us,']'',[',Us,']);'));
  end 
 end
else
 %Simplified for one Degree of Freedom
 pd1=-diff(H,q1);
 pd2=-diff(H,qd1);
 u1=solve(diff(H,u1),u1);
 if Torquesubs
  pd1=subs(pd1,'u1',u1);
  pd2=subs(pd2,'u1',u1);
  Qdd=subs(Qdd,'u1',u1);
 end
end
disp('Done. Simpling... '); 
%This Part is for converting from symboic to characteric process
if DOF>1
 for i=1:DOF
  ii=int2str(i);jj=int2str(i+DOF);kk1=int2str(i+2*DOF);kk2=int2str(i+3*DOF);
  eval(strcat('u',ii,'=char(simplify(u',ii,'));'));
  eval(strcat('d',ii,'=''qd',ii,''';'));
  eval(strcat('d',jj,'=char(simplify(Qdd.qdd',ii,'));'));
  eval(strcat('d',kk1,'=char(simplify(pd',ii,'));'));
  eval(strcat('d',kk2,'=char(simplify(pd',jj,'));'));
 end
else
%simplified for ONE Degree Of Freedom
    u1=char(simplify(u1));
  d1='qd1';
  d2=char(simplify(Qdd));
  if pd1~=0
    d3=char(simplify(pd1));
  else
    d3='0';
  end
  if pd2~=0
    d4=char(simplify(pd2));
  else
    d4='0';
  end
end

% Very Advanced Matlab code for get away from hard working for DOF=1 and
% more!
disp('Done. Making File... '); 
for i=1:2*DOF
 for j=1:4*DOF
  eval(strcat('d',int2str(j),' = regexprep(d',int2str(j),',strcat(''p'',int2str(i)), strcat(''x('',int2str(i+',int2str(2*DOF),'),'')''));'));
  if (j<=DOF)
   eval(strcat('u',int2str(j),' = regexprep(u',int2str(j),',strcat(''p'',int2str(i)), strcat(''x('',int2str(i+',int2str(2*DOF),'),'')''));'));         
  end
 end
 if i<=DOF 
  for j=1:4*DOF
   eval(strcat('d',int2str(j),' = regexprep(d',int2str(j),',strcat(''q'',int2str(i)), strcat(''x('',int2str(i),'')''));'));
   eval(strcat('d',int2str(j),' = regexprep(d',int2str(j),',strcat(''qd'',int2str(i)), strcat(''x('',int2str(i+',int2str(DOF),'),'')''));'));
   if j<=DOF
    eval(strcat('u',int2str(j),' = regexprep(u',int2str(j),',strcat(''q'',int2str(i)), strcat(''x('',int2str(i),'')''));'));
    eval(strcat('u',int2str(j),' = regexprep(u',int2str(j),',strcat(''qd'',int2str(i)), strcat(''x('',int2str(i+',int2str(DOF),'),'')''));'));
   end
  end
 end
end

%This part for write a function into the file
fid = fopen('matnode.m', 'wt');
fprintf(fid, 'function dxdt = mat8ode(t,x)\n');
if symbolic
 eval(strcat('fprintf(fid, ''global',symParams,'\n'');')); 
end
if ~Torquesubs
  for i=1:DOF
    ii=int2str(i);
    eval(strcat('fprintf(fid, strcat(''u',ii,'='',char(u',ii,'),'';\n''));'));
  end
end
fprintf(fid, strcat('dxdt=zeros(',int2str(4*DOF),',1);\n'));
for i=1:4*DOF
    ii=int2str(i);
    eval(strcat('fprintf(fid,strcat(''dxdt(',ii,')='',char(d',ii,'),'';\n''));'));
end
fprintf(fid,'end');
fclose(fid);
disp('Complete. ');