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.
217 lines
8.7 KiB
217 lines
8.7 KiB
function [cgJacobians] = createCGJacobianDH(rob,varargin)
|
|
%% CREATECGJACOBIANDH Computes the symbolic geometric robot link centre of gravity jacobians for open chain robot arms.
|
|
% =========================================================================
|
|
%
|
|
% [cgJacobians] = createCGJacobianDH(rob)
|
|
% [cgJacobians] = createCGJacobianDH(rob,robOpts)
|
|
%
|
|
% Description:
|
|
% The function computes the symbolic expression of the geometric
|
|
% per link centre of gravity jacobians for the robot defined in the
|
|
% input structure rob. The optional robOpts structure allows to specify
|
|
% the automatic generation of m-functions and real-time capable simulink
|
|
% blocks.
|
|
%
|
|
% 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, the function
|
|
% creates an m-file for the geometric centre of gravity jacobians. The
|
|
% m-function takes a vector of generalized joint values as well as a
|
|
% vector of generalized joint velocities as input and outputs the
|
|
% numerical jacobian matrix for these joint values. The m-functions are
|
|
% stored in a subdirectory named after the robot.
|
|
%
|
|
% If the createSLBlock flag is set in the robOpts structure, a Simulink
|
|
% Embedded Matlab Function Block is automatically generated for the
|
|
% geometric centre of gravity jacobians. The blocks take a vector of
|
|
% generalized joint values as input and output the numerical jacobian.
|
|
% 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:
|
|
% cgJacobians: {1xn} including the geometrical [6xn] centre of garvity
|
|
% jacobian for each link
|
|
%
|
|
% Example:
|
|
% rob = stanfordarm3;
|
|
% cgJacobian = createCGJacobianDH(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 createDirKinematicDH,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
|
|
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
|
|
|
|
%% Load existing or invoke computation of forward kinematics
|
|
nJoints = size(rob.DH,1); % Number of joints
|
|
q = createGenCoordinates(rob);
|
|
|
|
curPath = [pwd '\',rob.name,'\']; % compose path to output directory
|
|
if (~exist(curPath,'dir')) % create output directory if it does not already exist
|
|
dirKinC = createDirKinematicDH(rob,robOpts);
|
|
else
|
|
tmp = load([curPath,'\DirKinematicDH.mat']);
|
|
dirKinC = tmp.dirKinC;
|
|
end
|
|
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'%s: - Creating geometric centre of gravity jacobians ...\n',datestr(now));
|
|
|
|
%% Compute the linear motion jacobians for all centres of gravity
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tProcessing translation part for joint: ');
|
|
posJacobians = cell(1,nJoints);
|
|
for iJoints = 1:nJoints
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
|
|
posJacobians{iJoints} = ...
|
|
simplify(jacobian(dirKinC{iJoints}(1:3,4)+dirKinC{iJoints}(1:3,1:3)*rob.COG(:,iJoints),q ));
|
|
end
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
|
|
% Compute the rotary motion jacobians for all centres of gravity
|
|
rotJacobians = cell(1,nJoints); % Initialize jacobian container
|
|
z0 = [0 0 1].'; % default rotary axis.
|
|
rotJacobians{1} = sym(zeros(3,nJoints));
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tProcessing rotational part for joint: ');
|
|
for iJoints = 1:nJoints
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
|
|
if rob.joints(iJoints) ~= 'r'
|
|
if iJoints ==1
|
|
% do nothing for the first joint
|
|
else
|
|
rotJacobians{iJoints} = rotJacobians{iJoints-1}; % copy old jacobian
|
|
end
|
|
|
|
else
|
|
if iJoints ==1
|
|
rotJacobians{iJoints}(:,1) = z0;
|
|
else
|
|
rotJacobians{iJoints} = rotJacobians{iJoints-1}; % copy old jacobian
|
|
rotJacobians{iJoints}(:,iJoints) = dirKinC{iJoints-1}(1:3,1:3)*z0; % insert new axis of rotation expressed in base frame
|
|
end
|
|
end
|
|
end
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
|
|
cgJacobians = cell(1,nJoints);
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s ','Now simplifying jacobian for joint...');
|
|
for iJoints = 1:nJoints
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
|
|
cgJacobians{iJoints} = simplify([posJacobians{iJoints};rotJacobians{iJoints}]);
|
|
end
|
|
multIDFprintf([robOpts.verbose, logFid],'\t%s\n',' done!');
|
|
|
|
|
|
if robOpts.saveResult
|
|
multIDFprintf([robOpts.verbose, logFid],...
|
|
'\tSaving computed jacobians: ');
|
|
% cosmetics to the symbolic equations
|
|
save([curPath,'cgJacobianDH'],'cgJacobians');
|
|
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 centre of gravity jacobian m-functions
|
|
for iJoints=1:nJoints
|
|
multIDFprintf([robOpts.verbose, logFid],' %i ',iJoints);
|
|
|
|
funName = ['CGJacobian_0_',num2str(iJoints)];
|
|
fileName = [curPath,rob.name,funName,'.m']; % intermediate joint forward kinematics
|
|
|
|
|
|
matlabFunction(cgJacobians{iJoints},'file',fileName,... % generate function m-file
|
|
'outputs', {'cgJ'},...
|
|
'vars', {q(1:end)});
|
|
|
|
hStruct = createHeaderStruct(rob,iJoints,nJoints,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 = createBlockCGJacobianDH( 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)
|
|
fclose(logFid);
|
|
end
|
|
|
|
%% Are the results correct?
|
|
if robOpts.selfCheck
|
|
% [testResult.cgJacobian, maxErr ] = checkCGJacobian(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 = ['Centre of gravity jacobians for the ',rob.name,' arm up to link ',int2str(curBody),' of ',int2str(nBodies),'.'];
|
|
hStruct.calls= {['cgJ = ',fName,'(q)']};
|
|
hStruct.detailedDescription = {['Given a full set of ',int2str(nBodies),' joint variables the function'],...
|
|
'computes the geometric centre of gravity jacobian with respect to the',...
|
|
['base frame for the link subsequent to joint ',int2str(curBody),'.']};
|
|
hStruct.inputs = {['q: ',int2str(nBodies),'-element vector of generalized coordinates.'],...
|
|
' Angles have to be given in radians!'};
|
|
hStruct.outputs = {['cgJ: [6x',num2str(nBodies),'] geometric jacobian relating the Cartesian'],...
|
|
[' velocity of the centre of gravity belonging to link ',int2str(curBody),' of ',int2str(nBodies)],...
|
|
'expressed in the robot 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 createCGJacobianDH written by:',...
|
|
'Jörn Malzahn '};
|
|
hStruct.seeAlso = {rob.name,'createCGJacobianDH'};
|
|
end
|