directkinematic

   DIRECTKINEMATIC        Direct Kinematic for serial robots.

     T = DIRECTKINEMATIC(robot, Q) returns the transformation matrix T
   of the end effector according to the vector q of joint coordinates.

    See also DH

   Author: Arturo Gil. Universidad Miguel Hernández de Elche.
   email: arturo.gil@umh.es date:   01/04/2012

0001 %   DIRECTKINEMATIC        Direct Kinematic for serial robots.
0002 %
0003 %     T = DIRECTKINEMATIC(robot, Q) returns the transformation matrix T
0004 %   of the end effector according to the vector q of joint coordinates.
0005 %
0006 %    See also DH
0007 %
0008 %   Author: Arturo Gil. Universidad Miguel Hernández de Elche.
0009 %   email: arturo.gil@umh.es date:   01/04/2012
0010 
0011 % Copyright (C) 2012, by Arturo Gil Aparicio
0012 %
0013 % This file is part of ARTE (A Robotics Toolbox for Education).
0014 %
0015 % ARTE is free software: you can redistribute it and/or modify
0016 % it under the terms of the GNU Lesser General Public License as published by
0017 % the Free Software Foundation, either version 3 of the License, or
0018 % (at your option) any later version.
0019 %
0020 % ARTE is distributed in the hope that it will be useful,
0021 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0022 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0023 % GNU Lesser General Public License for more details.
0024 %
0025 % You should have received a copy of the GNU Leser General Public License
0026 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0027 function T = directkinematic(robot, q)
0028 
0029 %In the case of a parallel robot, switch to its particular direct kinematic
0030 %function
0031 if isfield(robot, 'parallel')
0032     %Call specific direct kinematic function for parallel robots
0033     T = eval(robot.directkinematic_fn);
0034     return;
0035 end
0036 
0037 %compute direct kinematics for serial robotics
0038 theta = eval(robot.DH.theta);
0039 d = eval(robot.DH.d);
0040 a = eval(robot.DH.a);
0041 alfa = eval(robot.DH.alpha);
0042 
0043 n=length(theta); %number of DOFs
0044 
0045 if robot.debug
0046     fprintf('\nComputing direct kinematics for the %s robot with %d DOFs\n',robot.name, n);
0047 end
0048 %load the position/orientation of the robot's base
0049 T = robot.T0;
0050 
0051 for i=1:n,
0052     T=T*dh(theta(i), d(i), a(i), alfa(i));    
0053 end
0054 
0055 %if there is a tool attached to it, consider it in the computation of
0056 % direct kinematics
0057 % if isfield(robot, 'tool')
0058 %     T=T*robot.tool.TCP; %dh(theta(i), d(i), a(i), alfa(i));
0059 % end