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.
586 lines
18 KiB
586 lines
18 KiB
%SerialLink Serial-link robot class
|
|
%
|
|
% A concrete class that represents a serial-link arm-type robot. The
|
|
% mechanism is described using Denavit-Hartenberg parameters, one set
|
|
% per joint.
|
|
%
|
|
% Methods::
|
|
%
|
|
% plot display graphical representation of robot
|
|
% teach drive the graphical robot
|
|
% isspherical test if robot has spherical wrist
|
|
% islimit test if robot at joint limit
|
|
%
|
|
% fkine forward kinematics
|
|
% ikine6s inverse kinematics for 6-axis spherical wrist revolute robot
|
|
% ikine3 inverse kinematics for 3-axis revolute robot
|
|
% ikine inverse kinematics using iterative method
|
|
% jacob0 Jacobian matrix in world frame
|
|
% jacobn Jacobian matrix in tool frame
|
|
% maniplty manipulability
|
|
%
|
|
% jtraj a joint space trajectory
|
|
%
|
|
% accel joint acceleration
|
|
% coriolis Coriolis joint force
|
|
% dyn show dynamic properties of links
|
|
% fdyn joint motion
|
|
% friction friction force
|
|
% gravload gravity joint force
|
|
% inertia joint inertia matrix
|
|
% nofriction set friction parameters to zero
|
|
% rne joint torque/force
|
|
% payload add a payload in end-effector frame
|
|
% perturb randomly perturb link dynamic parameters
|
|
%
|
|
% Properties (read/write)::
|
|
%
|
|
% links vector of Link objects (1xN)
|
|
% gravity direction of gravity [gx gy gz]
|
|
% base pose of robot's base (4x4 homog xform)
|
|
% tool robot's tool transform, T6 to tool tip (4x4 homog xform)
|
|
% qlim joint limits, [qmin qmax] (Nx2)
|
|
% offset kinematic joint coordinate offsets (Nx1)
|
|
% name name of robot, used for graphical display
|
|
% manuf annotation, manufacturer's name
|
|
% comment annotation, general comment
|
|
% plotopt options for plot() method (cell array)
|
|
%
|
|
% Object properties (read only)::
|
|
%
|
|
% n number of joints
|
|
% config joint configuration string, eg. 'RRRRRR'
|
|
% mdh kinematic convention boolean (0=DH, 1=MDH)
|
|
%
|
|
% Note::
|
|
% - SerialLink is a reference object.
|
|
% - SerialLink objects can be used in vectors and arrays
|
|
%
|
|
% Reference::
|
|
% - Robotics, Vision & Control, Chaps 7-9,
|
|
% P. Corke, Springer 2011.
|
|
% - Robot, Modeling & Control,
|
|
% M.Spong, S. Hutchinson & M. Vidyasagar, Wiley 2006.
|
|
%
|
|
% See also Link, DHFactor.
|
|
|
|
% 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
|
|
|
|
classdef SerialLink < handle
|
|
|
|
properties
|
|
name
|
|
gravity
|
|
base
|
|
tool
|
|
manuf
|
|
comment
|
|
|
|
plotopt
|
|
lineopt
|
|
shadowopt
|
|
|
|
qteach
|
|
|
|
fast_rne % mex version of rne detected
|
|
|
|
end
|
|
|
|
events
|
|
Moved
|
|
end
|
|
|
|
properties (SetAccess = private)
|
|
n
|
|
links
|
|
mdh
|
|
T
|
|
end
|
|
|
|
properties (Dependent = true, SetAccess = private)
|
|
config
|
|
end
|
|
|
|
properties (Dependent = true)
|
|
offset
|
|
qlim
|
|
end
|
|
|
|
methods
|
|
function r = SerialLink(L, varargin)
|
|
%SerialLink Create a SerialLink robot object
|
|
%
|
|
% R = SerialLink(LINKS, OPTIONS) is a robot object defined by a vector
|
|
% of Link objects.
|
|
%
|
|
% R = SerialLink(DH, OPTIONS) is a robot object with kinematics defined
|
|
% by the matrix DH which has one row per joint and each row is
|
|
% [theta d a alpha] and joints are assumed revolute. An optional
|
|
% fifth column sigma indicate revolute (sigma=0, default) or
|
|
% prismatic (sigma=1).
|
|
%
|
|
% R = SerialLink(OPTIONS) is a null robot object with no links.
|
|
%
|
|
% R = SerialLink([R1 R2 ...], OPTIONS) concatenate robots, the base of
|
|
% R2 is attached to the tip of R1.
|
|
%
|
|
% R = SerialLink(R1, options) is a deep copy of the robot object R1,
|
|
% with all the same properties.
|
|
%
|
|
% Options::
|
|
%
|
|
% 'name', name set robot name property
|
|
% 'comment', comment set robot comment property
|
|
% 'manufacturer', manuf set robot manufacturer property
|
|
% 'base', base set base transformation matrix property
|
|
% 'tool', tool set tool transformation matrix property
|
|
% 'gravity', g set gravity vector property
|
|
% 'plotopt', po set plotting options property
|
|
%
|
|
% Examples::
|
|
%
|
|
% Create a 2-link robot
|
|
% L(1) = Link([ 0 0 a1 0], 'standard');
|
|
% L(2) = Link([ 0 0 a2 0], 'standard');
|
|
% twolink = SerialLink(L, 'name', 'two link');
|
|
%
|
|
% Robot objects can be concatenated in two ways
|
|
% R = R1 * R2;
|
|
% R = SerialLink([R1 R2]);
|
|
%
|
|
% Note::
|
|
% - SerialLink is a reference object, a subclass of Handle object.
|
|
% - SerialLink objects can be used in vectors and arrays
|
|
% - When robots are concatenated (either syntax) the intermediate base and
|
|
% tool transforms are removed since general constant transforms cannot
|
|
% be represented in Denavit-Hartenberg notation.
|
|
%
|
|
% See also Link, SerialLink.plot.
|
|
|
|
r.name = 'noname';
|
|
r.manuf = '';
|
|
r.comment = '';
|
|
r.links = [];
|
|
r.n = 0;
|
|
r.mdh = 0;
|
|
r.gravity = [0; 0; 9.81];
|
|
r.base = eye(4,4);
|
|
r.tool = eye(4,4);
|
|
|
|
r.lineopt = {'Color', 'black', 'Linewidth', 4};
|
|
r.shadowopt = {'Color', 0.7*[1 1 1], 'Linewidth', 3};
|
|
r.plotopt = {};
|
|
|
|
if exist('frne') == 3
|
|
r.fast_rne = true;
|
|
end
|
|
|
|
if nargin == 0
|
|
% zero argument constructor, sets default values
|
|
return;
|
|
else
|
|
% at least one argument, either a robot or link array
|
|
|
|
if isa(L, 'SerialLink')
|
|
if length(L) == 1
|
|
% clone the passed robot
|
|
for j=1:L.n
|
|
newlinks(j) = copy(L.links(j));
|
|
end
|
|
r.links = newlinks;
|
|
else
|
|
% compound the robots in the vector
|
|
r = L(1);
|
|
for k=2:length(L)
|
|
r = r * L(k);
|
|
end
|
|
end
|
|
elseif isa(L, 'Link')
|
|
r.links = L; % attach the links
|
|
elseif isa(L, 'double')
|
|
% legacy matrix
|
|
dh_dyn = L;
|
|
clear L
|
|
for j=1:numrows(dh_dyn)
|
|
L(j) = Link();
|
|
L(j).theta = dh_dyn(j,1);
|
|
L(j).d = dh_dyn(j,2);
|
|
L(j).a = dh_dyn(j,3);
|
|
L(j).alpha = dh_dyn(j,4);
|
|
|
|
if numcols(dh_dyn) > 4
|
|
L(j).sigma = dh_dyn(j,5);
|
|
end
|
|
end
|
|
r.links = L;
|
|
else
|
|
error('unknown type passed to robot');
|
|
end
|
|
r.n = length(r.links);
|
|
end
|
|
|
|
% process the rest of the arguments in key, value pairs
|
|
opt.name = 'robot';
|
|
opt.comment = [];
|
|
opt.manufacturer = [];
|
|
opt.base = [];
|
|
opt.tool = [];
|
|
opt.offset = [];
|
|
opt.qlim = [];
|
|
opt.plotopt = [];
|
|
|
|
[opt,out] = tb_optparse(opt, varargin);
|
|
if ~isempty(out)
|
|
error('unknown option <%s>', out{1});
|
|
end
|
|
|
|
% copy the properties to robot object
|
|
p = properties(r);
|
|
for i=1:length(p)
|
|
if isfield(opt, p{i}) && ~isempty(getfield(opt, p{i}))
|
|
r.(p{i}) = getfield(opt, p{i});
|
|
end
|
|
end
|
|
|
|
% set the robot object mdh status flag
|
|
mdh = [r.links.mdh];
|
|
if all(mdh == 0)
|
|
r.mdh = mdh(1);
|
|
elseif all (mdh == 1)
|
|
r.mdh = mdh(1);
|
|
else
|
|
error('robot has mixed D&H links conventions');
|
|
end
|
|
end
|
|
|
|
%{
|
|
function delete(r)
|
|
disp('in destructor');
|
|
if ~isempty(r.teachfig)
|
|
delete(r.teachfig);
|
|
end
|
|
rh = findobj('Tag', r.name);
|
|
for f=rh
|
|
delete(f);
|
|
end
|
|
end
|
|
%}
|
|
|
|
function r2 = mtimes(r, l)
|
|
%SerialLink.mtimes Concatenate robots
|
|
%
|
|
% R = R1 * R2 is a robot object that is equivalent to mechanically attaching
|
|
% robot R2 to the end of robot R1.
|
|
%
|
|
% Notes::
|
|
% - If R1 has a tool transform or R2 has a base transform these are
|
|
% discarded since DH convention does not allow for arbitrary intermediate
|
|
% transformations.
|
|
if isa(l, 'SerialLink')
|
|
r2 = SerialLink(r);
|
|
r2.links = [r2.links l.links];
|
|
r2.base = r.base;
|
|
r2.n = length(r2.links);
|
|
elseif isa(l, 'Link')
|
|
r2 = SerialLink(r);
|
|
r2.links = [r2.links l];
|
|
r2.n = length(r2.links);
|
|
end
|
|
end
|
|
|
|
function display(r)
|
|
%SerialLink.display Display parameters
|
|
%
|
|
% R.display() displays the robot parameters in human-readable form.
|
|
%
|
|
% Notes::
|
|
% - This method is invoked implicitly at the command line when the result
|
|
% of an expression is a SerialLink object and the command has no trailing
|
|
% semicolon.
|
|
%
|
|
% See also SerialLink.char, SerialLink.dyn.
|
|
|
|
loose = strcmp( get(0, 'FormatSpacing'), 'loose');
|
|
if loose
|
|
disp(' ');
|
|
end
|
|
disp([inputname(1), ' = '])
|
|
if loose
|
|
disp(' ');
|
|
end
|
|
disp(char(r))
|
|
if loose
|
|
disp(' ');
|
|
end
|
|
end
|
|
|
|
function s = char(robot)
|
|
%SerialLink.char Convert to string
|
|
%
|
|
% S = R.char() is a string representation of the robot's kinematic parameters,
|
|
% showing DH parameters, joint structure, comments, gravity vector, base and
|
|
% tool transform.
|
|
|
|
s = '';
|
|
for j=1:length(robot)
|
|
r = robot(j);
|
|
|
|
% informational line
|
|
if r.mdh
|
|
convention = 'modDH';
|
|
else
|
|
convention = 'stdDH';
|
|
end
|
|
s = sprintf('%s (%d axis, %s, %s)', r.name, r.n, r.config, convention);
|
|
|
|
% comment and other info
|
|
line = '';
|
|
if ~isempty(r.manuf)
|
|
line = strcat(line, sprintf(' %s;', r.manuf));
|
|
end
|
|
if ~isempty(r.comment)
|
|
line = strcat(line, sprintf(' %s;', r.comment));
|
|
end
|
|
s = char(s, line);
|
|
|
|
% link parameters
|
|
s = char(s, '+---+-----------+-----------+-----------+-----------+');
|
|
s = char(s, '| j | theta | d | a | alpha |');
|
|
s = char(s, '+---+-----------+-----------+-----------+-----------+');
|
|
s = char(s, char(r.links, true));
|
|
s = char(s, '+---+-----------+-----------+-----------+-----------+');
|
|
|
|
% gravity, base, tool
|
|
s_grav = horzcat(char('grav = ', ' ', ' '), mat2str(r.gravity));
|
|
s_grav = char(s_grav, ' ');
|
|
s_base = horzcat(char(' base = ',' ',' ', ' '), mat2str(r.base));
|
|
|
|
s_tool = horzcat(char(' tool = ',' ',' ', ' '), mat2str(r.tool));
|
|
|
|
line = horzcat(s_grav, s_base, s_tool);
|
|
|
|
s = char(s, ' ', line);
|
|
if j ~= length(robot)
|
|
s = char(s, ' ');
|
|
end
|
|
end
|
|
end
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% set/get methods
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
|
|
function set.tool(r, v)
|
|
if isempty(v)
|
|
r.base = eye(4,4);
|
|
elseif ~ishomog(v)
|
|
error('tool must be a homogeneous transform');
|
|
else
|
|
r.tool = v;
|
|
end
|
|
end
|
|
|
|
function set.base(r, v)
|
|
if isempty(v)
|
|
r.base = eye(4,4);
|
|
elseif ~ishomog(v)
|
|
error('base must be a homogeneous transform');
|
|
else
|
|
r.base = v;
|
|
end
|
|
end
|
|
|
|
function set.offset(r, v)
|
|
if length(v) ~= length(v)
|
|
error('offset vector length must equal number DOF');
|
|
end
|
|
L = r.links;
|
|
for i=1:r.n
|
|
L(i).offset = v(i);
|
|
end
|
|
r.links = L;
|
|
end
|
|
|
|
function v = get.offset(r)
|
|
v = [r.links.offset];
|
|
end
|
|
|
|
function set.qlim(r, v)
|
|
if numrows(v) ~= r.n
|
|
error('insufficient rows in joint limit matrix');
|
|
end
|
|
L = r.links;
|
|
for i=1:r.n
|
|
L(i).qlim = v(i,:);
|
|
end
|
|
r.links = L;
|
|
end
|
|
|
|
function v = get.qlim(r)
|
|
L = r.links;
|
|
v = zeros(r.n, 2);
|
|
for i=1:r.n
|
|
if isempty(L(i).qlim)
|
|
if L(i).isrevolute
|
|
v(i,:) = [-pi pi];
|
|
else
|
|
v(i,:) = [-Inf Inf];
|
|
end
|
|
else
|
|
v(i,:) = L(i).qlim;
|
|
end
|
|
end
|
|
end
|
|
|
|
function set.gravity(r, v)
|
|
if isvec(v, 3)
|
|
r.gravity = v;
|
|
else
|
|
error('gravity must be a 3-vector');
|
|
end
|
|
end
|
|
|
|
function v = get.config(r)
|
|
v = '';
|
|
for i=1:r.n
|
|
v(i) = r.links(i).RP;
|
|
end
|
|
end
|
|
|
|
% general methods
|
|
|
|
function v = islimit(r,q)
|
|
%SerialLink.islimit Joint limit test
|
|
%
|
|
% V = R.islimit(Q) is a vector of boolean values, one per joint,
|
|
% false (0) if Q(i) is within the joint limits, else true (1).
|
|
%
|
|
% Notes::
|
|
% - Joint limits are purely advisory and are not used in any
|
|
% other function. Just seemed like a useful thing to include...
|
|
%
|
|
% See also Link.islimit.
|
|
L = r.links;
|
|
if length(q) ~= r.n
|
|
error('argument for islimit method is wrong length');
|
|
end
|
|
v = zeros(r.n, 2);
|
|
for i=1:r.n
|
|
v(i,:) = L(i).islimit(q(i));
|
|
end
|
|
end
|
|
|
|
function v = isspherical(r)
|
|
%SerialLink.isspherical Test for spherical wrist
|
|
%
|
|
% R.isspherical() is true if the robot has a spherical wrist, that is, the
|
|
% last 3 axes are revolute and their axes intersect at a point.
|
|
%
|
|
% See also SerialLink.ikine6s.
|
|
L = r.links(end-2:end);
|
|
|
|
v = false;
|
|
% first test that all lengths are zero
|
|
if ~all( [L(1).a L(2).a L(3).a L(2).d L(3).d] == 0 )
|
|
return
|
|
end
|
|
|
|
if (abs(L(1).alpha) == pi/2) && (abs(L(1).alpha + L(2).alpha) < eps)
|
|
v = true;
|
|
return;
|
|
end
|
|
end
|
|
|
|
function payload(r, m, p)
|
|
%SerialLink.payload Add payload mass
|
|
%
|
|
% R.payload(M, P) adds a payload with point mass M at position P
|
|
% in the end-effector coordinate frame.
|
|
%
|
|
% See also SerialLink.rne, SerialLink.gravload.
|
|
lastlink = r.links(r.n);
|
|
lastlink.m = m;
|
|
lastlink.r = p;
|
|
end
|
|
|
|
function jt = jtraj(r, T1, T2, t, varargin)
|
|
%SerialLink.jtraj Joint space trajectory
|
|
%
|
|
% Q = R.jtraj(T1, T2, K) is a joint space trajectory (KxN) where the joint
|
|
% coordinates reflect motion from end-effector pose T1 to T2 in K steps with
|
|
% default zero boundary conditions for velocity and acceleration.
|
|
% The trajectory Q has one row per time step, and one column per joint,
|
|
% where N is the number of robot joints.
|
|
%
|
|
% Note::
|
|
% - Requires solution of inverse kinematics. R.ikine6s() is used if
|
|
% appropriate, else R.ikine(). Additional trailing arguments to R.jtraj()
|
|
% are passed as trailing arugments to these functions.
|
|
%
|
|
% See also jtraj, SerialLink.ikine, SerialLink.ikine6s.
|
|
if r.isspherical && (r.n == 6)
|
|
q1 = r.ikine6s(T1, varargin{:});
|
|
q2 = r.ikine6s(T2, varargin{:});
|
|
else
|
|
q1 = r.ikine(T1, varargin{:});
|
|
q2 = r.ikine(T2, varargin{:});
|
|
end
|
|
|
|
jt = jtraj(q1, q2, t);
|
|
end
|
|
|
|
|
|
function dyn(r, j)
|
|
%SerialLink.dyn Display inertial properties
|
|
%
|
|
% R.dyn() displays the inertial properties of the SerialLink object in a multi-line
|
|
% format. The properties shown are mass, centre of mass, inertia, gear ratio,
|
|
% motor inertia and motor friction.
|
|
%
|
|
% R.dyn(J) as above but display parameters for joint J only.
|
|
%
|
|
% See also Link.dyn.
|
|
if nargin == 2
|
|
r.links(j).dyn()
|
|
else
|
|
r.links.dyn();
|
|
end
|
|
end
|
|
|
|
function record(r, q)
|
|
r.qteach = [r.qteach; q];
|
|
end
|
|
|
|
function sr = sym(r)
|
|
sr = SerialLink(r);
|
|
for i=1:r.n
|
|
sr.links(i) = r.links(i).sym;
|
|
end
|
|
end
|
|
end % methods
|
|
|
|
end % classdef
|
|
|
|
% utility function
|
|
function s = mat2str(m)
|
|
m(abs(m)<eps) = 0;
|
|
s = num2str(m);
|
|
end
|