%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MoveC: Make a circular path in space. The circular path is defined by three points: - The current robot position. - Robtarget1, which should be traversed by the robot. - Robtarget2, as the final point. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % MoveC: Make a circular path in space. 0003 % The circular path is defined by three points: 0004 % - The current robot position. 0005 % - Robtarget1, which should be traversed by the robot. 0006 % - Robtarget2, as the final point. 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 MoveC(robtarget1, robtarget2, speeddata, zonedata, gripper, Wobj) 0026 0027 global configuration robot 0028 0029 fprintf('\nCall to MoveC', robot.name); 0030 0031 0032 %Ttool, transformation from the robot's end to the TCP 0033 Ttool = transform_to_homogeneous(gripper(2:end)); 0034 0035 %current position and orientation 0036 T=directkinematic(robot, robot.q); 0037 0038 Ttotal = T*Ttool; 0039 0040 %current position and orientation 0041 p1 = Ttotal(1:3,4); 0042 %orientation 0043 Q1 = T2quaternion(Ttotal); 0044 0045 Ttotal = transform_to_homogeneous(robtarget1); 0046 %middle, through point 0047 p2 = Ttotal(1:3,4); 0048 % and orientation 0049 Q2 = T2quaternion(Ttotal); 0050 0051 %final position 0052 Ttotal = transform_to_homogeneous(robtarget2); 0053 %middle, through point 0054 p3 = Ttotal(1:3,4); 0055 % and orientation 0056 Q3 = T2quaternion(Ttotal); 0057 0058 %fit a circle with three points 0059 [pm, R, v1, v2] = circlefit3d(p1', p2', p3'); 0060 0061 h=figure(configuration.figure.robot); 0062 0063 speed = obtain_linear_speed(robot, speeddata); 0064 distance = 2*pi*R/2; 0065 tmax = distance/speed; 0066 0067 if tmax <= configuration.delta_time 0068 disp('/RAPID/MOVEC: No movement is performed. Making a null movement of 1 second'); 0069 tmax = 2*configuration.delta_time; %avoid errors 0070 distance = 1; 0071 u=[0 0 0]'; 0072 end 0073 0074 0075 %vectors joining the central point, starting and end point 0076 x1 = (p1-pm')/norm(p1-pm'); 0077 x2 = (p2-pm')/norm(p2-pm'); 0078 x3 = (p3-pm')/norm(p3-pm'); 0079 0080 theta_ini=compute_angle(v1, v2, x1); 0081 theta_middle=compute_angle(v1, v2, x2); 0082 theta_end=compute_angle(v1, v2, x3); 0083 0084 0085 delta_theta=speed/R*configuration.delta_time; 0086 0087 0088 theta=[theta_ini:delta_theta:theta_middle theta_middle:delta_theta:theta_end]; 0089 0090 current_conf = compute_configuration(robot, robot.q); 0091 0092 q_circle=[]; 0093 pp=[]; 0094 N=length(theta); 0095 for j=1:N, 0096 0097 pj = pm' + v1'*R*cos(theta(j)) + v2'*R*sin(theta(j)); 0098 0099 pp = [pp pj]; 0100 0101 %interpolate between the initial and final quaternions 0102 %to find an orientation between both 0103 [Qm] = slerp(Q1, Q3, (j-1)/N, 0.01); 0104 0105 %find homogeneous matrix corresponding to pi and Qm 0106 Ttotal = quaternion2T(Qm, pj); 0107 0108 T=Ttotal*inv(Ttool); 0109 0110 %find all the possible solutions to reach T 0111 qinv = inversekinematic(robot, T); 0112 0113 %interpolate configuration 0114 %middle_conf = current_conf*(1-(i-1)/N) + final_conf*((i-1)/N); 0115 %select at each timestep the closes configuration 0116 % q=select_closest_configuration(robot, qinv, middle_conf); 0117 %q=select_closest_configuration(robot, qinv, current_conf); 0118 q=select_closest_joint_coordinates(qinv, robot.q); 0119 0120 %store the joint coordinates in a vector for ulterior animation 0121 q_circle = [q_circle q]; 0122 0123 0124 %the robot performs the movement until the index found. The coordinates, joint speed and acceleratin 0125 %are stored and used in the planning of the next point 0126 robot.q=q; 0127 robot.qd=zeros(robot.DOF,1); 0128 robot.qdd=zeros(robot.DOF,1); 0129 0130 %store all the trajectory for plotting 0131 %the joint trajectories, speeds and acceleration of susequent movements are 0132 %store here 0133 % robot.q_vector=[robot.q_vector q]; 0134 % robot.qd_vector=[robot.qd_vector zeros(robot.DOF,1)]; 0135 % robot.qdd_vector=[robot.qdd_vector zeros(robot.DOF,1)]; 0136 0137 robot.q_vector=[robot.q_vector q]; 0138 robot.qd_vector=[robot.q_vector zeros(robot.DOF,1)]; 0139 robot.qdd_vector=[robot.q_vector zeros(robot.DOF,1)]; 0140 0141 %update current configuration 0142 current_conf = compute_configuration(robot, robot.q); 0143 end 0144 0145 0146 %local time for the planning 0147 t = [0:configuration.delta_time:2*tmax]'; 0148 0149 t=t(1:N); 0150 0151 %a global time for the planning is computed. 0152 %in this way, the total trajectory of different movements can be plotted 0153 if length(robot.time)==0 0154 tend = 0; 0155 else 0156 tend = robot.time(end); 0157 end 0158 t = t + tend; 0159 %store total time 0160 robot.time=[robot.time t']; 0161 0162 %Test whether there are joints outside mechanical limits 0163 test_joint_limits(robot); 0164 0165 %Plot position, velocity and acceleration 0166 %plot_joint_data(robot); 0167 %Now, animate the robot in 3D 0168 animate(robot, q_circle); 0169 0170 %Plot an arroy between pm an p1, p2 and p3 0171 vect_arrow(pm', p1,'r') 0172 vect_arrow(pm', p2,'g') 0173 vect_arrow(pm', p3,'b') 0174 %Plot the trajectory 0175 plot3(pp(1,:),pp(2,:),pp(3,:), 'k', 'LineWidth', 2) 0176 0177 0178 0179 0180 %compute the angle between x and v1 in the reference system formed by v1 0181 %and v2 0182 function theta=compute_angle(v1, v2, x) 0183 %cos 0184 ctheta = dot(v1, x); 0185 stheta = dot(v2, x); 0186 0187 theta = atan2(stheta, ctheta); 0188 0189