%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MoveL: Make a linear planning in space %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % MoveL: Make a linear planning in space 0003 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0004 0005 % Copyright (C) 2012, by Arturo Gil Aparicio 0006 % 0007 % This file is part of ARTE (A Robotics Toolbox for Education). 0008 % 0009 % ARTE is free software: you can redistribute it and/or modify 0010 % it under the terms of the GNU Lesser General Public License as published by 0011 % the Free Software Foundation, either version 3 of the License, or 0012 % (at your option) any later version. 0013 % 0014 % ARTE is distributed in the hope that it will be useful, 0015 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0016 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0017 % GNU Lesser General Public License for more details. 0018 % 0019 % You should have received a copy of the GNU Leser General Public License 0020 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0021 function MoveL(robtarget, speeddata, zonedata, gripper, Wobj) 0022 0023 global configuration robot 0024 0025 fprintf('\nCall to MoveL', robot.name); 0026 0027 %obtain current joint coordinates 0028 q_current= robot.q; 0029 0030 %Ttool, transformation from the robot's end to the TCP 0031 Ttool = transform_to_homogeneous(gripper(2:end)); 0032 0033 %compute current transformation matrix, as well as position p0 and 0034 %quaternion q0 0035 %current position and orientation of the last link 0036 T=directkinematic(robot, robot.q); 0037 0038 %consider the total transformation including the gripper 0039 Ttotal = T*Ttool; 0040 0041 %last target 0042 p0 = Ttotal(1:3,4); 0043 %orientation 0044 Q0 = T2quaternion(Ttotal); 0045 0046 0047 %in this case, plan a linear trajectory from point 0048 %if robot.last_zone_data=='fine' 0049 %compute target joint coordinates, from the inverse kinematics 0050 %select desired joint values by using conf data from robtarget 0051 %select configuration 0052 final_conf = get_conf_data(robtarget); 0053 Ttotal = transform_to_homogeneous(robtarget); 0054 0055 0056 %final position 0057 p1 = Ttotal(1:3,4); 0058 % and orientation 0059 Q1 = T2quaternion(Ttotal); 0060 0061 distance = norm(p1-p0); 0062 %unit vector in the direction of the trajectory normalize 0063 u = (p1-p0)/norm(p1-p0); 0064 0065 speed = obtain_linear_speed(robot, speeddata); 0066 0067 tmax = distance/speed; 0068 0069 %compute current robot speed in modulus. m/s 0070 v_current = 1; 0071 0072 %in this case, if the total time for the movement is lower than the 0073 %configuration.delta_time time then 10 points are interpolated 0074 if tmax <= configuration.delta_time 0075 %disp('/RAPID/MOVEL: No movement is performed. Making a null movement of 1 second'); 0076 %tmax = 2*configuration.delta_time; %avoid errors 0077 %distance = 1; 0078 %u = (p1-p0)/norm(p1-p0); 0079 t = [0:tmax/10:tmax]'; 0080 else 0081 %local time for the planning, normal case 0082 t = [0:configuration.delta_time:tmax]'; 0083 end 0084 0085 0086 %when fine is specified, the radius is zero, otherwise, 0087 %the radius represents a sphere around the target where the 0088 %movement is changed to the next target 0089 radius = obtain_zone_data(zonedata); 0090 if radius == 0 %target point is fine 0091 %plan an acceleration profile one the line that connects both 0092 %points. Asume as a one joint planifier 0093 [pos, vel, accel] = single_joint_spline(0, distance, v_current, 0, t); 0094 else 0095 [pos, vel, accel] = single_joint_spline(0, distance, v_current, speed, t); 0096 end 0097 0098 %current_conf = compute_configuration(robot, robot.q); 0099 0100 pp=[]; 0101 q_line=[]; 0102 0103 N=length(pos); 0104 for j=1:N, 0105 pj = p0 + u*pos(j); 0106 0107 %interpolate between the initial and final quaternions 0108 %to find an orientation between both 0109 [Qm] = slerp(Q0, Q1, (j-1)/N, 0.01); 0110 0111 %find homogeneous matrix corresponding to pi and Qm 0112 Ttotal = quaternion2T(Qm, pj); 0113 0114 pp=[pp T(1:3,4)]; 0115 0116 %find T, of the robot's end points 0117 %In case there is a tool attached to the robot, undo the Tool 0118 %transformation 0119 %T=Ttotal*inv(Ttool); 0120 T=Ttotal/Ttool; 0121 0122 %find all the possible solutions to reach T 0123 qinv = inversekinematic(robot, T); 0124 0125 % select at each timestep the joint coordinates closer to the current 0126 % one. This strategy fails if the robot goes through a singularity. 0127 % In addition, care must be taken when programming the robot, since the 0128 % configuration control may stop the robot. In RAPID the ConfJ and 0129 % ConfL instructions should be used to control the possible changes of 0130 % configuration in linear or joint coordinated motions. 0131 q=select_closest_joint_coordinates(qinv, robot.q); 0132 0133 %store the joint coordinates in a vector for ulterior animation 0134 q_line = [q_line q]; 0135 0136 %the robot performs the movement until the index found. The coordinates, joint speed and acceleratin 0137 %are stored and used in the planning of the next point 0138 robot.q=q; 0139 robot.qd=zeros(robot.DOF,1); 0140 robot.qdd=zeros(robot.DOF,1); 0141 0142 %store all the trajectory for plotting 0143 %the joint trajectories, speeds and acceleration of susequent movements are 0144 %store here 0145 robot.q_vector=[robot.q_vector q]; 0146 robot.qd_vector=[robot.qd_vector zeros(robot.DOF,1)]; 0147 robot.qdd_vector=[robot.qdd_vector zeros(robot.DOF,1)]; 0148 0149 %update current configuration 0150 % current_conf = compute_configuration(robot, robot.q); 0151 end 0152 0153 0154 %a global time for the planning is computed. 0155 %in this way, the total trajectory of different movements can be plotted 0156 if length(robot.time)==0 0157 tend = 0; 0158 else 0159 tend = robot.time(end); 0160 end 0161 t = t + tend; 0162 %store total time 0163 robot.time=[robot.time t']; 0164 0165 0166 %Test whether there are joints outside mechanical limits 0167 test_joint_limits(robot); 0168 0169 %Plot position, velocity and acceleration 0170 %plot_joint_data(robot); 0171 %Now, animate the robot in 3D 0172 animate(robot, q_line); 0173 0174 %Plot an arroy between p0 an p1 0175 vect_arrow(p0,p1,'b') 0176 0177 plot3(pp(1,:),pp(2,:),pp(3,:), 'k', 'LineWidth', 2) 0178 0179