get_end_position

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Position of the end effector off a Delta 3DOF robot.
 
 [X,Y,Z]=get_end_position(P1,P2,P3,L2)
 P1, P2 and P3 are three points in the space.
 L2 is the radius of the sphere
 
 Returns: the two posibles solutions for the position effector.
 
 
   Author: Ángel Rodríguez
   Date: 18/12/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 % Position of the end effector off a Delta 3DOF robot.
0003 %
0004 % [X,Y,Z]=get_end_position(P1,P2,P3,L2)
0005 % P1, P2 and P3 are three points in the space.
0006 % L2 is the radius of the sphere
0007 %
0008 % Returns: the two posibles solutions for the position effector.
0009 %
0010 %
0011 %   Author: Ángel Rodríguez
0012 %   Date: 18/12/2013
0013 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0014 
0015 
0016 function [X,Y,Z]=get_end_position(P1,P2,P3,L2)
0017 
0018 x1=P1(1);
0019 y1=P1(2);
0020 z1=P1(3);
0021 x2=P2(1);
0022 y2=P2(2);
0023 z2=P2(3);
0024 x3=P3(1);
0025 y3=P3(2);
0026 z3=P3(3);
0027 
0028 %Constants needed in the system solve
0029 
0030 c1=(x1^2 + y1^2 + z1^2);
0031 c2=(x2^2 + y2^2 + z2^2);
0032 c3=(x3^2 + y3^2 + z3^2);
0033 
0034 d1=((z3-z2)*(y2-y1)-(z2-z1)*(y3-y2))/((y3-y2)*(x2-x1)-(x3-x2)*(y2-y1));
0035 d2=((c2-c1)*(y3-y2)-(c3-c2)*(y2-y1))/(2*((y3-y2)*(x2-x1)-(x3-x2)*(y2-y1)));
0036 d3=((z2-z1)*(x3-x2)-(z3-z2)*(x2-x1))/((y3-y2)*(x2-x1)-(x3-x2)*(y2-y1));
0037 d4=((c3-c2)*(x2-x1)-(c2-c1)*(x3-x2))/(2*((y3-y2)*(x2-x1)-(x3-x2)*(y2-y1)));
0038 
0039 %Parameters os the second degree equation to obtain Z
0040 
0041 a=(d2-x1)^2 + (d4-y1)^2 + z1^2 - L2^2;
0042 b=2*(d1*(d2-x1) + d3*(d4-y1) - z1);
0043 c=d1^2+d3^2 +1;
0044 
0045 root=sqrt(b^2 - 4*a*c);
0046 
0047 %Solution
0048 
0049 Z(1)=(-b+root)/(2*c);
0050 Z(2)=(-b-root)/(2*c);
0051 
0052 X(1)= d1*Z(1)+d2;
0053 X(2)= d1*Z(2)+d2;
0054 
0055 Y(1)= d3*Z(1)+d4;
0056 Y(2)= d3*Z(2)+d4;
0057 
0058 end