%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MoveAbsJ: Moves robot to a particular joint coordinates Example: MoveAbsJ(robot, joint_coord, 'v1000', 'z100', gripper, wobj0); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % MoveAbsJ: 0003 % Moves robot to a particular joint coordinates 0004 % Example: MoveAbsJ(robot, joint_coord, 'v1000', 'z100', gripper, wobj0); 0005 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0006 0007 % Copyright (C) 2012, by Arturo Gil Aparicio 0008 % 0009 % This file is part of ARTE (A Robotics Toolbox for Education). 0010 % 0011 % ARTE is free software: you can redistribute it and/or modify 0012 % it under the terms of the GNU Lesser General Public License as published by 0013 % the Free Software Foundation, either version 3 of the License, or 0014 % (at your option) any later version. 0015 % 0016 % ARTE is distributed in the hope that it will be useful, 0017 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0018 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0019 % GNU Lesser General Public License for more details. 0020 % 0021 % You should have received a copy of the GNU Leser General Public License 0022 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0023 function MoveAbsJ(joint_coord, speeddata, zonedata, gripper, Wobj) 0024 0025 global configuration robot 0026 0027 fprintf('\nCall to MoveAbsJ'); 0028 0029 %obtain current joint coordinates 0030 q_current=robot.q; 0031 0032 %obtain target joint coordinates 0033 q_final= joint_coord(1:robot.DOF)'; 0034 0035 %test joint limits for the final values 0036 test_joints(robot, q_final); 0037 0038 speed = obtain_joint_speed(robot, speeddata); 0039 %when fine is specified, the radius is zero, otherwise, 0040 %the radius represents a sphere around the target where the 0041 %movement is changed to the next target 0042 radius = obtain_zone_data(zonedata); 0043 0044 %find time to perform movement 0045 [actual_speed, tmax]=synchronize(q_current, q_final, speed, robot.accelmax); 0046 0047 %in this case, if the total time for the movement is lower than the 0048 %configuration.delta_time time then 10 points are interpolated 0049 if tmax <= configuration.delta_time 0050 t = [0:tmax/10:tmax]'; 0051 else 0052 %local time for the planning, normal case 0053 t = [0:configuration.delta_time:tmax]'; 0054 end 0055 0056 if radius==0 0057 %compute a smooth trajectory in q and qd 0058 %in this case, the final speed is zero 0059 [q, qd, qdd] = compute_joint_trajectory_indep(robot, q_current, q_final, robot.qd, zeros(1,robot.DOF), t); 0060 else 0061 %in this case, the final speed is selected as the speed selected by the 0062 %variable speeddata 0063 [q, qd, qdd] = compute_joint_trajectory_indep(robot, q_current, q_final, robot.qd, actual_speed, t); 0064 end 0065 0066 T=directkinematic(robot, q_final); 0067 %finds the first joint coordinates that are inside a radius r of the target 0068 %point 0069 index = find_first_in_zone_data(robot, q, T, radius); 0070 0071 %the robot performs the movement until the index found. The coordinates, joint speed and acceleratin 0072 %are stored and used in the planning of the next point 0073 robot.q=q(:,index); 0074 robot.qd=qd(:,index); 0075 robot.qdd=qdd(:,index); 0076 0077 %store all the trajectory for plotting 0078 %the joint trajectories, speeds and acceleration of susequent movements are 0079 %store here 0080 robot.q_vector=[q(:, 1:index)]; 0081 robot.qd_vector=[qd(:, 1:index)]; 0082 robot.qdd_vector=[qdd(:, 1:index)]; 0083 0084 0085 %a global time for the planning is computed. 0086 %in this way, the total trajectory of different movements can be plotted 0087 if length(robot.time)==0 0088 tend = 0; 0089 else 0090 tend = robot.time(end); 0091 end 0092 t = t + tend; 0093 %store total time 0094 %robot.time=[robot.time t(1:index)']; 0095 robot.time=[t(1:index)']; 0096 0097 0098 %Test whether there are joints outside mechanical limits 0099 test_joint_limits(robot); 0100 0101 %Plot position, velocity and acceleration 0102 %plot_joint_data(robot); 0103 %Now, animate the robot in 3D 0104 animate(robot, q); 0105 0106 %Plot the trajectory 0107 pp = zeros(3,size(q,2)); 0108 for i=1:size(q,2), 0109 T=directkinematic(robot, q(:,i)); 0110 pp(1:3,i)=T(1:3,4); 0111 end 0112 plot3(pp(1,:),pp(2,:),pp(3,:), 'k', 'LineWidth', 2) 0113 0114 0115 0116 0117 0118 0119 0120 0121