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.
122 lines
3.5 KiB
122 lines
3.5 KiB
%SerialLink.MANIPLTY Manipulability measure
|
|
%
|
|
% M = R.maniplty(Q, OPTIONS) is the manipulability index measure for the robot
|
|
% at the joint configuration Q. It indicates dexterity, how isotropic the
|
|
% robot's motion is with respect to the 6 degrees of Cartesian motion.
|
|
% The measure is low when the manipulator is close to a singularity.
|
|
% If Q is a matrix M is a column vector of manipulability
|
|
% indices for each pose specified by a row of Q.
|
|
%
|
|
% Two measures can be selected:
|
|
% - Yoshikawa's manipulability measure is based on the shape of the velocity
|
|
% ellipsoid and depends only on kinematic parameters.
|
|
% - Asada's manipulability measure is based on the shape of the acceleration
|
|
% ellipsoid which in turn is a function of the Cartesian inertia matrix and
|
|
% the dynamic parameters. The scalar measure computed here is the ratio of
|
|
% the smallest/largest ellipsoid axis. Ideally the ellipsoid would be
|
|
% spherical, giving a ratio of 1, but in practice will be less than 1.
|
|
%
|
|
% Options::
|
|
% 'T' compute manipulability for just transational motion
|
|
% 'R' compute manipulability for just rotational motion
|
|
% 'yoshikawa' use Asada algorithm (default)
|
|
% 'asada' use Asada algorithm
|
|
%
|
|
% Notes::
|
|
% - by default the measure includes rotational and translational dexterity, but
|
|
% this involves adding different units. It can be more useful to look at the
|
|
% translational and rotational manipulability separately.
|
|
%
|
|
% See also SerialLink.inertia, SerialLink.jacob0.
|
|
|
|
% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
|
|
%
|
|
% Copyright (C) 1993-2011, by Peter I. Corke
|
|
%
|
|
% This file is part of The Robotics Toolbox for MATLAB (RTB).
|
|
%
|
|
% RTB is free software: you can redistribute it and/or modify
|
|
% it under the terms of the GNU Lesser General Public License as published by
|
|
% the Free Software Foundation, either version 3 of the License, or
|
|
% (at your option) any later version.
|
|
%
|
|
% RTB is distributed in the hope that it will be useful,
|
|
% but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
% GNU Lesser General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Leser General Public License
|
|
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
|
|
%
|
|
% http://www.petercorke.com
|
|
|
|
function [w,mx] = maniplty(robot, q, varargin)
|
|
n = robot.n;
|
|
|
|
opt.yoshikawa = true;
|
|
opt.asada = false;
|
|
opt.axes = {'all', 'T', 'R'};
|
|
|
|
opt = tb_optparse(opt, varargin);
|
|
|
|
if length(q) == robot.n,
|
|
q = q(:)';
|
|
end
|
|
|
|
w = [];
|
|
MX = [];
|
|
|
|
if opt.yoshikawa
|
|
for Q = q',
|
|
w = [w; yoshi(robot, Q, opt)];
|
|
end
|
|
elseif opt.asada
|
|
for Q = q',
|
|
if nargout > 1
|
|
[ww,mm] = asada(robot, Q);
|
|
w = [w; ww];
|
|
MX = cat(3, MX, mm);
|
|
else
|
|
w = [w; asada(robot, Q, opt)];
|
|
end
|
|
end
|
|
end
|
|
|
|
if nargout > 1
|
|
mx = MX;
|
|
end
|
|
|
|
function m = yoshi(robot, q, opt)
|
|
J = jacob0(robot, q);
|
|
switch opt.axes
|
|
case 'T'
|
|
J = J(1:3,:);
|
|
case 'R'
|
|
J = J(4:6,:);
|
|
end
|
|
m = sqrt(det(J * J'));
|
|
|
|
function [m, mx] = asada(robot, q, opt)
|
|
J = jacob0(robot, q);
|
|
|
|
if rank(J) < 6,
|
|
warning('robot is in degenerate configuration')
|
|
m = 0;
|
|
return;
|
|
end
|
|
|
|
switch opt.axes
|
|
case 'T'
|
|
J = J(1:3,:)
|
|
case 'R'
|
|
J = J(4:6,:);
|
|
end
|
|
Ji = inv(J);
|
|
M = inertia(robot, q);
|
|
Mx = Ji' * M * Ji;
|
|
e = eig(Mx(1:3,1:3));
|
|
m = min(e) / max(e);
|
|
|
|
if nargout > 1
|
|
mx = Mx;
|
|
end
|