synchronize

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    [velo2, tmax]= SINCHRONIZE(qini, qfinal, velocity) Finds a mean speed and the required 
   time to perform a movement between the joint coordinates qini and qfinal. 
   If the speed of each joint is different, the maximum time to perform the movement
   by the slower joint is taken as a basis.
   
   Inputs:
        Qini: initial position in joint coordinates.
        Qfinal: final position in joint coordinates.
        Velocity: stores the maximum velocity of each joint.
   Outputs:
        velo2: new maximum speed for each joint.
        tmax: time needed to perform the movement.

    See also: MOVEJ, COMPUTE_JOINT_TRAJECTORY_INDEP

   Author: Arturo Gil
   Date:   29/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %
0003 %    [velo2, tmax]= SINCHRONIZE(qini, qfinal, velocity) Finds a mean speed and the required
0004 %   time to perform a movement between the joint coordinates qini and qfinal.
0005 %   If the speed of each joint is different, the maximum time to perform the movement
0006 %   by the slower joint is taken as a basis.
0007 %
0008 %   Inputs:
0009 %        Qini: initial position in joint coordinates.
0010 %        Qfinal: final position in joint coordinates.
0011 %        Velocity: stores the maximum velocity of each joint.
0012 %   Outputs:
0013 %        velo2: new maximum speed for each joint.
0014 %        tmax: time needed to perform the movement.
0015 %
0016 %    See also: MOVEJ, COMPUTE_JOINT_TRAJECTORY_INDEP
0017 %
0018 %   Author: Arturo Gil
0019 %   Date:   29/03/2012
0020 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0021 
0022 % Copyright (C) 2012, by Arturo Gil Aparicio
0023 %
0024 % This file is part of ARTE (A Robotics Toolbox for Education).
0025 %
0026 % ARTE is free software: you can redistribute it and/or modify
0027 % it under the terms of the GNU Lesser General Public License as published by
0028 % the Free Software Foundation, either version 3 of the License, or
0029 % (at your option) any later version.
0030 %
0031 % ARTE is distributed in the hope that it will be useful,
0032 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0033 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0034 % GNU Lesser General Public License for more details.
0035 %
0036 % You should have received a copy of the GNU Leser General Public License
0037 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0038 function [actual_speed, maxtime]=synchronize(qini, qfinal, speed, accel)
0039 
0040 tacel = speed./accel;
0041 
0042 tcte = (abs(qfinal(:)-qini(:))-accel(:).*tacel.^2)./speed(:);
0043 
0044 time_total = tcte + 2*tacel;
0045 
0046 maxtime=max(time_total);
0047 
0048 actual_speed = (qfinal(:)-qini(:)-accel(:).*tacel.^2)/maxtime(:);