directkinematic_DELTA3DOF

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Inverse kinematics for the Delta 3DOF parallel robot
 

 DIRECTKINEMATIC_3RRR(robot, Q)
 Q: joints vector with structure: 
 [theta1 beta1 delta1 theta2 beta2 delta2 theta3 beta3 delta3]
 theta is the active joint´s vector
 beta is the passive joint's vector in the arm plan
 theta is the passive joint's vector out of the arm plan
 robot: structure with arm parameters
 returns: T solution. 
 shows T1 and T2 with the 2 posible solution. T1 it´s unachievable
 

 
 
   Author: Ángel Rodríguez
   Date: 18/12/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Copyright (C) 2012, by Arturo Gil Aparicio

 This file is part of ARTE (A Robotics Toolbox for Education).
 
 ARTE is free software: you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 ARTE is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.
 
 You should have received a copy of the GNU Leser General Public License
 along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 % Inverse kinematics for the Delta 3DOF parallel robot
0003 %
0004 %
0005 % DIRECTKINEMATIC_3RRR(robot, Q)
0006 % Q: joints vector with structure:
0007 % [theta1 beta1 delta1 theta2 beta2 delta2 theta3 beta3 delta3]
0008 % theta is the active joint´s vector
0009 % beta is the passive joint's vector in the arm plan
0010 % theta is the passive joint's vector out of the arm plan
0011 % robot: structure with arm parameters
0012 % returns: T solution.
0013 % shows T1 and T2 with the 2 posible solution. T1 it´s unachievable
0014 %
0015 %
0016 %
0017 %
0018 %   Author: Ángel Rodríguez
0019 %   Date: 18/12/2013
0020 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0021 % Copyright (C) 2012, by Arturo Gil Aparicio
0022 %
0023 % This file is part of ARTE (A Robotics Toolbox for Education).
0024 %
0025 % ARTE is free software: you can redistribute it and/or modify
0026 % it under the terms of the GNU Lesser General Public License as published by
0027 % the Free Software Foundation, either version 3 of the License, or
0028 % (at your option) any later version.
0029 %
0030 % ARTE is distributed in the hope that it will be useful,
0031 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0032 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0033 % GNU Lesser General Public License for more details.
0034 %
0035 % You should have received a copy of the GNU Leser General Public License
0036 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0037 
0038 function T=directkinematic_DELTA3DOF(robot,q)
0039 
0040 %Robot parameters
0041 
0042 L1=robot.L1;
0043 L2=robot.L2;
0044 d1=robot.d1;
0045 d2=robot.d2;
0046 
0047 alpha=robot.alpha;
0048 
0049 %Base´s rotationals joints position
0050 A1=[d1*cos(alpha(1)),d1*sin(alpha(1)),0];
0051 A2=[d1*cos(alpha(2)),d1*sin(alpha(2)),0];
0052 A3=[d1*cos(alpha(3)),d1*sin(alpha(3)),0];
0053 
0054 %Cardans joints position. Union of voth arms
0055 B1=[(d1+ L1*cos(q(1)))*cos(alpha(1)),(d1+ L1*cos(q(1)))*sin(alpha(1)),-L1*sin(q(1))];
0056 B2=[(d1+ L1*cos(q(4)))*cos(alpha(2)),(d1+ L1*cos(q(4)))*sin(alpha(2)),-L1*sin(q(4))];
0057 B3=[(d1+ L1*cos(q(7)))*cos(alpha(3)),(d1+ L1*cos(q(7)))*sin(alpha(3)),-L1*sin(q(7))];
0058 
0059 %Displacement of the cardan position that is the center of the
0060 %circunference needed to solve de direct kinematic
0061 
0062 B1_displaced=[B1(1) - (d2*cos(alpha(1))), B1(2) - (d2*sin(alpha(1))), B1(3)];
0063 B2_displaced=[B2(1) - (d2*cos(alpha(2))), B2(2) - (d2*sin(alpha(2))), B2(3)];
0064 B3_displaced=[B3(1) - (d2*cos(alpha(3))), B3(2) - (d2*sin(alpha(3))), B3(3)];
0065 
0066 [X,Y,Z] = get_end_position(B1_displaced, B2_displaced, B3_displaced,L2);
0067 
0068 T1=eye(4);
0069 T2=eye(4);
0070 T1(1:3,4)=[X(1), Y(1), Z(1)]'
0071 T2(1:3,4)=[X(2), Y(2), Z(2)]'
0072 
0073 
0074 T=T2;
0075 
0076 end