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.
478 lines
12 KiB
478 lines
12 KiB
%PLOT Graphical robot animation
|
|
%
|
|
% PLOT(ROBOT, Q)
|
|
% PLOT(ROBOT, Q, options)
|
|
%
|
|
% Displays a graphical animation of a robot based on the
|
|
% kinematic model. A stick figure polyline joins the origins of
|
|
% the link coordinate frames.
|
|
% The robot is displayed at the joint angle Q, or if a matrix it is
|
|
% animated as the robot moves along the trajectory.
|
|
%
|
|
% The graphical robot object holds a copy of the robot object and
|
|
% the graphical element is tagged with the robot's name (.name method).
|
|
% This state also holds the last joint configuration
|
|
% drawn (.q method).
|
|
%
|
|
% If no robot of this name is currently displayed then a robot will
|
|
% be drawn in the current figure. If the robot already exists then
|
|
% that graphical model will be found and moved.
|
|
%
|
|
% MULTIPLE VIEWS OF THE SAME ROBOT
|
|
% If one or more plots of this robot already exist then these will all
|
|
% be moved according to the argument Q.
|
|
%
|
|
% MULTIPLE ROBOTS
|
|
% Multiple robots can be displayed in the same plot, by using "hold on"
|
|
% before calls to plot(robot).
|
|
%
|
|
% options is a list of any of the following:
|
|
% 'workspace' [xmin, xmax ymin ymax zmin zmax]
|
|
% 'perspective' 'ortho' controls camera view mode
|
|
% 'erase' 'noerase' controls erasure of arm during animation
|
|
% 'base' 'nobase' controls display of base 'pedestal'
|
|
% 'loop' 'noloop' controls display of base 'pedestal'
|
|
% 'wrist' 'nowrist' controls display of wrist
|
|
% 'name', 'noname' display the robot's name near the first joint
|
|
% 'shadow' 'noshadow' controls display of shadow
|
|
% 'xyz' 'noa' wrist axis label
|
|
% 'joints' 'nojoints' controls display of joints
|
|
% 'mag' scale annotation scale factor
|
|
%
|
|
% The options come from 3 sources and are processed in the order:
|
|
% 1. Cell array of options returned by the function PLOTBOTOPT
|
|
% 2. Cell array of options returned by the .plotopt method of the
|
|
% robot object. These are set by the .plotopt method.
|
|
% 3. List of arguments in the command line.
|
|
%
|
|
% GRAPHICAL ANNOTATIONS:
|
|
%
|
|
% The basic stick figure robot can be annotated with
|
|
% shadow on the floor
|
|
% XYZ wrist axes and labels
|
|
% joint cylinders and axes
|
|
%
|
|
% All of these require some kind of dimension and this is determined
|
|
% using a simple heuristic from the workspace dimensions. This dimension
|
|
% can be changed by setting the multiplicative scale factor using the
|
|
% 'mag' option above.
|
|
%
|
|
% GETTING GRAPHICAL ROBOT STATE:
|
|
% q = PLOT(ROBOT)
|
|
% Returns the joint configuration from the state of an existing
|
|
% graphical representation of the specified robot. If multiple
|
|
% instances exist, that of the first one returned by findobj() is
|
|
% given.
|
|
|
|
%
|
|
% See also PLOTBOTOPT, DRIVEBOT, FKINE, ROBOT.
|
|
|
|
% Copright (C) Peter Corke 1993
|
|
|
|
% HANDLES:
|
|
%
|
|
% A robot comprises a bunch of individual graphical elements and these are
|
|
% kept in a structure which can be stored within the .handle element of a
|
|
% robot object.
|
|
% h.robot the robot stick figure
|
|
% h.shadow the robot's shadow
|
|
% h.x wrist vectors
|
|
% h.y
|
|
% h.z
|
|
% h.xt wrist vector labels
|
|
% h.yt
|
|
% h.zt
|
|
%
|
|
% The plot function returns a new robot object with the handle element set.
|
|
%
|
|
% For the h.robot object we additionally:
|
|
% save this new robot object as its UserData
|
|
% tag it with the name field from the robot object
|
|
%
|
|
% This enables us to find all robots with a given name, in all figures,
|
|
% and update them.
|
|
|
|
% MOD.HISTORY
|
|
% 12/94 make axis scaling adjust to robot kinematic params
|
|
% 4/99 use objects
|
|
% 2/01 major rewrite, axis names, drivebot, state etc.
|
|
|
|
|
|
% 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 rnew = plot(robot, tg, varargin)
|
|
|
|
%
|
|
% q = PLOT(robot)
|
|
% return joint coordinates from a graphical robot of given name
|
|
%
|
|
if nargin == 1,
|
|
rh = findobj('Tag', robot.name);
|
|
if ~isempty(rh),
|
|
r = get(rh(1), 'UserData');
|
|
rnew = r.q;
|
|
end
|
|
return
|
|
end
|
|
|
|
np = numrows(tg);
|
|
n = robot.n;
|
|
|
|
if numcols(tg) ~= n,
|
|
error('Insufficient columns in q')
|
|
end
|
|
|
|
if ~isfield(robot, 'handles'),
|
|
%
|
|
% if there are no handles in this object we assume it has
|
|
% been invoked from the command line not drivebot() so we
|
|
% process the options
|
|
%
|
|
|
|
%%%%%%%%%%%%%% process options
|
|
erasemode = 'xor';
|
|
joints = 1;
|
|
wrist = 1;
|
|
repeat = 1;
|
|
shadow = 1;
|
|
wrist = 1;
|
|
dims = [];
|
|
base = 0;
|
|
wristlabel = 'xyz';
|
|
projection = 'orthographic';
|
|
magscale = 1;
|
|
name = 1;
|
|
|
|
% read options string in the order
|
|
% 1. robot.plotopt
|
|
% 2. the M-file plotbotopt if it exists
|
|
% 3. command line arguments
|
|
if exist('plotbotopt', 'file') == 2,
|
|
options = plotbotopt;
|
|
options = [options robot.plotopt varargin];
|
|
else
|
|
options = [robot.plotopt varargin];
|
|
end
|
|
i = 1;
|
|
while i <= length(options),
|
|
switch lower(options{i}),
|
|
case 'workspace'
|
|
dims = options{i+1};
|
|
i = i+1;
|
|
case 'mag'
|
|
magscale = options{i+1};
|
|
i = i+1;
|
|
case 'perspective'
|
|
projection = 'perspective';
|
|
case 'ortho'
|
|
projection = 'orthographic';
|
|
case 'erase'
|
|
erasemode = 'xor';
|
|
case 'noerase'
|
|
erasemode = 'none';
|
|
case 'base'
|
|
base = 1;
|
|
case 'nobase'
|
|
base = 0;
|
|
case 'loop'
|
|
repeat = Inf;
|
|
case 'noloop'
|
|
repeat = 1;
|
|
case 'name',
|
|
name = 1;
|
|
case 'noname',
|
|
name = 0;
|
|
case 'wrist'
|
|
wrist = 1;
|
|
case 'nowrist'
|
|
wrist = 0;
|
|
case 'shadow'
|
|
shadow = 1;
|
|
case 'noshadow'
|
|
shadow = 0;
|
|
case 'xyz'
|
|
wristlabel = 'XYZ';
|
|
case 'noa'
|
|
wristlabel = 'NOA';
|
|
case 'joints'
|
|
joints = 1;
|
|
case 'nojoints'
|
|
joints = 0;
|
|
otherwise
|
|
error(['unknown option: ' options{i}]);
|
|
end
|
|
i = i+1;
|
|
end
|
|
|
|
if isempty(dims),
|
|
%
|
|
% simple heuristic to figure the maximum reach of the robot
|
|
%
|
|
L = robot.links;
|
|
reach = 0;
|
|
for i=1:n,
|
|
reach = reach + abs(L(i).a) + abs(L(i).d);
|
|
end
|
|
dims = [-reach reach -reach reach -reach reach];
|
|
mag = reach/10;
|
|
else
|
|
reach = min(abs(dims));
|
|
end
|
|
mag = magscale * reach/10;
|
|
end
|
|
|
|
%
|
|
% setup an axis in which to animate the robot
|
|
%
|
|
if isempty(robot.handle) & isempty(findobj(gcf, 'Tag', robot.name)),
|
|
if ~ishold,
|
|
clf
|
|
axis(dims);
|
|
end
|
|
figure(gcf); % bring to the top
|
|
xlabel('X')
|
|
ylabel('Y')
|
|
zlabel('Z')
|
|
set(gca, 'drawmode', 'fast');
|
|
grid on
|
|
|
|
zlim = get(gca, 'ZLim');
|
|
h.zmin = zlim(1);
|
|
|
|
if base,
|
|
b = transl(robot.base);
|
|
line('xdata', [b(1);b(1)], ...
|
|
'ydata', [b(2);b(2)], ...
|
|
'zdata', [h.zmin;b(3)], ...
|
|
'LineWidth', 4, ...
|
|
'color', 'red');
|
|
end
|
|
if name,
|
|
b = transl(robot.base);
|
|
text(b(1), b(2)-mag, [' ' robot.name])
|
|
end
|
|
% create a line which we will
|
|
% subsequently modify. Set erase mode to xor for fast
|
|
% update
|
|
%
|
|
h.robot = line(robot.lineopt{:}, ...
|
|
'Erasemode', erasemode);
|
|
if shadow == 1,
|
|
h.shadow = line(robot.shadowopt{:}, ...
|
|
'Erasemode', erasemode);
|
|
end
|
|
|
|
if wrist == 1,
|
|
h.x = line('xdata', [0;0], ...
|
|
'ydata', [0;0], ...
|
|
'zdata', [0;0], ...
|
|
'color', 'red', ...
|
|
'erasemode', 'xor');
|
|
h.y = line('xdata', [0;0], ...
|
|
'ydata', [0;0], ...
|
|
'zdata', [0;0], ...
|
|
'color', 'green', ...
|
|
'erasemode', 'xor');
|
|
h.z = line('xdata', [0;0], ...
|
|
'ydata', [0;0], ...
|
|
'zdata', [0;0], ...
|
|
'color', 'blue', ...
|
|
'erasemode', 'xor');
|
|
h.xt = text(0, 0, wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
h.yt = text(0, 0, wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
h.zt = text(0, 0, wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
end
|
|
|
|
%
|
|
% display cylinders (revolute) or boxes (pristmatic) at
|
|
% each joint, as well as axis centerline.
|
|
%
|
|
if joints == 1,
|
|
L = robot.links;
|
|
for i=1:robot.n,
|
|
|
|
% cylinder or box to represent the joint
|
|
if L(i).sigma == 0,
|
|
N = 8;
|
|
else
|
|
N = 4;
|
|
end
|
|
|
|
[xc,yc,zc] = cylinder([mag/4, mag/4], N);
|
|
zc(zc==0) = -mag/2;
|
|
zc(zc==1) = mag/2;
|
|
|
|
% add the surface object and color it
|
|
h.joint(i) = surface(xc,yc,zc);
|
|
%set(h.joint(i), 'erasemode', 'xor');
|
|
set(h.joint(i), 'FaceColor', 'blue');
|
|
|
|
% build a matrix of coordinates so we
|
|
% can transform the cylinder in animate()
|
|
% and hang it off the cylinder
|
|
xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)];
|
|
set(h.joint(i), 'UserData', xyz);
|
|
|
|
% add a dashed line along the axis
|
|
h.jointaxis(i) = line('xdata', [0;0], ...
|
|
'ydata', [0;0], ...
|
|
'zdata', [0;0], ...
|
|
'color', 'blue', ...
|
|
'linestyle', '--', ...
|
|
'erasemode', 'xor');
|
|
|
|
end
|
|
end
|
|
h.mag = mag;
|
|
robot.handle = h;
|
|
set(h.robot, 'Tag', robot.name);
|
|
set(h.robot, 'UserData', robot);
|
|
end
|
|
|
|
|
|
% save the handles in the passed robot object
|
|
|
|
rh = findobj('Tag', robot.name);
|
|
for r=1:repeat,
|
|
for p=1:np,
|
|
for r=rh',
|
|
animate( get(r, 'UserData'), tg(p,:));
|
|
end
|
|
end
|
|
end
|
|
|
|
% save the last joint angles away in the graphical robot
|
|
for r=rh',
|
|
rr = get(r, 'UserData');
|
|
rr.q = tg(end,:);
|
|
set(r, 'UserData', rr);
|
|
end
|
|
|
|
if nargout > 0,
|
|
rnew = robot;
|
|
end
|
|
|
|
function animate(robot, q)
|
|
|
|
n = robot.n;
|
|
h = robot.handle;
|
|
L = robot.links;
|
|
|
|
mag = h.mag;
|
|
|
|
b = transl(robot.base);
|
|
x = b(1);
|
|
y = b(2);
|
|
z = b(3);
|
|
|
|
xs = b(1);
|
|
ys = b(2);
|
|
zs = h.zmin;
|
|
|
|
% compute the link transforms, and record the origin of each frame
|
|
% for the animation.
|
|
t = robot.base;
|
|
Tn = t;
|
|
for j=1:n,
|
|
Tn(:,:,j) = t;
|
|
|
|
|
|
t = t * L(j).A(q(j));
|
|
|
|
x = [x; t(1,4)];
|
|
y = [y; t(2,4)];
|
|
z = [z; t(3,4)];
|
|
xs = [xs; t(1,4)];
|
|
ys = [ys; t(2,4)];
|
|
zs = [zs; h.zmin];
|
|
end
|
|
t = t *robot.tool;
|
|
|
|
%
|
|
% draw the robot stick figure and the shadow
|
|
%
|
|
set(h.robot,'xdata', x, 'ydata', y, 'zdata', z);
|
|
if isfield(h, 'shadow'),
|
|
set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs);
|
|
end
|
|
|
|
|
|
%
|
|
% display the joints as cylinders with rotation axes
|
|
%
|
|
if isfield(h, 'joint'),
|
|
xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1];
|
|
|
|
for j=1:n,
|
|
% get coordinate data from the cylinder
|
|
xyz = get(h.joint(j), 'UserData');
|
|
xyz = Tn(:,:,j) * xyz;
|
|
ncols = numcols(xyz)/2;
|
|
xc = reshape(xyz(1,:), 2, ncols);
|
|
yc = reshape(xyz(2,:), 2, ncols);
|
|
zc = reshape(xyz(3,:), 2, ncols);
|
|
|
|
set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ...
|
|
'Zdata', zc);
|
|
|
|
xyzl = Tn(:,:,j) * xyz_line;
|
|
set(h.jointaxis(j), 'Xdata', xyzl(1,:), 'Ydata', xyzl(2,:), 'Zdata', xyzl(3,:));
|
|
xyzl = Tn(:,:,j) * xyz_line;
|
|
h.jointlabel(j) = text(xyzl(1,1), xyzl(2,1) , xyzl(3,1), num2str(j), 'HorizontalAlignment', 'Center');
|
|
end
|
|
end
|
|
|
|
%
|
|
% display the wrist axes and labels
|
|
%
|
|
if isfield(h, 'x'),
|
|
%
|
|
% compute the wrist axes, based on final link transformation
|
|
% plus the tool transformation.
|
|
%
|
|
xv = t*[mag;0;0;1];
|
|
yv = t*[0;mag;0;1];
|
|
zv = t*[0;0;mag;1];
|
|
|
|
%
|
|
% update the line segments, wrist axis and links
|
|
%
|
|
set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ...
|
|
'zdata', [t(3,4) xv(3)]);
|
|
set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ...
|
|
'zdata', [t(3,4) yv(3)]);
|
|
set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ...
|
|
'zdata', [t(3,4) zv(3)]);
|
|
|
|
wristlabel = "XYZ";
|
|
% text(xv(1),xv(2),xv(3), ['X'])
|
|
% text(yv(1),yv(2),yv(3), ['Y'])
|
|
% text(zv(1),zv(2),zv(3), ['Z'])
|
|
h.xt = text(xv(1),xv(2),xv(3), wristlabel(1), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
h.yt = text(yv(1),yv(2),yv(3), wristlabel(2), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
h.zt = text(zv(1),zv(2),zv(3), wristlabel(3), 'FontWeight', 'bold', 'HorizontalAlignment', 'Center');
|
|
|
|
% set(h.zt, 'rotation', zv(1:3));
|
|
% set(h.xt, 'Position', xv(1:3));
|
|
% set(h.yt, 'Position', yv(1:3));
|
|
end
|
|
|
|
drawnow
|