toy_example.m

% Uses the robotics toolbox from Peter I, Corke
% http://www.petercorke.com
% Robot model from the classic Puma 560
% https://en.wikipedia.org/wiki/Programmable_Universal_Machine_for_Assembly
% video on youtube: https://www.youtube.com/watch?v=uSICxNwDfn0&feature=youtu.be

% Calculate inverse kinematics for a given cartesion path

Ta = transl(-0.2, 0, 0.2) * angvec2tr(pi / 4, [0 1 0]) * transl(0.5, -0.5, 0.5); % define the start point
Tb = angvec2tr(pi / 4, [0 1 0]) *  transl(0.5, -0.5, 0.5);
Tc = angvec2tr(pi / 4, [0 1 0]) * transl(0.5, 0.5, 0.5);	% and destination
Td = transl(-0.2, 0, 0.2) * angvec2tr(pi / 4, [0 1 0]) * transl(0.5, 0.5, 0.5); % define the start point

figure()
trplot(Ta, 'frame', 'A')
hold on
trplot(Tb, 'frame', 'B')
trplot(Tc, 'frame', 'C')
trplot(Tc, 'frame', 'D')
hold off

%%

plot3([0.8 0.8], [-0.5 0.5], [0 0], 'o-')
axis([ -1 1 -1 1 -1 1])
hold on

%%

T_in = ctraj(Ta, Tb, 10);
T_weld = ctraj(Tb, Tc, 30); 	% compute a Cartesian path
T_out = ctraj(Tc, Td, 10);

mdl_puma560

q_in = p560.ikine6s(T_in);
q_weld = p560.ikine6s(T_weld);
q_out = p560.ikine6s(T_out);

q_total = [q_in; q_weld; q_out];

p560.plot(q_total)