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.
102 lines
3.0 KiB
102 lines
3.0 KiB
%SerialLink.QMINCON Use redundancy to avoid joint limits
|
|
%
|
|
% QS = R.qmincon(Q) exploits null space motion and returns a set of joint
|
|
% angles QS (1xN) that result in the same end-effector pose but are away
|
|
% from the joint coordinate limits. N is the number of robot joints.
|
|
%
|
|
% [Q,ERR] = R.qmincon(Q) as above but also returns ERR which is the
|
|
% scalar final value of the objective function.
|
|
%
|
|
% [Q,ERR,EXITFLAG] = R.qmincon(Q) as above but also returns the
|
|
% status EXITFLAG from fmincon.
|
|
%
|
|
% Trajectory operation::
|
|
%
|
|
% In all cases if Q is MxN it is taken as a pose sequence and R.qmincon()
|
|
% returns the adjusted joint coordinates (MxN) corresponding to each of the
|
|
% poses in the sequence.
|
|
%
|
|
% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation
|
|
% for the corresponding trajectory step.
|
|
%
|
|
% Notes::
|
|
% - Requires fmincon from the Optimization Toolbox.
|
|
% - Robot must be redundant.
|
|
%
|
|
% Author::
|
|
% Bryan Moutrie
|
|
%
|
|
% See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.jacob0.
|
|
|
|
% Copyright (C) Bryan Moutrie, 2013-2015
|
|
% Licensed under the GNU Lesser General Public License
|
|
% see full file for full statement
|
|
%
|
|
% LICENSE STATEMENT:
|
|
%
|
|
% This file is part of pHRIWARE.
|
|
%
|
|
% pHRIWARE 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.
|
|
%
|
|
% pHRIWARE 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 General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Lesser General Public
|
|
% License along with pHRIWARE. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
function [qstar, error, exitflag] = qmincon(robot, q)
|
|
|
|
% check if Optimization Toolbox exists, we need it
|
|
if ~exist('fmincon')
|
|
error('rtb:qmincon:nosupport', 'Optimization Toolbox required');
|
|
end
|
|
M = size(q,1);
|
|
n = robot.n;
|
|
|
|
qstar = zeros(M,n);
|
|
error = zeros(M,1);
|
|
exitflag = zeros(M,1);
|
|
|
|
opt = optimoptions('fmincon', ...
|
|
'Algorithm', 'active-set', ...
|
|
'Display', 'off');
|
|
|
|
lb = robot.qlim(:,1);
|
|
ub = robot.qlim(:,2);
|
|
|
|
x_m = 0; % Little trick for setting x0 in first iteration of loop
|
|
|
|
for m = 1:M
|
|
q_m = q(m,:);
|
|
|
|
J = robot.jacobn(q(m,:));
|
|
N = null(J);
|
|
|
|
if isempty(N)
|
|
error('rtb:qmincon:badarg', 'pHRIWARE:Robot is not redundant');
|
|
end
|
|
|
|
f = @(x) sumsqr((2*(N*x + q_m') - ub - lb)./(ub-lb));
|
|
|
|
x0 = zeros(size(N,2), 1) + x_m;
|
|
|
|
A = [N; -N];
|
|
b = [ub-q_m'; q_m'-lb];
|
|
|
|
[x_m, err_m, ef_m] = fmincon(f,x0,A,b,[],[],[],[],[],opt);
|
|
|
|
qstar(m,:) = q(m,:) + (N*x_m)';
|
|
error(m) = err_m;
|
|
exitflag(m) = ef_m;
|
|
end
|
|
|
|
end
|
|
|
|
function s = sumsqr(A)
|
|
s = sum(A.^2);
|
|
end
|