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