%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Inverse kinematics for the Delta 3DOF parallel robot
The Delta robot is divided into three different 2DOF arms.
INVERSEKINEMATIC_DELTA3DOF(robot, T)
T: homogeneous matrix
robot: structure with arm parameters
returns: all possible solutions for q = [Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8]
that place the end effectors at the position specified by T.
Any Qi is a 9 value vector that contains:
Qi=[theta1 beta1 delta1 theta2 beta2 delta2 theta3 beta3 delta3]'
being theta1, beta1, delta1 the joint variables of the first arm
and theta2, beta2, delta2 the joint variables of the second arm
and theta3, beta3, delta3 the joint variables of the third arm
Eight possible solutions q = [Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8],
generally called elbow out and elbow in in eight different combinations for
each of the three arms, as described in the following table:
Arm 1 Arm 2 Arm 3
Sol 1 0 0 0
Sol 2 0 0 1
Sol 3 0 1 0
Sol 4 0 1 1
Sol 5 1 0 0
Sol 6 1 0 1
Sol 7 1 1 0
Sol 8 1 1 1
1: elbow out solution
0: elbow in solution
Author: Ángel Rodríguez
Date: 18/12/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the Delta 3DOF parallel robot 0003 % The Delta robot is divided into three different 2DOF arms. 0004 % 0005 % INVERSEKINEMATIC_DELTA3DOF(robot, T) 0006 % T: homogeneous matrix 0007 % robot: structure with arm parameters 0008 % returns: all possible solutions for q = [Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8] 0009 % that place the end effectors at the position specified by T. 0010 % Any Qi is a 9 value vector that contains: 0011 % Qi=[theta1 beta1 delta1 theta2 beta2 delta2 theta3 beta3 delta3]' 0012 % being theta1, beta1, delta1 the joint variables of the first arm 0013 % and theta2, beta2, delta2 the joint variables of the second arm 0014 % and theta3, beta3, delta3 the joint variables of the third arm 0015 % 0016 % Eight possible solutions q = [Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8], 0017 % generally called elbow out and elbow in in eight different combinations for 0018 % each of the three arms, as described in the following table: 0019 % 0020 % Arm 1 Arm 2 Arm 3 0021 % Sol 1 0 0 0 0022 % Sol 2 0 0 1 0023 % Sol 3 0 1 0 0024 % Sol 4 0 1 1 0025 % Sol 5 1 0 0 0026 % Sol 6 1 0 1 0027 % Sol 7 1 1 0 0028 % Sol 8 1 1 1 0029 % 0030 % 1: elbow out solution 0031 % 0: elbow in solution 0032 % Author: Ángel Rodríguez 0033 % Date: 18/12/2013 0034 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0035 0036 % Copyright (C) 2012, by Arturo Gil Aparicio 0037 % 0038 % This file is part of ARTE (A Robotics Toolbox for Education). 0039 % 0040 % ARTE is free software: you can redistribute it and/or modify 0041 % it under the terms of the GNU Lesser General Public License as published by 0042 % the Free Software Foundation, either version 3 of the License, or 0043 % (at your option) any later version. 0044 % 0045 % ARTE is distributed in the hope that it will be useful, 0046 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0047 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0048 % GNU Lesser General Public License for more details. 0049 % 0050 % You should have received a copy of the GNU Leser General Public License 0051 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0052 0053 %Inverse kinematics for a parallel delta robot 0054 %This function povides the angle solution for a TCP gave 0055 %Arms length can be provided. If it's not default lengts will be used 0056 %Base and efector size is fixed. 0057 %Elbow = 0 means that the elbow's solution is out 0058 %Can be placed as the thrird input argument 0059 0060 function q=inversekinematic_DELTA3DOF(robot,T) 0061 0062 % fprintf('\nComputing inverse kinematics for the %s robot', robot.name); 0063 0064 X=T(1,4); %Get´s the x positión of the final efector. 0065 Y=T(2,4); %Get´s the y positión of the final efector. 0066 Z=T(3,4); %Get´s the z positión of the final efector. 0067 0068 %Rotational joints angle 0069 alpha=robot.alpha; 0070 0071 %Base and effector parameters 0072 d1=robot.d1; 0073 d2=robot.d2; 0074 0075 TX=robot.TX; 0076 0077 %Effector apex position depending on X, Y & Z 0078 E1=[X+d2*cos(alpha(1)),Y+d2*sin(alpha(1)),Z]; 0079 E2=[X+d2*cos(alpha(2)),Y+d2*sin(alpha(2)),Z]; 0080 E3=[X+d2*cos(alpha(3)),Y+d2*sin(alpha(3)),Z]; 0081 0082 %Transformation of each effector's apex into his arm's reference system 0083 E1p=inv(robot.robot1.T0)*[E1,1]'; 0084 E2p=inv(robot.robot2.T0)*[E2,1]'; 0085 E3p=inv(robot.robot3.T0)*[E3,1]'; 0086 0087 T1=eye(4); 0088 T1(1:3,4)=E1p(1:3); 0089 T2=eye(4); 0090 T2(1:3,4)=E2p(1:3); 0091 T3=eye(4); 0092 T3(1:3,4)=E3p(1:3); 0093 0094 %Sintaxis for the ARTE library 0095 %Must return both posibilities and delta angles in q 0096 0097 %Inversekinematic_3dofspherical returns elbow in&out joint angle and delta 0098 %angles in q=[in out delta1 delta2] 0099 q1=inversekinematic(robot.robot1,T1); 0100 q2=inversekinematic(robot.robot2,T2); 0101 q3=inversekinematic(robot.robot3,T3); 0102 0103 %Returns 8 posibilities 0104 %If is used q(1) elbow IN solution will be taked 0105 %If is used q(2) elbow OUT solution will be taked 0106 %[theta1 beta1 delta1 theta2 beta2 delta2 theta3 beta3 delta3]' 0107 0108 q=[q1(:,1), q1(:,1), q1(:,1), q1(:,1), q1(:,2), q1(:,2), q1(:,2), q1(:,2); 0109 q2(:,1), q2(:,1), q2(:,2), q2(:,2), q2(:,1), q2(:,1), q2(:,2), q2(:,2); 0110 q3(:,1), q3(:,2), q3(:,1), q3(:,2), q3(:,1), q3(:,2), q3(:,1), q3(:,2)]; 0111 0112 % Saves for final configuration the angles for elbow out 0113 qf=q(:,1); 0114 0115 if qf(2)>=pi || qf(5)>=pi || qf(8)>=pi 0116 fprintf('\nWARNING: inversekinematic_DELTA: unfeasible solution. The arm2 impacts arm 1.\n'); 0117 end 0118 if qf(1)>pi/2 | qf(1)<-pi | qf(4)>pi/2 | qf(4)<-pi | qf(4)>pi/2 | qf(4)<-pi 0119 fprintf('\nWARNING: inversekinematic_3dofplanar: unfeasible solution. The arm 1 impacts the robot´s base.\n'); 0120 end 0121 0122 0123 end