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.
121 lines
4.0 KiB
121 lines
4.0 KiB
%SerialLink.IKCON Numerical inverse kinematics with joint limits
|
|
%
|
|
% Q = R.ikcon(T) are the joint coordinates (1xN) corresponding to the robot
|
|
% end-effector pose T (4x4) which is a homogenenous transform.
|
|
%
|
|
% [Q,ERR] = robot.ikcon(T) as above but also returns ERR which is the
|
|
% scalar final value of the objective function.
|
|
%
|
|
% [Q,ERR,EXITFLAG] = robot.ikcon(T) as above but also returns the
|
|
% status EXITFLAG from fmincon.
|
|
%
|
|
% [Q,ERR,EXITFLAG] = robot.ikcon(T, Q0) as above but specify the
|
|
% initial joint coordinates Q0 used for the minimisation.
|
|
%
|
|
% [Q,ERR,EXITFLAG] = robot.ikcon(T, Q0, options) as above but specify the
|
|
% options for fmincon to use.
|
|
%
|
|
% Trajectory operation::
|
|
%
|
|
% In all cases if T is 4x4xM it is taken as a homogeneous transform
|
|
% sequence and R.ikcon() returns the joint coordinates corresponding to
|
|
% each of the transforms in the sequence. Q is MxN where N is the number
|
|
% of robot joints. The initial estimate of Q for each time step is taken as
|
|
% the solution from the previous time step.
|
|
%
|
|
% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation
|
|
% for the corresponding trajectory step.
|
|
%
|
|
% Notes::
|
|
% - Requires fmincon from the Optimization Toolbox.
|
|
% - Joint limits are considered in this solution.
|
|
% - Can be used for robots with arbitrary degrees of freedom.
|
|
% - In the case of multiple feasible solutions, the solution returned
|
|
% depends on the initial choice of Q0.
|
|
% - Works by minimizing the error between the forward kinematics of the
|
|
% joint angle solution and the end-effector frame as an optimisation.
|
|
% The objective function (error) is described as:
|
|
% sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
|
|
% Where omega is some gain matrix, currently not modifiable.
|
|
%
|
|
% Author::
|
|
% Bryan Moutrie
|
|
%
|
|
% See also SerialLink.ikunc, fmincon, SerialLink.ikine, SerialLink.fkine.
|
|
|
|
% 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] = ikcon(robot, T, q0, options)
|
|
|
|
% check if Optimization Toolbox exists, we need it
|
|
if ~exist('fmincon')
|
|
error('rtb:ikcon:nosupport', 'Optimization Toolbox required');
|
|
end
|
|
|
|
% create output variables
|
|
T_sz = size(T, 3);
|
|
qstar = zeros(T_sz, robot.n);
|
|
error = zeros(T_sz, 1);
|
|
exitflag = zeros(T_sz, 1);
|
|
|
|
problem.x0 = zeros(1, robot.n);
|
|
problem.options = optimoptions('fmincon', ...
|
|
'Algorithm', 'active-set', ...
|
|
'Display', 'off'); % default options for ikcon
|
|
|
|
if nargin > 2
|
|
% user passed initial joint coordinates
|
|
problem.x0 = q0;
|
|
end
|
|
if nargin > 3
|
|
% if given, add optional argument to the list of optimiser options
|
|
problem.options = optimset(problem.options, options);
|
|
end
|
|
|
|
% set the joint limit bounds
|
|
problem.lb = robot.qlim(:,1);
|
|
problem.ub = robot.qlim(:,2);
|
|
problem.solver = 'fmincon';
|
|
|
|
reach = sum(abs([robot.a, robot.d]));
|
|
omega = diag([1 1 1 3/reach]);
|
|
|
|
for t = 1: T_sz
|
|
problem.objective = ...
|
|
@(x) sumsqr(((T(:,:,t) \ robot.fkine(x)) - eye(4)) * omega);
|
|
|
|
[q_t, err_t, ef_t] = fmincon(problem);
|
|
|
|
qstar(t,:) = q_t;
|
|
error(t) = err_t;
|
|
exitflag(t) = ef_t;
|
|
|
|
problem.x0 = q_t;
|
|
end
|
|
|
|
end
|
|
|
|
function s = sumsqr(A)
|
|
s = sum(A(:).^2);
|
|
end
|
|
|