directkinematic_3RRR

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Inverse kinematics for the 3RRR planar parallel robot
 

 DIRECTKINEMATIC_3RRR(robot, Q)
 T: homogeneous matrix
 robot: structure with arm parameters
 returns: all possible solutions for T = [Q1 Q2 Q3 Q4 Q5 Q6) 
 that can be achieved with the joint angles specified by Q, being Q the
 set of active joints in the mechanism.

 
 
   Author: Arturo Gil Aparicio arturo.gil@umh.es
   Date: 08/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 % Inverse kinematics for the 3RRR planar parallel robot
0003 %
0004 %
0005 % DIRECTKINEMATIC_3RRR(robot, Q)
0006 % T: homogeneous matrix
0007 % robot: structure with arm parameters
0008 % returns: all possible solutions for T = [Q1 Q2 Q3 Q4 Q5 Q6)
0009 % that can be achieved with the joint angles specified by Q, being Q the
0010 % set of active joints in the mechanism.
0011 %
0012 %
0013 %
0014 %   Author: Arturo Gil Aparicio arturo.gil@umh.es
0015 %   Date: 08/03/2012
0016 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0017 
0018 % Copyright (C) 2012, by Arturo Gil Aparicio
0019 %
0020 % This file is part of ARTE (A Robotics Toolbox for Education).
0021 %
0022 % ARTE is free software: you can redistribute it and/or modify
0023 % it under the terms of the GNU Lesser General Public License as published by
0024 % the Free Software Foundation, either version 3 of the License, or
0025 % (at your option) any later version.
0026 %
0027 % ARTE is distributed in the hope that it will be useful,
0028 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0029 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0030 % GNU Lesser General Public License for more details.
0031 %
0032 % You should have received a copy of the GNU Leser General Public License
0033 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0034 function T = directkinematic_3RRR(robot, q)
0035 
0036 fprintf('\nComputing direct kinematics for the %s robot', robot.name);
0037 
0038 %theta1, theta2 and theta3
0039 t1d=q(1);
0040 t2d=q(2);
0041 t3d=q(3);
0042 
0043 %xq, yq is the position of the second arm in the mechanism.
0044 xq=robot.robot2.T0(1, 4);
0045 yq=robot.robot2.T0(2, 4);
0046 
0047 %xr, yr is the position of the third arm in the mechanism.
0048 xr=robot.robot3.T0(1, 4);
0049 yr=robot.robot3.T0(2, 4);
0050 
0051 %set the geometrical parameters.
0052 a=eval(robot.robot1.DH.a);%(1);
0053 a1=a(1);
0054 b1=a(2);
0055 a=eval(robot.robot2.DH.a);
0056 a2=a(1);
0057 b2=a(2);
0058 a=eval(robot.robot3.DH.a);
0059 a3=a(1);
0060 b3=a(2);
0061 
0062 %side of the triangle
0063 h=robot.h;
0064 
0065 %coefficient vector
0066 c=[];
0067 
0068 c(1)=-2*a1*cos(t1d);
0069 c(2)=-2*a1*sin(t1d);
0070 c(3)=a1*a1-b1*b1;
0071 c(4)=-2*xq-2*a2*cos(t2d);
0072 c(5)=2*h;
0073 c(6)=-2*yq-2*a2*sin(t2d);
0074 c(7)=2*h;
0075 c(8)=xq*xq+yq*yq+a2*a2+h*h+2*xq*a2*cos(t2d)+2*yq*a2*sin(t2d)-b2*b2;
0076 c(9)=-2*xq*h-2*a2*h*cos(t2d);
0077 c(10)=-2*yq*h-2*a2*h*sin(t2d);
0078 c(11)=-2*xr-2*a3*cos(t3d);
0079 c(12)=h;
0080 c(13)=-sqrt(3)*h;
0081 c(14)=-2*yr-2*a3*sin(t3d);
0082 c(15)=sqrt(3)*h;
0083 c(16)=h;
0084 c(17)=xr*xr+yr*yr+a3*a3+h*h+2*xr*a3*cos(t3d)+2*yr*a3*sin(t3d)-b3*b3;
0085 c(18)=-xr*h-yr*sqrt(3)*h-a3*cos(t3d)*h-a3*sin(t3d)*sqrt(3)*h;
0086 c(19)=sqrt(3)*h*xr-yr*h+a3*cos(t3d)*sqrt(3)*h-a3*sin(t3d)*h;
0087 c(20)=c(1)-c(4);
0088 c(21)=-c(5);
0089 c(22)=c(2)-c(6);
0090 c(23)=-c(7);
0091 c(24)=c(3)-c(8);
0092 c(25)=-c(9);
0093 c(26)=-c(10);
0094 c(27)=c(1)-c(11);
0095 c(28)=-c(12);
0096 c(29)=-c(13);
0097 c(30)=c(2)-c(14);
0098 c(31)=-c(15);
0099 c(32)=-c(16);
0100 c(33)=c(3)-c(17);
0101 c(34)=-c(18);
0102 c(35)=-c(19);
0103 c(36)=c(21)*c(31);
0104 c(37)=c(21)*c(32)-c(23)*c(28);
0105 c(38)=c(20)*c(31)+c(21)*c(30)-c(22)*c(28);
0106 c(39)=-c(23)*c(29);
0107 c(40)=c(20)*c(32)-c(22)*c(29)-c(23)*c(27);
0108 c(41)=c(20)*c(30)-c(22)*c(27);
0109 c(42)=-c(25)*c(31);
0110 c(43)=c(23)*c(35)-c(26)*c(32);
0111 c(44)=c(23)*c(34)-c(25)*c(32)-c(26)*c(31);
0112 c(45)=c(22)*c(34)-c(24)*c(31)-c(25)*c(30);
0113 c(46)=c(22)*c(35)+c(23)*c(33)-c(24)*c(32)-c(26)*c(30);
0114 c(47)=c(22)*c(33)-c(24)*c(30);
0115 c(48)=c(25)*c(28)-c(21)*c(34);
0116 c(49)=c(26)*c(29);
0117 c(50)=-c(21)*c(35)+c(25)*c(29)+c(26)*c(28);
0118 c(51)=-c(20)*c(34)-c(21)*c(33)+c(24)*c(28)+c(25)*c(27);
0119 c(52)=-c(20)*c(35)+c(24)*c(29)+c(26)*c(27);
0120 c(53)=c(24)*c(27)-c(20)*c(33);
0121 c(54)=c(1)*c(36)*c(42)+c(2)*c(36)*c(48)+c(3)*c(36)*c(36)+c(42)*c(42)+c(48)*c(48);
0122 c(55)=c(1)*(c(36)*c(44)+c(37)*c(42))+c(2)*(c(36)*c(50)+c(37)*c(48))+2*c(3)*c(36)*c(37)+2*c(42)*c(44)+2*c(48)*c(50);
0123 c(56)=c(1)*(c(36)*c(45)+c(38)*c(42))+c(2)*(c(36)*c(51)+c(38)*c(48))+2*c(3)*c(36)*c(38)+2*c(42)*c(45)+2*c(48)*c(51);
0124 c(57)=c(1)*(c(36)*c(43)+c(37)*c(44)+c(39)*c(42))+c(2)*(c(36)*c(49)+c(37)*c(50)+c(39)*c(48))+c(3)*(2*c(36)*c(39)+c(37)*c(37))+2*c(42)*c(43)+c(44)*c(44)+2*c(48)*c(49)+c(50)*c(50);
0125 c(58)=c(1)*(c(36)*c(46)+c(37)*c(45)+c(38)*c(44)+c(40)*c(42))+c(2)*(c(36)*c(52)+c(37)*c(51)+c(38)*c(50)+c(40)*c(48))+c(3)*(2*c(36)*c(40)+2*c(37)*c(38))+2*c(42)*c(46)+2*c(44)*c(45)+2*c(48)*c(52)+2*c(50)*c(51);
0126 c(59)=c(1)*(c(36)*c(47)+c(38)*c(45)+c(41)*c(42))+c(2)*(c(36)*c(53)+c(38)*c(51)+c(41)*c(48))+c(3)*(2*c(36)*c(41)+c(38)*c(38))+2*c(42)*c(47)+c(45)*c(45)+2*c(48)*c(53)+c(51)*c(51);
0127 c(60)=c(1)*(c(37)*c(43)+c(39)*c(44))+c(2)*(c(37)*c(49)+c(39)*c(50))+2*c(3)*c(37)*c(39)+2*c(43)*c(44)+2*c(49)*c(50);
0128 c(61)=c(1)*(c(37)*c(46)+c(38)*c(43)+c(39)*c(45)+c(40)*c(44))+c(2)*(c(37)*c(52)+c(38)*c(49)+c(39)*c(51)+c(40)*c(50))+c(3)*(2*c(37)*c(40)+2*c(38)*c(39))+2*c(43)*c(45)+2*c(44)*c(46)+2*c(49)*c(51)+2*c(50)*c(52);
0129 c(62)=c(1)*(c(37)*c(47)+c(38)*c(46)+c(40)*c(45)+c(41)*c(44))+c(2)*(c(37)*c(53)+c(38)*c(52)+c(40)*c(51)+c(41)*c(50))+c(3)*(2*c(37)*c(41)+2*c(38)*c(40))+2*c(44)*c(47)+2*c(45)*c(46)+2*c(50)*c(53)+2*c(51)*c(52);
0130 c(63)=c(1)*(c(38)*c(47)+c(41)*c(45))+c(2)*(c(38)*c(53)+c(41)*c(51))+2*c(3)*c(38)*c(41)+2*c(45)*c(47)+2*c(51)*c(53);
0131 c(64)=c(1)*c(39)*c(43)+c(2)*c(39)*c(49)+c(3)*c(39)*c(39)+c(43)*c(43)+c(49)*c(49);
0132 c(65)=c(1)*(c(39)*c(46)+c(40)*c(43))+c(2)*(c(39)*c(52)+c(40)*c(49))+2*c(3)*c(39)*c(40)+2*c(43)*c(46)+2*c(49)*c(52);
0133 c(66)=c(1)*(c(39)*c(47)+c(40)*c(46)+c(41)*c(43))+c(2)*(c(39)*c(53)+c(40)*c(52)+c(41)*c(49))+c(3)*(2*c(39)*c(41)+c(40)*c(40))+2*c(43)*c(47)+c(46)*c(46)+2*c(49)*c(53)+c(52)*c(52);
0134 c(67)=c(1)*(c(40)*c(47)+c(41)*c(46))+c(2)*(c(40)*c(53)+c(41)*c(52))+2*c(3)*c(40)*c(41)+2*c(46)*c(47)+2*c(52)*c(53);
0135 c(68)=c(1)*c(41)*c(47)+c(2)*c(41)*c(53)+c(3)*c(41)*c(41)+c(47)*c(47)+c(53)*c(53);
0136 
0137 
0138 %these are the coefficients of an eight degree polinomial in terms of
0139 %tan(phi/2)
0140 m(1)=c(54)+c(56)+c(59)+c(63)+c(68);
0141 m(2)=2*(c(55)+c(58)+c(62)+c(67));
0142 m(3)=-2*(2*c(54)+c(56)-2*c(57)-2*c(61)-c(63)-2*c(66)-2*c(68));
0143 m(4)=-2*(3*c(55)+c(58)-4*c(60)-c(62)-4*c(65)-3*c(67));
0144 m(5)=2*(3*c(54)-4*c(57)-c(59)+8*c(64)+4*c(66)+3*c(68));
0145 m(6)=2*(3*c(55)-c(58)-4*c(60)-c(62)+4*c(65)+3*c(67));
0146 m(7)=-2*(2*c(54)-c(56)-2*c(57)+2*c(61)+c(63)-2*c(66)-2*c(68));
0147 m(8)=-2*(c(55)-c(58)+c(62)-c(67));
0148 m(9)=c(54)-c(56)+c(59)-c(63)+c(68);
0149 
0150   %phid = findRoots(m);
0151 %phid=roots(m);
0152 
0153 %return roots in terms of t=tan(phi/2)
0154 tanphi2=roots(m(end:-1:1));
0155 count=0; %count the number of solutions
0156 phid=[];
0157 for i=1:length(tanphi2),
0158     %store real solutions for phi
0159     if(isreal(tanphi2(i)))
0160         phid=[phid 2*atan(tanphi2(i))];
0161         count=count+1;
0162     end
0163 end
0164 
0165 if count==0
0166     disp('ERROR: No valid solutions found');
0167 end
0168 
0169 %re
0170 %phi=2*atan(tanphi2(4));
0171 Temp=eye(4);
0172 T=[];
0173 for i=1:length(phid),
0174     xad=(c(42)*cos(phid(i))*cos(phid(i))+c(43)*sin(phid(i))*sin(phid(i))+c(44)*cos(phid(i))*sin(phid(i))+c(45)*cos(phid(i))+c(46)*sin(phid(i))+c(47))/(c(36)*cos(phid(i))*cos(phid(i))+c(39)*sin(phid(i))*sin(phid(i))+c(37)*cos(phid(i))*sin(phid(i))+c(38)*cos(phid(i))+c(40)*sin(phid(i))+c(41));
0175     yad=(c(48)*cos(phid(i))*cos(phid(i))+c(49)*sin(phid(i))*sin(phid(i))+c(50)*cos(phid(i))*sin(phid(i))+c(51)*cos(phid(i))+c(52)*sin(phid(i))+c(53))/(c(36)*cos(phid(i))*cos(phid(i))+c(39)*sin(phid(i))*sin(phid(i))+c(37)*cos(phid(i))*sin(phid(i))+c(38)*cos(phid(i))+c(40)*sin(phid(i))+c(41));
0176     Temp(1,4)=xad;
0177     Temp(2,4)=yad;
0178     Temp(1:3,1)=[cos(phid(i)) sin(phid(i)) 0]';
0179     Temp(1:3,2)=[-sin(phid(i)) cos(phid(i)) 0]';
0180     T=[T Temp];
0181 end
0182 figure, hold  
0183 %plot the solutions!
0184 for i=1:size(T,2)/4,
0185     %points of the end effector
0186     xa=T(1,i*4);
0187     ya=T(2,i*4);
0188     xb=xa+cos(phid(i));
0189     yb=ya+sin(phid(i));
0190     xc=xa+cos(phid(i)+pi/3);
0191     yc=ya+sin(phid(i)+pi/3);
0192     
0193     %These are the positions of the passive joints
0194     xd=a1*cos(t1d);
0195     yd=a1*sin(t1d);
0196     
0197     xe=a1*cos(t2d)+xq;
0198     ye=a1*sin(t2d)+yq;
0199     xf=a1*cos(t3d)+xr;
0200     yf=a1*sin(t3d)+yr;
0201     
0202     %plot solutions
0203    % plot_line([0 0 0], [xa ya 0], 'r', 2)
0204    % plot_line([xq yq 0], [xb yb 0], 'g', 2)
0205    % plot_line([xr yr 0], [xc yc 0], 'b', 2)
0206     %draw the end effector
0207     plot_line([xa ya 0], [xb yb 0], 'k', 2)    
0208     plot_line([xb yb 0], [xc yc 0], 'k', 2)
0209     plot_line([xc yc 0], [xa ya 0], 'k', 2)
0210     
0211     %draw arms!
0212     plot_line([0 0 0], [xd yd 0], 'r', 2)    
0213     plot_line([xd yd 0], [xa ya 0], 'r', 2)
0214     %second arm
0215     plot_line([xq yq 0], [xe ye 0], 'g', 2)    
0216     plot_line([xe ye 0], [xb yb 0], 'g', 2)
0217     %third arm
0218     plot_line([xr yr 0], [xf yf 0], 'b', 2)    
0219     plot_line([xf yf 0], [xc yc 0], 'b', 2)
0220     
0221 end
0222 
0223 
0224 function plot_line(p0, p1, color, w)
0225 x0 = p0(1);
0226 y0 = p0(2);
0227 z0 = p0(3);
0228 x1 = p1(1);
0229 y1 = p1(2);
0230 z1 = p1(3);
0231 % Draw a line between p0 and p1
0232 plot3([x0;x1],[y0;y1],[z0;z1], color, 'LineWidth',w);   
0233