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.
218 lines
7.7 KiB
218 lines
7.7 KiB
function [ ] = control(motion_config)
|
|
|
|
global do;
|
|
do = true;
|
|
|
|
% DH Parameter
|
|
a = [0,0,0,0,0,0,0];
|
|
alp = [-90,90,-90,90,-90,90,0];
|
|
d = [310.4,0,400.1,0,390,0,78];
|
|
|
|
links = 7;
|
|
L1 = Link('d',d(1),'a',a(1),'alpha',alp(1)/180*pi);
|
|
L1.isrevolute;
|
|
L2 = Link('d',d(2),'a',a(2),'alpha',alp(2)/180*pi);
|
|
L2.isrevolute;
|
|
L3 = Link('d',d(3),'a',a(3),'alpha',alp(3)/180*pi);
|
|
L3.isrevolute;
|
|
L4 = Link('d',d(4),'a',a(4),'alpha',alp(4)/180*pi);
|
|
L4.isrevolute;
|
|
L5 = Link('d',d(5),'a',a(5),'alpha',alp(5)/180*pi);
|
|
L5.isrevolute;
|
|
L6 = Link('d',d(6),'a',a(6),'alpha',alp(6)/180*pi);
|
|
L6.isrevolute;
|
|
L7 = Link('d',d(7),'a',a(7),'alpha',alp(7)/180*pi);
|
|
L7.isrevolute;
|
|
bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', 'Kuka LBR4 +');
|
|
|
|
|
|
axes(motion_config.robofig)
|
|
% plot once because the axis get otherwise messed up
|
|
bot.plot();
|
|
|
|
while (motion_config.run)
|
|
motion_config
|
|
%Calculating 'M'
|
|
|
|
% use this later for multiple points
|
|
point =1;
|
|
M_start = getM(motion_config.rx(point), ...
|
|
motion_config.ry(point), ...
|
|
motion_config.rz(point), ...
|
|
motion_config.tx(point), ...
|
|
motion_config.ty(point), ...
|
|
motion_config.tz(point));
|
|
|
|
M_end = getM(motion_config.rx(point+1), ...
|
|
motion_config.ry(point+1), ...
|
|
motion_config.rz(point+1), ...
|
|
motion_config.tx(point+1), ...
|
|
motion_config.ty(point+1), ...
|
|
motion_config.tz(point+1));
|
|
|
|
args = [motion_config.arm,...
|
|
motion_config.elbow,...
|
|
motion_config.flip,...
|
|
motion_config.delta(point)...
|
|
];
|
|
|
|
for i=1:3
|
|
if args(i) == 0
|
|
args(i) = -1;
|
|
end
|
|
end
|
|
|
|
animation_steps = ceil(motion_config.time/(motion_config.resolution/1000.0));
|
|
t = linspace(0,1,animation_steps)';
|
|
switch ( motion_config.type)
|
|
case 'joint'
|
|
% calculate start and end point
|
|
try
|
|
% calculate start joints
|
|
J_start = LBRinvKin(M_start,args,'LBR4+');
|
|
|
|
% calculate end joints
|
|
J_end = LBRinvKin(M_end,args,'LBR4+');
|
|
|
|
catch err
|
|
err.message
|
|
end
|
|
|
|
J_animation = repmat(J_start,animation_steps,1);
|
|
J_motion = J_end - J_start;
|
|
|
|
% do the interpolation
|
|
switch ( motion_config.core)
|
|
case 'linear'
|
|
% calculate joint movements
|
|
J_animation = repmat(linspace(0,1,animation_steps)',1,links);
|
|
J_animation = J_animation.*repmat(J_motion,animation_steps,1);
|
|
[~,J_animation_d] = gradient( J_animation,t(2));
|
|
[~,J_animation_dd] = gradient( J_animation_d,t(2));
|
|
|
|
case 'bang bang'
|
|
[J_animation, J_animation_d, J_animation_dd] = mtraj(@bangbang, J_motion*0, J_motion, t);
|
|
case '5poly'
|
|
[J_animation, J_animation_d, J_animation_dd] = mtraj(@tpoly, J_motion*0, J_motion, t);
|
|
case 'lspb'
|
|
[J_animation, J_animation_d, J_animation_dd] = mtraj(@lspb, J_motion*0, J_motion, t);
|
|
end
|
|
% add start joints
|
|
J_animation = J_animation+repmat(J_start,animation_steps,1);
|
|
|
|
% calculate motion in 3d
|
|
|
|
|
|
|
|
case 'carthesian'
|
|
switch ( motion_config.core)
|
|
case 'linear'
|
|
s = t;
|
|
case 'bang bang'
|
|
s = bangbang(0, 1, t);
|
|
case '5poly'
|
|
s = tpoly(0, 1, t);
|
|
case 'lspb'
|
|
s = lspb(0,1,t);
|
|
end
|
|
% generate a carthesian based line in space
|
|
M_animation = zeros(4,4,animation_steps);
|
|
|
|
for i = 1:animation_steps
|
|
M_animation(:,:,i) = getM(...
|
|
motion_config.rx(point)+s(i)*(motion_config.rx(point+1)-motion_config.rx(point)), ...
|
|
motion_config.ry(point)+s(i)*(motion_config.ry(point+1)-motion_config.ry(point)), ...
|
|
motion_config.rz(point)+s(i)*(motion_config.rz(point+1)-motion_config.rz(point)), ...
|
|
motion_config.tx(point)+s(i)*(motion_config.tx(point+1)-motion_config.tx(point)), ...
|
|
motion_config.ty(point)+s(i)*(motion_config.ty(point+1)-motion_config.ty(point)), ...
|
|
motion_config.tz(point)+s(i)*(motion_config.tz(point+1)-motion_config.tz(point)) ...
|
|
);
|
|
end
|
|
%M_animation = ctraj(M_start,M_end,s);
|
|
|
|
J_animation = zeros(animation_steps,links);
|
|
|
|
%setup the configuration of the current position
|
|
arm = motion_config.arm;
|
|
elbow = motion_config.elbow;
|
|
flip = motion_config.flip;
|
|
for i = 1:animation_steps
|
|
try
|
|
if (i == 1)
|
|
% set the starting point
|
|
J_animation(i,:) = LBRinvKin(M_animation(:,:,i),args,'LBR4+');
|
|
else
|
|
%not the starting point -> calculate by minimal
|
|
%change
|
|
[J_animation(i,:),new_arm,new_elbow,new_flip] = LBRinvKinMinChange(M_animation(:,:,i),J_animation(i-1,:),motion_config.delta(point),'LBR4+');
|
|
if (new_arm ~= arm || new_elbow ~= elbow || new_flip ~= flip )
|
|
|
|
arm = new_arm;
|
|
elbow = new_elbow;
|
|
flip = new_flip;
|
|
fprintf('config change on %d arm %d, elbow %d, flip %d \n',i,arm,elbow,flip);
|
|
|
|
end
|
|
end
|
|
|
|
catch err
|
|
%err.message
|
|
%fprintf('no join angle for animation_step %d use prev value instead\n',i);
|
|
if(i > 1)
|
|
J_animation(i,:) = J_animation(i-1,:);
|
|
end
|
|
end
|
|
end
|
|
[~,J_animation_d] = gradient( J_animation,t(2));
|
|
[~,J_animation_dd] = gradient( J_animation_d,t(2));
|
|
end
|
|
|
|
try
|
|
|
|
% draw the joint speed and derivitives
|
|
|
|
% plot the joint movement
|
|
axes(motion_config.joinsfig)
|
|
plot(t, J_animation);
|
|
h = rotate3d;
|
|
set(h,'RotateStyle','box','Enable','off');
|
|
|
|
|
|
% plot the joint velocity
|
|
axes(motion_config.joinsdfig)
|
|
plot(t,J_animation_d);
|
|
h = rotate3d;
|
|
set(h,'RotateStyle','box','Enable','off');
|
|
|
|
% plot the joint acceleration
|
|
axes(motion_config.joinsddfig)
|
|
plot(t,J_animation_dd);
|
|
h = rotate3d;
|
|
set(h,'RotateStyle','box','Enable','off');
|
|
|
|
% do the animation
|
|
axes(motion_config.robofig)
|
|
h = rotate3d;
|
|
set(h,'RotateStyle','box','Enable','on');
|
|
|
|
tic % start timer
|
|
|
|
acutual_animations = 0;
|
|
while motion_config.run && toc <= motion_config.time
|
|
i = toc / motion_config.time * animation_steps;
|
|
i = ceil(i);
|
|
if (i > animation_steps)
|
|
i = animation_steps;
|
|
end
|
|
bot.plot(J_animation(i,:).*(pi/180));
|
|
acutual_animations = acutual_animations +1 ;
|
|
end
|
|
if (acutual_animations < animation_steps)
|
|
display(sprintf('warning skipped %d frames',animation_steps-acutual_animations));
|
|
end
|
|
catch err
|
|
err.message
|
|
end
|
|
|
|
end
|
|
|