%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Inverse kinematics for the 5R planar parallel robot
The 5R robot is divided into two different 2DOF arms.
INVERSEKINEMATIC_5R(robot, T)
T: homogeneous matrix
robot: structure with arm parameters
returns: all possible solutions for q = [Q1 Q2 Q3 Q4]
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},
being q1, fi1, the joint variables of the first arm
and q2, fi2, the joint variables of the second arm
Four possible solutions q = [Q1 Q2 Q3 Q4],
generally called elbow up and elbow down in four different combinations.
Author: Arturo Gil Aparicio arturo.gil@umh.es
Date: 08/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Inverse kinematics for the 5R planar parallel robot 0003 % The 5R robot is divided into two different 2DOF arms. 0004 % INVERSEKINEMATIC_5R(robot, T) 0005 % T: homogeneous matrix 0006 % robot: structure with arm parameters 0007 % returns: all possible solutions for q = [Q1 Q2 Q3 Q4] 0008 % that place the end effectors at the position specified by T. 0009 % Any Qi is a 4 value vector that contains: Qi={q1 fi1, q2, fi2}, 0010 % being q1, fi1, the joint variables of the first arm 0011 % and q2, fi2, the joint variables of the second arm 0012 % Four possible solutions q = [Q1 Q2 Q3 Q4], 0013 % generally called elbow up and elbow down in four different combinations. 0014 % 0015 % Author: Arturo Gil Aparicio arturo.gil@umh.es 0016 % Date: 08/03/2012 0017 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0018 0019 % Copyright (C) 2012, by Arturo Gil Aparicio 0020 % 0021 % This file is part of ARTE (A Robotics Toolbox for Education). 0022 % 0023 % ARTE is free software: you can redistribute it and/or modify 0024 % it under the terms of the GNU Lesser General Public License as published by 0025 % the Free Software Foundation, either version 3 of the License, or 0026 % (at your option) any later version. 0027 % 0028 % ARTE is distributed in the hope that it will be useful, 0029 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0030 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0031 % GNU Lesser General Public License for more details. 0032 % 0033 % You should have received a copy of the GNU Leser General Public License 0034 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0035 function Q = inversekinematic_5R(robot, T) 0036 0037 fprintf('\nComputing inverse kinematics for the %s robot', robot.name); 0038 0039 0040 %T= [ nx ox ax Px; 0041 % ny oy ay Py; 0042 % nz oz az Pz]; 0043 P=T(1:3,4); 0044 0045 %transformation between the reference systems corresponding to the first 0046 %and second arm 0047 T2=robot.robot2.T0; 0048 %the same point expressed in the second reference frame 0049 P2=inv(T2)*[P; 1]; 0050 T2(1:3,4)=P2(1:3); 0051 0052 %solve the two first variables using inversekinematic for the first arm 0053 q1=inversekinematic(robot.robot1, T); 0054 0055 %solve the two second variables using inversekinematic for the second arm 0056 q2=inversekinematic(robot.robot2, T2); 0057 0058 0059 Q=[q1(1,1) q1(1,2) q1(1,1) q1(1,2); 0060 q1(2,1) q1(2,2) q1(2,1) q1(2,2); 0061 q2(1,1) q2(1,1) q2(1,2) q2(1,2); 0062 q2(2,1) q2(2,1) q2(2,2) q2(2,2)]; 0063