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.
128 lines
3.2 KiB
128 lines
3.2 KiB
%SerialLink.animate Update a robot animation
|
|
%
|
|
% R.animate(q) updates an existing animation for the robot R. This will have
|
|
% been created using R.plot().
|
|
%
|
|
% Updates graphical instances of this robot in all figures.
|
|
%
|
|
% Notes::
|
|
% - Not a general purpose method, used for Simulink robot animation.
|
|
%
|
|
% See also SerialLink.plot.
|
|
|
|
function animate(robot, q, handles)
|
|
|
|
if nargin < 3
|
|
handles = findobj('Tag', robot.name);
|
|
end
|
|
|
|
for handle=handles' % for each graphical robot instance
|
|
h = get(handle, 'UserData');
|
|
animate2( h, q);
|
|
end
|
|
|
|
|
|
function animate2(h, q)
|
|
|
|
robot = h.robot;
|
|
n = robot.n;
|
|
L = robot.links;
|
|
|
|
mag = h.mag;
|
|
|
|
% compute the link transforms, and record the origin of each frame
|
|
% for the animation.
|
|
t = robot.base;
|
|
Tn = t;
|
|
x = zeros(1,n);
|
|
y = zeros(1,n);
|
|
z = zeros(1,n);
|
|
xs = zeros(1,n);
|
|
ys = zeros(1,n);
|
|
zs = zeros(1,n);
|
|
|
|
% add first point to the link/shadow line, the base
|
|
x(1) = t(1,4);
|
|
y(1) = t(2,4);
|
|
z(1) = t(3,4);
|
|
xs(1) = t(1,4);
|
|
ys(1) = t(2,4);
|
|
zs(1) = h.zmin;
|
|
|
|
% add subsequent points
|
|
for j=1:n
|
|
Tn(:,:,j) = t;
|
|
|
|
t = t * L(j).A(q(j));
|
|
|
|
x(j+1) = t(1,4);
|
|
y(j+1) = t(2,4);
|
|
z(j+1) = t(3,4);
|
|
xs(j+1) = t(1,4);
|
|
ys(j+1) = t(2,4);
|
|
zs(j+1) = h.zmin;
|
|
end
|
|
t = t *robot.tool;
|
|
|
|
%
|
|
% draw the robot stick figure and the shadow
|
|
%
|
|
set(h.links,'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;
|
|
if isfield(h, 'jointaxis')
|
|
set(h.jointaxis(j), 'Xdata', xyzl(1,:), ...
|
|
'Ydata', xyzl(2,:), ...
|
|
'Zdata', xyzl(3,:));
|
|
set(h.jointlabel(j), 'Position', xyzl(1:3,1));
|
|
end
|
|
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)]);
|
|
set(h.xt, 'Position', xv(1:3));
|
|
set(h.yt, 'Position', yv(1:3));
|
|
set(h.zt, 'Position', zv(1:3));
|
|
end
|