directkinematic_5R

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Direct kinematics for the 5R planar parallel robot

   Author: Arturo Gil Aparicio arturo.gil@umh.es
   Date: 13/09/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 % Direct kinematics for the 5R planar parallel robot
0003 %
0004 %   Author: Arturo Gil Aparicio arturo.gil@umh.es
0005 %   Date: 13/09/2013
0006 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0007 
0008 % Copyright (C) 2012, by Arturo Gil Aparicio
0009 %
0010 % This file is part of ARTE (A Robotics Toolbox for Education).
0011 %
0012 % ARTE is free software: you can redistribute it and/or modify
0013 % it under the terms of the GNU Lesser General Public License as published by
0014 % the Free Software Foundation, either version 3 of the License, or
0015 % (at your option) any later version.
0016 %
0017 % ARTE is distributed in the hope that it will be useful,
0018 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0019 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0020 % GNU Lesser General Public License for more details.
0021 %
0022 % You should have received a copy of the GNU Leser General Public License
0023 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0024 
0025 function T=directkinematic_5R(robot, q)
0026 %close all
0027 
0028 a1=eval(robot.robot1.DH.a);
0029 a2=eval(robot.robot2.DH.a);
0030 
0031 %Link lengths
0032 l1=abs(a1(1));
0033 l2=abs(a1(2));
0034 l3=abs(a2(1)); 
0035 l4=abs(a2(2));
0036 L=robot.L;%2.5;
0037 
0038 
0039 xA=cos(q(1));
0040 yA=sin(q(1));
0041 
0042 xB=cos(q(2))+L;
0043 yB=sin(q(2));
0044 
0045 figure,
0046 plot(0,0,'r.'), hold
0047 plot(xA, yA,'k.')
0048 plot(xB, yB,'k.')
0049 plot_line([0 0 0], [xA yA 0], 'r', 2)
0050 plot_line([L 0 0], [xB yB 0], 'r', 2)
0051 %plot(L, 0,'*g')
0052 
0053 
0054 %draw circles around xA, yA and xB yB as a first approximation
0055 x=-0.47:0.001:2;
0056 rr=[];
0057 for i=1:length(x),
0058     r=solve_poly(x(i), xA, yA, l2);    
0059     rr=[rr r];    
0060 end
0061 plot(x, real(rr(1,:)))
0062 plot(x, real(rr(2,:)))
0063 
0064 x=0.5:0.001:3.085;
0065 rr=[];
0066 for i=1:length(x),
0067     r=solve_poly(x(i), xB, yB, l3); 
0068     rr=[rr r];    
0069 end
0070 plot(x, real(rr(1,:)), 'b')
0071 plot(x, real(rr(2,:)), 'b')
0072 
0073 
0074 %compute polynomial in terms of alpha, beta and delta
0075 af=xA-xB;
0076 be=l2^2-l1^2-l3^2+xB^2+yB^2;
0077 de=(yB-yA);
0078 
0079 a=(de^2+af^2);
0080 b=af*be-2*xA*de^2-2*yA*af*de;
0081 c=de^2*(xA^2+yA^2)+be^2/4-yA*be*de-l2^2*de^2;
0082 
0083 %two different solutions for x
0084 x1=(-b+sqrt(b^2-4*a*c))/(2*a);
0085 x2=(-b-sqrt(b^2-4*a*c))/(2*a);
0086 
0087 %find y from polynomial, given x1 and x2
0088 r1=solve_poly(x1, xA, yA, l2);
0089 r2=solve_poly(x2, xA, yA, l2);
0090 r3=solve_poly(x1, xB, yB, l3);
0091 r4=solve_poly(x2, xB, yB, l3);
0092 
0093 %find those solutions that comply with both equations
0094 R1=[r1; r2];
0095 R2=[r3; r4];
0096 %to do this, obtain only those roots that are repeated
0097 y=[];
0098 for i=1:length(R1),
0099    val=is_double_root(R1(i),R2);
0100    if val==1
0101        y=[y R1(i)];
0102    end
0103 end
0104 
0105 %Plot error if an unfeasible solution is found
0106 if (~isreal(y))
0107     disp('ERROR: directkinematic_5R: unfeasible solution');
0108 end
0109 
0110 y1=y(1);
0111 y2=y(2);
0112 
0113 %return solutions in homogeneous matrices
0114 T1=eye(4);
0115 T2=eye(4);
0116 
0117 T1(1,4)=x1;
0118 T1(2,4)=y1;
0119 T2(1,4)=x2;
0120 T2(2,4)=y2;
0121 T=[];
0122 T=[T1 T2];
0123 
0124 % avoid warnings for imaginary parts when plottin
0125 % the above message should be triggered when an unfeasible solution is
0126 % found
0127 y1=real(y(1));
0128 y2=real(y(2));
0129 x1=real(x1);
0130 x2=real(x2);
0131 
0132 %plot solutions
0133 plot_line([xA yA 0], [x1 y1 0], 'y', 2)
0134 plot_line([xB yB 0], [x1 y1 0], 'y', 2)
0135 
0136 plot_line([xA yA 0], [x2 y2 0], 'g', 2)
0137 plot_line([xB yB 0], [x2 y2 0], 'g', 2)
0138 
0139 
0140 
0141 function r=solve_poly(x, xAB, yAB, L)
0142 a=1;
0143 b=-2*yAB;
0144 c=x^2-2*x*xAB-L^2+xAB^2+yAB^2;
0145 
0146 r =[(-b+sqrt(b^2-4*a*c))/(2*a); (-b-sqrt(b^2-4*a*c))/(2*a)];
0147 
0148 
0149 
0150 function yes_no=is_double_root(R1_i,R2)
0151 thres=0.0001;
0152 [j, k, v]=find(abs(R2-R1_i)<thres);
0153 
0154 %in case a match has been found
0155 if length(j)>0
0156         yes_no=1;
0157 else
0158         yes_no=0;
0159 end
0160 
0161 
0162 
0163 
0164 function plot_line(p0, p1, color, w)
0165 x0 = p0(1);
0166 y0 = p0(2);
0167 z0 = p0(3);
0168 x1 = p1(1);
0169 y1 = p1(2);
0170 z1 = p1(3);
0171 % Draw a line between p0 and p1
0172 plot3([x0;x1],[y0;y1],[z0;z1], color, 'LineWidth',w);   
0173 
0174