%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Inverse kinematics for the 3RRR planar parallel robot
The 3RRR robot is divided into three different 2DOF arms.
INVERSEKINEMATIC_3RRR(robot, T)
T: homogeneous matrix
robot: structure with arm parameters
returns: all possible solutions for q = [Q1 Q2 Q3 Q4 Q5 Q6]
that place the end effectors at the position specified by T.
Any Qi is a 4 value vector that contains: Qi={q1 fi1, q2, fi2, q3, fi3},
being q1, fi1, the joint variables of the first arm
and q2, fi2, the joint variables of the second arm
and q3, fi3, the joint variables of the second arm
Eight possible solutions q = [Q1 Q2 Q3 Q4 Q5 Q6],
generally called elbow up and elbow down 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 up solution
0: elbow down solution
Author: Arturo Gil Aparicio arturo.gil@umh.es
Date: 08/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the 3RRR planar parallel robot 0003 % The 3RRR robot is divided into three different 2DOF arms. 0004 % 0005 % INVERSEKINEMATIC_3RRR(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] 0009 % that place the end effectors at the position specified by T. 0010 % Any Qi is a 4 value vector that contains: Qi={q1 fi1, q2, fi2, q3, fi3}, 0011 % being q1, fi1, the joint variables of the first arm 0012 % and q2, fi2, the joint variables of the second arm 0013 % and q3, fi3, the joint variables of the second arm 0014 % 0015 % Eight possible solutions q = [Q1 Q2 Q3 Q4 Q5 Q6], 0016 % generally called elbow up and elbow down in eight different combinations for 0017 % each of the three arms, as described in the following table: 0018 % 0019 % Arm 1 Arm 2 Arm 3 0020 % Sol 1 0 0 0 0021 % Sol 2 0 0 1 0022 % Sol 3 0 1 0 0023 % Sol 4 0 1 1 0024 % Sol 5 1 0 0 0025 % Sol 6 1 0 1 0026 % Sol 7 1 1 0 0027 % Sol 8 1 1 1 0028 % 0029 % 1: elbow up solution 0030 % 0: elbow down solution 0031 % Author: Arturo Gil Aparicio arturo.gil@umh.es 0032 % Date: 08/03/2012 0033 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0034 0035 % Copyright (C) 2012, by Arturo Gil Aparicio 0036 % 0037 % This file is part of ARTE (A Robotics Toolbox for Education). 0038 % 0039 % ARTE is free software: you can redistribute it and/or modify 0040 % it under the terms of the GNU Lesser General Public License as published by 0041 % the Free Software Foundation, either version 3 of the License, or 0042 % (at your option) any later version. 0043 % 0044 % ARTE is distributed in the hope that it will be useful, 0045 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0046 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0047 % GNU Lesser General Public License for more details. 0048 % 0049 % You should have received a copy of the GNU Leser General Public License 0050 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0051 function Q = inversekinematic_3RRR(robot, T) 0052 0053 fprintf('\nComputing inverse kinematics for the %s robot', robot.name); 0054 0055 %T= [ nx ox ax Px; 0056 % ny oy ay Py; 0057 % nz oz az Pz]; 0058 0059 0060 %Now find the orientation from the angle phi encoded in T=(ux, uy, uz) 0061 u=T(1:3,1); 0062 phi = atan2(u(2), u(1)); 0063 0064 h=robot.h; 0065 0066 %Position of the point A in base coordinates 0067 xA=T(1,4); 0068 yA=T(2,4); 0069 0070 %Position of the point B in base coordinates 0071 xB=xA+h*cos(phi); 0072 yB=yA+h*sin(phi); 0073 0074 %Position of the point C in base coordinates 0075 xC=xA+h*cos(phi+pi/3); 0076 yC=yA+h*sin(phi+pi/3); 0077 0078 0079 %Transformation of the second arm. Compute the point (xB, yB) in 0080 %coordinates of the second arm 0081 T2=robot.robot2.T0; 0082 %the same point expressed in the second reference frame 0083 P=inv(T2)*[xB; yB; 0; 1]; 0084 %store it in T2 0085 T2(1:3,4)=P(1:3); 0086 0087 0088 %transformation between the reference systems corresponding to the first 0089 %and second arm 0090 T3=robot.robot3.T0; 0091 %the same point expressed in the second reference frame 0092 P=inv(T3)*[xC; yC; 0; 1]; 0093 %Store it in T3 0094 T3(1:3,4)=P(1:3); 0095 0096 0097 0098 0099 0100 %solve the two first variables using inversekinematic for the first arm 0101 q1=inversekinematic(robot.robot1, T); 0102 0103 %solve the two second variables using inversekinematic for the second arm 0104 q2=inversekinematic(robot.robot2, T2); 0105 0106 %solve the two first variables using inversekinematic for the first arm 0107 q3=inversekinematic(robot.robot3, T3); 0108 0109 0110 Q=[q1(1,1) q1(1,2) q1(1,1) q1(1,2) q1(1,1) q1(1,2) q1(1,1) q1(1,2); 0111 q1(2,1) q1(2,2) q1(2,1) q1(2,2) q1(2,1) q1(2,2) q1(2,1) q1(2,2); 0112 q2(1,1) q2(1,1) q2(1,2) q2(1,2) q2(1,1) q2(1,1) q2(1,2) q2(1,2); 0113 q2(2,1) q2(2,1) q2(2,2) q2(2,2) q2(2,1) q2(2,1) q2(2,2) q2(2,2); 0114 q3(1,1) q3(1,1) q3(1,1) q3(1,1) q3(1,2) q3(1,2) q3(1,2) q3(1,2); 0115 q3(2,1) q3(2,1) q3(2,1) q3(2,1) q3(2,2) q3(2,2) q3(2,2) q3(2,2)]; 0116