You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
198 lines
8.2 KiB
198 lines
8.2 KiB
function [dirKinC varargout] = createDirKinematicDH(rob,varargin)
|
|
%% CREATEDIRKINEMATICDH Computes the symbolic forward kinematics.
|
|
% =========================================================================
|
|
%
|
|
% [dirKinC] = createDirKinematicDH(rob)
|
|
% [dirKinC] = createDirKinematicDH(rob,robOpts)
|
|
%
|
|
% Description:
|
|
% The function outputs the symbolic representation of the per joint
|
|
% analytic forward kinematics for the robot specified by rob. The
|
|
% optional robOpts structure allows to specify the automatic generation
|
|
% of m-functions and real-time capable simulink blocks.
|
|
%
|
|
% The computation of the forward kinematics expression is based on the
|
|
% Denavit-Hartenberg (DH) formalism. Up to now only serial rigid robot
|
|
% arms are supported. Joints may be revolute or prismatic.
|
|
%
|
|
% If the createMFun flag is set in the robOpts structure, m-files are
|
|
% generated for the forward kinematics of each joint as well as the
|
|
% end-effector. The generated m-functions take a vector of generalized
|
|
% joint values as input and output the numerical homogenous
|
|
% transformation. The m-functions are stored in a subdirectory named
|
|
% after the robot.
|
|
%
|
|
% If the createSLBlock flag is set in the robOpts structure, Simulink
|
|
% Embedded Matlab Function Blocks are automatically generated for the
|
|
% forward kinematics of each joint as well as the end-effector. The
|
|
% Embedded Matlab Function Blocks take a vector of generalized joint
|
|
% values as input and output the numerical homogenous transformation.
|
|
% The blocks are added to a block library named "mdlrobotname" in the
|
|
% subdirectory named after the robot.
|
|
%
|
|
% Input:
|
|
% rob : Robot definition structure
|
|
% robOpts: Optional robot model generation options structure.
|
|
%
|
|
% Output:
|
|
% dirKinC: {1xn} containing [4x4] symbolic homogenous transforms for the
|
|
% direct kinematics for each of the n bodies.
|
|
%
|
|
% Example:
|
|
% rob = stanfordarm3;
|
|
% dirKinC = createDirKinematicDH(rob)
|
|
%
|
|
% Known Bugs:
|
|
%
|
|
% References:
|
|
% 1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar
|
|
% 2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano
|
|
% 3) Introduction to Robotics, Mechanics and Control - Craig
|
|
% 4) Modeling, Identification & Control of Robots - Khalil & Dombre
|
|
%
|
|
% Authors:
|
|
% Jörn Malzahn
|
|
%
|
|
%
|
|
% See also createJacobianDH,stanfordarm3,getRobModelOpts.
|
|
%
|
|
% This software may be used under the terms of CC BY-SA 3.0 license
|
|
% < http://creativecommons.org/licenses/by-sa/3.0/ >
|
|
%
|
|
% 2012 RST, Technische Universität Dortmund, Germany
|
|
% < http://www.rst.e-technik.tu-dortmund.de >
|
|
%
|
|
% ========================================================================
|
|
|
|
%% Handle robot modeling options
|
|
% Read user specified options
|
|
if nargin > 1
|
|
robOpts = completeRobOpts(varargin{1});
|
|
else
|
|
robOpts = getRobotModelOpts('default');
|
|
end
|
|
|
|
% Output to logfile?
|
|
if ~isempty(robOpts.logFile)
|
|
logFid = fopen(robOpts.logFile,'a');
|
|
else
|
|
logFid = [];
|
|
end
|
|
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'%s: - Creating forward kinematics ...\n',datestr(now));
|
|
|
|
%% Gather information about the robot and prepare temporary variables
|
|
nBodies = rob.n;
|
|
[ q phie dPhiedx] = createGenCoordinates(rob); % create symbolic variables for all generalized coordinates
|
|
if (robOpts.createMFun)
|
|
curPath = fullfile(pwd, rob.name); % compose path to output directory
|
|
if (~exist(curPath,'dir')) % create output directory if it does not already exist
|
|
mkdir(curPath);
|
|
end
|
|
end
|
|
|
|
%% Create the direct kinematics symbolic expression
|
|
dirKin = eye(4); % initialize containers for speed
|
|
dirKinC = cell(1,nBodies);
|
|
stateOffset = 0;
|
|
|
|
multIDFprintf([robOpts.verbose, logFid],'\tProcessing joint: ');
|
|
for iBodies = 1:nBodies
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies);
|
|
|
|
T = rob.links(iBodies).sym.A(q(iBodies));
|
|
|
|
% Append the current homogenous transformation to the kinematic chain
|
|
dirKin = dirKin*T;
|
|
dirKinC{1,iBodies} = dirKin;
|
|
end
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
|
|
%{
|
|
%% Save results
|
|
if (robOpts.saveResult)
|
|
savePath = fullfile(curPath,'DirKinematicDH');
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tSaving computed kinematics: ');
|
|
% cosmetics to the symbolic direct kinematics equation
|
|
dirKin = simple(dirKin);
|
|
save(savePath,'dirKinC');
|
|
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
end
|
|
|
|
%% Create m-functions
|
|
if (robOpts.createMFun)
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tGenerating m-function for joint: ');
|
|
|
|
% create per joint direct kinematics m-functions
|
|
for iBodies=1:nBodies
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iBodies);
|
|
|
|
if (iBodies ~= nBodies)
|
|
funName = ['DirKinDH_0_',num2str(iBodies)];
|
|
fileName = fullfile(curPath,rob.name,... % intermediate joint direct kinematics
|
|
[funName,'.m']);
|
|
else
|
|
funName = 'DirKinDH';
|
|
fileName = fullfile(curPath,rob.name, [funName,'.m']); % end-effector direct kinematics
|
|
end
|
|
|
|
matlabFunction(dirKinC{1,iBodies},'file',fileName,... % generate function m-file
|
|
'outputs', {'T'},...
|
|
'vars', {[q(1:end), findsym(phie),findsym(dPhiedx)]});
|
|
|
|
hStruct = createHeaderStruct(rob,iBodies,nBodies,funName); % replace autogenerated function header
|
|
replaceHeader(hStruct,fileName);
|
|
|
|
end
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
end
|
|
|
|
%% Create Simulink Embedded Matlab Function Block
|
|
if robOpts.createSLBlock
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tGenerating Simulink Block... ');
|
|
status = createBlockDirKinematicDH( rob );
|
|
if (status == 1)
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
else
|
|
multIDFprintf([robOpts.verbose*2, logFid],'\t%s\n',' failed!');
|
|
end
|
|
end
|
|
|
|
if ~isempty(robOpts.logFile) % close log file if necessary
|
|
fclose(logFid);
|
|
end
|
|
|
|
%% Are the results correct?
|
|
if robOpts.selfCheck
|
|
[testResult.directKinematics, maxErr ] = checkDirKinematics(rob,robOpts);
|
|
end
|
|
|
|
end % of main function
|
|
|
|
|
|
%% Definition of the RST-header contents for each generated file
|
|
function hStruct = createHeaderStruct(rob,curBody,nBodies,fName)
|
|
hStruct.funName = fName;
|
|
hStruct.shortDescription = ['Forward kinematics solution for the ',rob.name,' arm up to frame ',int2str(curBody),' of ',int2str(nBodies),'.'];
|
|
hStruct.calls= {['T = ',fName,'(q)']};
|
|
hStruct.detailedDescription = {['Given a set of joint variables up to joint number ',int2str(curBody),' the function'],...
|
|
'computes the pose belonging to that joint with respect to the base frame.'};
|
|
hStruct.inputs = {['q: ',int2str(curBody),'-element vector of generalized coordinates.'],...
|
|
'Angles have to be given in radians!'};
|
|
hStruct.outputs = {['T: [4x4] Homogenous transformation matrix relating the pose of joint ',int2str(curBody),' of ',int2str(nBodies)],...
|
|
' for the given joint values to the base frame.'};
|
|
hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
|
|
'2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
|
|
'3) Introduction to Robotics, Mechanics and Control - Craig',...
|
|
'4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
|
|
hStruct.authors = {'This is an autogenerated function!',...
|
|
'Generator createDirKinematicDH written by:',...
|
|
'Jörn Malzahn '};
|
|
hStruct.seeAlso = {rob.name,'createDirKinematicDH'};
|
|
end
|
|
%}
|