SCRIPT TO TEST THE KINEMATICS OF THE 3RRR robot 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 % 0002 % SCRIPT TO TEST THE KINEMATICS OF THE 3RRR robot 0003 % 0004 % Copyright (C) 2012, by Arturo Gil Aparicio 0005 % 0006 % This file is part of ARTE (A Robotics Toolbox for Education). 0007 % 0008 % ARTE is free software: you can redistribute it and/or modify 0009 % it under the terms of the GNU Lesser General Public License as published by 0010 % the Free Software Foundation, either version 3 of the License, or 0011 % (at your option) any later version. 0012 % 0013 % ARTE is distributed in the hope that it will be useful, 0014 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0015 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0016 % GNU Lesser General Public License for more details. 0017 % 0018 % You should have received a copy of the GNU Leser General Public License 0019 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0020 0021 close all 0022 0023 %load the robot 0024 %the robot is represented (interiorly) by three planar 2dof arms 0025 robot=load_robot('example','3RRR'); 0026 0027 adjust_view(robot) 0028 0029 0030 %init T 0031 T=eye(4); 0032 %Represent a final point in space that will be achieved by the end effector 0033 %P=[xA, yA]=[1.5 1.5] 0034 T(1,4)=1.0; 0035 T(2,4)=1.0; 0036 %orientation 0037 phi=pi/3; 0038 T(1,1)=cos(phi) 0039 T(2,1)=sin(phi) 0040 0041 %find 8 solutions for the INVERSE KINEMATIC in position 0042 q=inversekinematic(robot, T) 0043 0044 %plot every solution 0045 drawrobot3d(robot, q(:,1)), pause(2); 0046 drawrobot3d(robot, q(:,2)), pause(2); 0047 drawrobot3d(robot, q(:,3)), pause(2); 0048 drawrobot3d(robot, q(:,4)), pause(2); 0049 drawrobot3d(robot, q(:,5)), pause(2); 0050 drawrobot3d(robot, q(:,6)), pause(2); 0051 drawrobot3d(robot, q(:,7)), pause(2); 0052 drawrobot3d(robot, q(:,8)), pause(2); 0053 0054 0055 %Make a line in space with a variation in the angle phi 0056 x=linspace(1,1.4,50); 0057 y=linspace(1.1,1.15,50); 0058 th=linspace(0,pi/3,50); 0059 Q=[]; 0060 for i=1:length(x), 0061 T(1,4)=x(i); 0062 T(2,4)=y(i); 0063 %orientation 0064 0065 T(1,1)=cos(th(i)); 0066 T(2,1)=sin(th(i)) 0067 0068 q=inversekinematic(robot, T); 0069 0070 0071 drawrobot3d(robot, q(:,1), 1) %noclear activated 0072 drawrobot3d(robot, q(:,1)) %noclear deactivated 0073 end 0074 0075 0076 %TEST NOW the directkinematic function 0077 T(1,4)=1.0; 0078 T(2,4)=1.0; 0079 %orientation 0080 phi=pi/3; 0081 T(1,1)=cos(phi) 0082 T(2,1)=sin(phi) 0083 0084 %find 8 solutions for the INVERSE KINEMATIC in position 0085 q=inversekinematic(robot, T) 0086 0087 %now, for the first solution 0088 T=directkinematic(robot, [q(1,1) q(3,1) q(5,1)]) 0089 %caution: there may exist less than 8 possible solutions 0090 for i=1:8, 0091 drawrobot3d(robot, q(:,i)) 0092 % 1 and 3 correspond to the joint variables of the robot 0093 T=directkinematic(robot, [q(1,i) q(3,i) q(5,i)]) 0094 pause(2); 0095 close all 0096 end 0097