Copyright (C) 2012, by Arturo Gil Aparicio This file is part of ARTE (A Robotics Toolbox for Education). ARTE 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. ARTE 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 ARTE. If not, see <http://www.gnu.org/licenses/>.
0001 % Copyright (C) 2012, by Arturo Gil Aparicio 0002 % 0003 % This file is part of ARTE (A Robotics Toolbox for Education). 0004 % 0005 % ARTE is free software: you can redistribute it and/or modify 0006 % it under the terms of the GNU Lesser General Public License as published by 0007 % the Free Software Foundation, either version 3 of the License, or 0008 % (at your option) any later version. 0009 % 0010 % ARTE is distributed in the hope that it will be useful, 0011 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0012 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0013 % GNU Lesser General Public License for more details. 0014 % 0015 % You should have received a copy of the GNU Leser General Public License 0016 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0017 0018 %Compute points in line 0019 close all 0020 0021 robot = load_robot('mitsubishi','pa-10'); 0022 0023 0024 %NOA matrix initial point 0025 T1=[1 0 0 0.5; 0026 0 1 0 -0.3; 0027 0 0 1 0.4; 0028 0 0 0 1] 0029 %NOA matrix end point 0030 T2=[1 0 0 0.5; 0031 0 1 0 0.4; 0032 0 0 1 0.3; 0033 0 0 0 1] 0034 0035 %distancia entre puntos consecutivos 0036 delta = 0.02; 0037 0038 punto_inicial = T1(1:3,4); 0039 punto_final = T2(1:3,4); 0040 0041 v=(punto_final-punto_inicial); 0042 v=delta*v/norm(v); %vector normalizado en la dirección de la recta 0043 distancia = sqrt((punto_final-punto_inicial)'*(punto_final-punto_inicial)); 0044 %Generación de puntos en la trayectoria 0045 num_points = floor(distancia/delta); 0046 puntos = punto_inicial; 0047 for i=1:num_points, 0048 puntos=[puntos i*v+punto_inicial]; 0049 end 0050 puntos=[puntos punto_final]; 0051 0052 figure, hold on, grid, plot3(puntos(1,:),puntos(2,:),puntos(3,:)), title('Trajectory in space'), xlabel('X (m)'), ylabel('Y (m)') 0053 0054 qs=[]; 0055 for i=1:length(puntos), 0056 T1(1:3,4)=puntos(1:3,i); 0057 qinv = inversekinematic(robot, T1); 0058 0059 q=select_closest_joint_coordinates(robot, qinv, robot.q); 0060 qs=[qs q]; 0061 robot.q=q; 0062 end 0063 0064 drawrobot3d(robot, qs(:,1)) 0065 adjust_view(robot) 0066 drawrobot3d(robot, qs(:,end)) 0067 0068 %Now, animate the robot in 3D 0069 animate(robot, qs); 0070 0071 figure, hold, plot(qs(1,:), 'r.'),plot(qs(2,:), 'g.'), plot(qs(3,:), 'b.'), plot(qs(4,:), 'c.'), 0072 plot(qs(5,:), 'm.'), plot(qs(6,:), 'y.'), 0073 legend('q_1 (rad)','q_2 (rad)','q_3 (rad)', 'q_4 (rad)', 'q_5 (rad)', 'q_6 (rad)' ), title('Joint trajectories'), xlabel('Step number') 0074