%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% JOINT_REFERENCES Obtain q when the robot makes a linear trajectory of the end effector in cartesian space See also INVERSEKINEMATIC %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % JOINT_REFERENCES 0003 % Obtain q when the robot makes a linear trajectory of the end effector 0004 % in cartesian space 0005 % 0006 % See also INVERSEKINEMATIC 0007 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0008 0009 % Copyright (C) 2012, by Arturo Gil Aparicio 0010 % 0011 % This file is part of ARTE (A Robotics Toolbox for Education). 0012 % 0013 % ARTE is free software: you can redistribute it and/or modify 0014 % it under the terms of the GNU Lesser General Public License as published by 0015 % the Free Software Foundation, either version 3 of the License, or 0016 % (at your option) any later version. 0017 % 0018 % ARTE is distributed in the hope that it will be useful, 0019 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0020 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0021 % GNU Lesser General Public License for more details. 0022 % 0023 % You should have received a copy of the GNU Leser General Public License 0024 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0025 function q=joint_references(time) 0026 0027 global robot 0028 0029 time; 0030 if time == 0 0031 robot.q=[0 0 0 0 0 0]'; 0032 end 0033 0034 simulation_time = 0.5;%s 0035 0036 %this is important, as this will be the initial joint 0037 %values in the first integrator 0038 %initial position 0039 T1 = directkinematic(robot, [0 0 0 0 0.1 0]); 0040 %final position 0041 T2 = directkinematic(robot, [0.3 0.3 0.3 0.1 0.1 0.1]); 0042 0043 Q1 = T2quaternion(T1); 0044 Q2 = T2quaternion(T2); 0045 %interpolate between the two orientations 0046 [Qm] = slerp(Q1, Q2, time/simulation_time, 0.01); 0047 0048 0049 0050 initial_point=T1(1:3,4); 0051 end_point=T2(1:3,4); 0052 0053 % %NOA matrix end point 0054 % T2=[1 0 0 0.5; 0055 % 0 1 0 0.4; 0056 % 0 0 1 0.3; 0057 % 0 0 0 1]; 0058 0059 %speed of the movement 0060 speed = norm(initial_point-end_point)/simulation_time;%m/s 0061 0062 v=(end_point-initial_point); 0063 v=v/norm(v); %unit vector in the direction of the line 0064 0065 point = initial_point + speed*time*v; 0066 0067 %T1(1:3,4)=point(1:3); 0068 0069 Ttotal = quaternion2T(Qm, point(1:3)); 0070 qinv = inversekinematic(robot, Ttotal); 0071 q = select_closest_joint_coordinates(robot, qinv, robot.q); 0072 0073 %update current robot joints 0074 robot.q=q;