follow_line_scara

 FOLLOW A LINE IN SPACE WITH A SCARA ROBOT

0001 % FOLLOW A LINE IN SPACE WITH A SCARA ROBOT
0002 
0003 % Copyright (C) 2012, by Arturo Gil Aparicio
0004 %
0005 % This file is part of ARTE (A Robotics Toolbox for Education).
0006 %
0007 % ARTE is free software: you can redistribute it and/or modify
0008 % it under the terms of the GNU Lesser General Public License as published by
0009 % the Free Software Foundation, either version 3 of the License, or
0010 % (at your option) any later version.
0011 %
0012 % ARTE is distributed in the hope that it will be useful,
0013 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0014 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0015 % GNU Lesser General Public License for more details.
0016 %
0017 % You should have received a copy of the GNU Leser General Public License
0018 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0019 
0020 %Compute points in line
0021 close all
0022 
0023 
0024 robot =  load_robot('example','scara');
0025 %matriz de posición/orientación
0026 T=[1  0  0 0;
0027    0 -1  0 0;
0028    0  0 -1 0;
0029    0  0  0 1]
0030 %distancia entre puntos consecutivos
0031 delta = 0.001;
0032 
0033 punto_inicial = [0.7 0 0];
0034 punto_final = [0 0.8 0];
0035 
0036 V=(punto_final-punto_inicial);
0037 d=delta*V/norm(V); %vector normalizado en la dirección de la recta
0038 distancia = sqrt((punto_final-punto_inicial)*(punto_final-punto_inicial)');
0039 %Generación de puntos en la trayectoria
0040 num_points = distancia/delta;
0041 puntos = punto_inicial;
0042 for i=1:num_points,
0043     puntos=[puntos; i*d+punto_inicial];
0044 end
0045 puntos=[puntos;punto_final];
0046 
0047 figure, plot(puntos(:,1),puntos(:,2)), title('Trayectoria en el espacio'), xlabel('X (m)'), ylabel('Y (m)')
0048 
0049 qs=[];
0050 vqs=[];
0051 for i=1:length(puntos),
0052     T(1,4)=puntos(i,1);
0053     T(2,4)=puntos(i,2);
0054     
0055     q = inversekinematic(robot, T);
0056     vq = compute_joint_velocity(robot, q(:,1), V/norm(V)');
0057     %vq = compute_joint_velocity(robot, q(:,2), V/norm(V)');
0058     qs=[qs q(:,1)];
0059     vqs = [vqs vq];
0060 end
0061 
0062 figure, hold, plot(qs(1,:), 'r'),plot(qs(2,:), 'g'), plot(qs(3,:), 'b'), plot(qs(4,:), 'c'),
0063 legend('q_1 (rad)','q_2 (rad)','q_3 (rad)', 'q_4 (rad)'), title('Joint trajectories'), xlabel('Step number')
0064 
0065 figure, hold, plot(vqs(1,:), 'r'),plot(vqs(2,:), 'g'), plot(vqs(3,:), 'b'),
0066 legend('vq_1 (rad/s)','vq_2 (rad/s)','vq_3 (rad/s)'), title('Joint speeds (rad/s)')
0067 
0068 % figure,hold, plot(vqs(1,:)*30/pi, 'r'),plot(vqs(2,:)*30/pi, 'g'), plot(vqs(3,:)*30/pi, 'b'),
0069 % legend('vq_1 (r.p.m.)','vq_2 (r.p.m.)','vq_3 (r.p.m.)'),  title('Joint speeds (r.p.m.)')