function [vCons,viMax,vMax] = consistent_velocity(numAlpha,w,wmax,R,r,delta,phi,gamma)
% Function that computes the consistent velocity of an omnidirectional
% platform given its constructive parameters and range of angular
% velocities of the platform to be explored.
%
% INPUTS:
%     - numAlpha. [-] Number of elements in the range of angles of the linear
%       velocity of the platform explored.
%     - w. [rad/s] Angular velocity of the platform to be explored.
%     - wmax. [rad/s] Maximum angular velocity of the motors driving the wheels.
%     - R. [m] Vector with the distances between the centroids of the robot and
%       each wheel.
%     - r. [m] Vector with the radius of each wheel.
%     - delta. [º] Vector with the angles defining the angular distribution of
%       the wheels.
%     - phi. [º] Vector with the angles defining the angular orientation of the
%        wheels referred to the xwk axis of each wheel when it is perpendicular to
%       the line joining the centroids of the robot and each wheel.
%     - gamma. [º] Vector with the angles of the rollers referred to the ywk
%        axis of each wheel.
%
% OUTPUTS:
%     - vCons. [m/s] Consistent velocity.
%     - viMax. [m/s] Vector with the maximum velocity in each direction alpha.
%     - vMax. [m/s] Maximum value of the maximum velocities in all directions.
%
% Ways of executing the function:
% [vCons,viMax,vMax] = consistent_velocity(numAlpha,w,wmax,R,r,delta,phi,gamma)
% [vCons,viMax,vMax] = consistent_velocity(numAlpha,w,wmax)
%
% Nomenclature used:
% "wk" refers to the angular velocities of the wheels
% "Mrobot" refers to the motion command (v,aplha,w)
% The suffix "_w0" means that the variable corresponds to a case in which the angular velocity of the robot is w = 0 rad/s
% The suffix "_w" means that the variable corresponds to a case in which the angular velocity of the robot is w NOT 0 rad/s

% Authors: http://robotica.udl.cat

% Inputs control ----------------------------------------------------------

if (nargin < 3) % Control that the velocity parameters are established
    disp('Error: velocity parameters not established')
elseif (nargin < 4) % If the user does not specify the dimensional parameters of the robot, the following dimensions are established
    R = [0.195 0.195 0.195];
    r = [0.148 0.148 0.148];
    delta = [60 180 300];
    phi = [0 0 0];
    gamma = [0 0 0];
else % If the user only specifies one value of R, r, phi or gamma, all the wheels of the platform have the same value for such parameter
    if length(R)== 1
        auxR = ones(1,length(delta));
        R = auxR*R;
    elseif length(R) ~= length(delta)
        disp('Error: R not properly defined.')
    end
    if length(r)== 1
        auxr = ones(1,length(delta));
        r = auxr*r;
    elseif length(r) ~= length(delta)
        disp('Error: r not properly defined.')
    end
    if length(phi)== 1
        auxphi = ones(1,length(delta));
        phi = auxphi*phi;
    elseif length(phi) ~= length(delta)
        disp('Error: phi not properly defined.')
    end
    if length(gamma)== 1
        auxgamma = ones(1,length(delta));
        gamma = auxgamma*gamma;
    elseif length(gamma) ~= length(delta)
        disp('Error: gamma not properly defined.')
    end
end

% Dimensional parameters of the platform ----------------------------------

omr.numWheels = length(delta); % Number of wheels.
omr.R = R; % Radial distances between the centroids of the robot and the wheels.
omr.r = r; % Radii of the wheels.
omr.gamma = gamma; % Angular orientations of the rollers, referred to the ywk axes of the wheels.
omr.delta = delta; % Angular distributions of the wheels, referred to the xb axis of the platform.
omr.phi = phi; % Angular orientations of the wheels, referred to the xwk axes of whe wheels when they are perpendicular to R.
omr.x = omr.R.*cosd(omr.delta);  % x-coordinates of the centroids of the wheels, referred to the center of the platform.
omr.y = omr.R.*sind(omr.delta); % y-coordinates of the centroids of the wheels, referred to the center of the platform.

% Definition of the range of alpha values explored ------------------------

v = 0.3; % [m/s] Seed value of the norm of the linear velocity of the platform.
incrementAlpha = 360/numAlpha; % [º] Increment in the range of angles of the linear velocity of the platform explored.
alphai = 0:incrementAlpha:360; % [º] Range of angles of the linear velocity of the platform explored.

% Computation of the consistent and maximum velocities --------------------

% Matrixes initialization (w = 0 rad/s). Each column will store the angular
% velocity of one wheel for the specific direction alpha of the row.
wkSeed_w0 = zeros(length(alphai),omr.numWheels); % Angular velocities of the wheels required to reach the seed linear velocity of the platform in each direction alpha when w = 0 rad/s.
wkMax_w0 = zeros(length(alphai),omr.numWheels); % Angular velocities of the wheels required to reach the maximum linear velocity of the platform in each direction alpha when w = 0 rad/s.

% Matrixes computation (w = 0 rad/s).
for alphaVal = 1:length(alphai)
    Mrobot_w0 = [v,alphai(alphaVal),0]; % Auxiliary motion command definition with w = 0 rad/s. 
    wkSeed_w0(alphaVal,:) = inverseKinematicsModel(omr,Mrobot_w0); % Angular velocities of the wheels required to reach the seed value v in the direction alphaVal (w=0)
    % Scale up the angular velocities of the wheels to their maximum when w = 0 rad/s.
    wkMax_w0(alphaVal,:) = wkSeed_w0(alphaVal,:)*wmax/max(abs(wkSeed_w0(alphaVal,:))); % Angular velocities of the wheels required to reach the maximum v in the direction alphaVal (w=0)
end

% Matrix initialization (w ~= 0 rad/s). Each column will store the angular
% velocity of one wheel for the specific direction alpha of the row.
wkSeed_w = zeros(length(alphai),omr.numWheels); % Angular velocities of the wheels required to reach the seed linear velocity of the platform in each direction alpha when w is NOT 0 rad/s.

% Matrix computation (w ~= 0 rad/s).
for alphaVal = 1:length(alphai)
    Mrobot_w = [v,alphai(alphaVal),w]; % Motion command definition when w ~= 0 rad/s.
    wkSeed_w(alphaVal,:)  = inverseKinematicsModel(omr,Mrobot_w); % Angular velocities of the wheels required to reach the seed value v in the direction alphaVal (w~=0).
end

% Offset computation: the sign must be maintained.
offseti = wkSeed_w-wkSeed_w0;
[~,pos] = max(max(abs(offseti))); % Position of the maximum absolute value of the offsets.
offset = offseti(1,pos); % The offsetting value corresponding to the position of the maximum absolute value is the offset, with its corresponding sign.

% The offset is used to scale down the maximum angular velocities of the
% wheels so that when the effect of w~=0 is introduced, the maximum angular
% velocity of the motor is not exceeded.
wkScaledDown_w0 = wkMax_w0.*(wmax-abs(offset))/wmax; % Scaled-down angular velocities (w=0).

% Computation of the motion command (v,alpha,w) required to reach the
% angular velocities of the wheels computed. v is the maximum velocity
% velocity in each direction.
% Matrix initialization. Columns will store the v, alpha, w values for the
% specific direction alpha of the row.
MrobotScaledDown_w0 = zeros(length(alphai),3); % Motion command (v,alpha,w): v is the vector with maximum values when w~=0.

for alphaVal = 1:length(alphai)
    MrobotScaledDown_w0(alphaVal,:) = forwardKinematicsModel(omr,wkScaledDown_w0(alphaVal,:)'); % Computation of the motion command (v,alpha,w): v is the maximum velocity in the direction alphaVal when w~=0.
end
viMax = MrobotScaledDown_w0(:,1); % [m/s] Vector with the values of v corresponding to the maximum linear velocity of the robot in each direction alpha given the angular velocity of the robot w~=0.
vMax = max(viMax); % [m/s] maximum value of the maximum linear velocity of the robot given the angular velocity of the robot w~=0.
vCons = min(viMax); % [m/s] consistent velocity of the robot given the angular velocity of the robot w~=0.

% Computation of the maximum angular velocities of the wheels required to
% execute the motion command with the maximum velocity v when w~=0.
% Matrixes initialization (viMax, w~=0). Each column will store the
% velocity of one wheel for the specific direction alpha of the row.
wkMax_w = zeros(length(alphai),omr.numWheels); % Angular velocities.
vdrive = zeros(length(alphai),omr.numWheels); % Linear velocities in the drive direction.
vslide = zeros(length(alphai),omr.numWheels); % Linear velocities in the slide direction.

% Matrixes computation (viMax, w~=0).
for alphaVal = 1:length(alphai)
    MrobotMax_w = [viMax(alphaVal),alphai(alphaVal),w]; % Motion command (v, alpha, w) required to reach the maximum velocity in the alphaVal direction when w~=0.
    [wkMax_w(alphaVal,:),vdrive(alphaVal,:),vslide(alphaVal,:)] = inverseKinematicsModel(omr,MrobotMax_w); % Angular velocities of the wheels required to reach the maximum v,alpha,w (w~=0).
end

% Graphical representation ------------------------------------------------

% Polar figure with the maximum linear velocities in each direction and the consistent velocity when w~=0

figPolar = figure;
p = polar(0*[1 1],[0 2],'w');
p.Color = [0.871 0.871 0.871];
hold on
for k = 1:length(alphai)
    if ~isnan(viMax(k))
        blueLine = quiver(0,0,viMax(k)*cosd(alphai(k)),viMax(k)*sind(alphai(k)),0,'b-');
    end
end
a = 0:0.1:(2*pi+0.1);
b = vCons*ones(size(a));
redLine = polar(a,b,'r-');
figPolar.Position = [360 502 386 420];
legend([blueLine,redLine],{'v_i_=_1_._._N_,_m_a_x','vcons'},'Position',[0.6873 0.0270 0.2876 0.1024]);
title(['vcons (', sprintf('\x03C9'),' = ',num2str(w),' rad/s)'])

% Figures with the evolution of the angular velocities of the wheels as a
% function of the angular orientation alpha

% Angular velocities of the wheels required to reach the consistent
% velocity (w~=0)
wkCons_w = zeros(length(alphai),omr.numWheels);
for alphaVal = 1:length(alphai)
    MrobotCons = [vCons,alphai(alphaVal),w];
    wkCons_w(alphaVal,:) = inverseKinematicsModel(omr,MrobotCons); % Computation of the angular velocities of the wheels required to reach the consistent velocity (w~=0)
end
figwCons = figure;
axwCons = axes;
[~,axwCons] = representAngularVelocities(figwCons,axwCons,alphai,wkCons_w,omr); % Function that represents the evolution of the angular velocities as a function of alpha.
tCons = title(axwCons,['Angular velocities of the wheels required to reach vcons (', sprintf('\x03C9'), ' = ',num2str(w),' rad/s)']);
tCons.Position(2) = 7.5;

% Angular velocities of the wheels required to reach the maximum velocity (w~=0)
figwMax = figure;
axwMax = axes;
[~,axwMax] = representAngularVelocities(figwMax,axwMax,alphai,wkMax_w,omr); % Function that represents the evolution of the angular velocities as a function of alpha.
tMax = title(axwMax,['Angular velocities of the wheels required to reach v_m_a_x (', sprintf('\x03C9'), ' = ',num2str(w),' rad/s)']);
tMax.Position(2) = 7.5;


function [wk, vdrive, vslide,mArank] = inverseKinematicsModel(omr,vrobot)
    % Inverse kinematics model of a platform
    %
    % ---INPUTS-----------------------------------------------------------------
    % omr. Structure of the platform with its dimensional parameters (defined below)
    % vrobot. Vector with the target velocity of the platform, expressed in
    % the platform base: vrobot = [v alpha w], [m/s, º, rad/s].
    %
    % ---OUTPUTS----------------------------------------------------------------
    % wk. [rad/s] Vector with the angular velocities of the wheels: wk = [w1 w2 w3 w4 ...]'.
    % vdrive. [m/s] Vector with the linear velocity of the wheels in the drive direction: vdrive = [vdrive 1, vdrive 2, vdrive 3, vdrive 4 ...]'.
    % vslide. [m/s] Vector with the linear velocity of the wheels in the slide direction: vslide = [vslide 1, vslide 2, vslide 3, vslide 4...]'.
    % mArank. [-] % Rank of the matrix used to compute the angular velocities of the wheels from the linear velocity v of the platform in the platform frame.
    %
    % ---Frames of the system: --------------------------------------------------
    % {s} = World frame (static). Axes: xs, ys, zs.
    % {b} = Platform frame (body-attached). Translated (xb,yb) and rotated an angle theta with respect to the world frame {s}. Axes: xb, yb, zb.
    % {w} = Frame of one wheel. Translated (xi, yi) and rotated an angle phi with respect to the perpendicular of the line joining the centroids of the platform and the wheel. Axes: xw, yw, zw.
    % ---Directions of motion:--------------------------------------------------
    % Driving direction = wheel plane (coincident with the xw axis of the wheel).
    % Free sliding direction = perpendicular to the axis of the rollers, direction in which the rollers rotate freely.
    % ---General parameters-----------------------------------------------------
    % theta = [º] rotation angle of the platform frame {b}, referred to the world frame {s}.
    % phi = [º] rotation angle of the wheel frame {w}, referred to the xw axis of a "reference wheel" whose hub is collinear with the centerline joining the centroids of the robot and the wheel.
    % gamma [º] angle of the sliding direction, referred to the yw axis of the wheel frame {w}.
    % delta = [º] angle of the wheel distribution, referred to the xb axis of the platform frame {b}.
    % R = [m] radial distance between the centroids of the robot and the wheel.
    % r = [m] radius of the wheel.
    % wk = [rad/s] angular velocity of the wheels.
    % vb = [m/s] norm of the linear velocity of the platform expressed in the platform frame {b}.
    % alpha = [º] angle of the linear velocity of the platform expressed in the platform frame {b}.
    % w = [rad/s] angular velocity of the robot.


    % vrobot = [v alpha w]. [m/s, º, rad/s]
    % ---Decomposition of the robot velocity ----------------------------------
    vx_b = vrobot(1)*cosd(vrobot(2));
    vy_b = vrobot(1)*sind(vrobot(2)); 
    vel_b = [vx_b; vy_b; vrobot(3)]; % Vector with the linear and angular velocity of the platform in the platform frame {b}.
    
    % ---Matrix generation ----------------------------------------------------
    m = zeros(omr.numWheels,2); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute the angular velocities of the wheels.
    mdrive = zeros(omr.numWheels,2); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute vdrive.
    mslide = zeros(omr.numWheels,2); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute vslide.
    mA = zeros(omr.numWheels,3); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute the angular velocities of the wheels.
    mdA = zeros(omr.numWheels,3); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the robot frame to compute vdrive.
    msA = zeros(omr.numWheels,3); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute vslide.
    
    for wheelID = 1:omr.numWheels 
        % The matrixes are generated row by row
        
        A_bwT = [1 0 -omr.y(wheelID); 0 1 omr.x(wheelID)]; % Translation matrix from the platform frame {b} to the wheel frame {w}.
        A_bwR = [cosd(omr.delta(wheelID)+90+omr.phi(wheelID)) sind(omr.delta(wheelID)+90+omr.phi(wheelID)); -sind(omr.delta(wheelID)+90+omr.phi(wheelID)) cosd(omr.delta(wheelID)+90+omr.phi(wheelID))]; % Rotation matrix from the robot frame robot {b} to the wheel frame {w}.
        A_bw = A_bwR*A_bwT; % Change-of-basis matrix from the platform frame {b} to the wheel frame {w}.
        
        m(wheelID,:) = [1/omr.r(wheelID) tand(omr.gamma(wheelID))/omr.r(wheelID)]; % Matrix to compute the angular velocities of the wheels from the linear velocity v of the platform in the wheel frame.
        mA(wheelID,:) = m(wheelID,:)*A_bw; % Matrix to compute the angular velocities of the wheels from the linear velocity v of the platform in the platform frame.
        
        mdrive(wheelID,:) = [1 tand(omr.gamma(wheelID))]; % Matrix to compute vdrive from the linear velocity v of the platform in the wheel frame.
        mdA(wheelID,:) = mdrive(wheelID,:)*A_bw; % Matrix to compute vdrive from the linear velocity v of the platform in the platform frame.
        
        mslide(wheelID,:) = [0 1/cosd(omr.gamma(wheelID))]; % Matrix to compute vslide from the linear velocity v of the platform in the wheel frame.
        msA(wheelID,:) = mslide(wheelID,:)*A_bw; % Matrix to compute vslide from the linear velocity v of the platform in the robot frame.
    end
    
    mArank = rank(mA); % Rank of the matrix.
    
    wk = mA*vel_b; % Angular velocities of the wheels [rad/s].
    
    vdrive = mdA*vel_b; % Linear velocity in the drive direction [m/s].
    vslide = msA*vel_b; % Linear velocity in the slide direction [m/s].
end

function vrobot = forwardKinematicsModel(omr, wk)
    % Forward kinematics model of a platform
    %
    % ---INPUTS-----------------------------------------------------------------
    % wk. [rad/s] Vector with the angular velocities of the wheels: wk = [w1 w2 w3 w4 ...]'.
    % omr. Structure of the platform with its dimensional parameters (defined below).
    %
    % ---OUTPUTS----------------------------------------------------------------
    % vrobot. Vector with the target velocity of the platform, expressed in
    % the platform base: vrobot = [v alpha w], [m/s, º, rad/s].
    %
    % ---Frames of the system: --------------------------------------------------
    % {s} = World frame (static). Axes: xs, ys, zs.
    % {b} = Platform frame (body-attached). Translated (xb,yb) and rotated an angle theta with respect to the world frame {s}. Axes: xb, yb, zb.
    % {w} = Frame of one wheel. Translated (xi, yi) and rotated an angle phi with respect to the perpendicular of the line joining the centroids of the platform and the wheel. Axes: xw, yw, zw.
    % ---Directions of motion:--------------------------------------------------
    % Driving direction = wheel plane (coincident with the xw axis of the wheel).
    % Free sliding direction = perpendicular to the axis of the rollers, direction in which the rollers rotate freely.
    % ---General parameters-----------------------------------------------------
    % theta = [º] rotation angle of the platform frame {b}, referred to the world frame {s}.
    % phi = [º] rotation angle of the wheel frame {w}, referred to the xw axis of a "reference wheel" whose hub is collinear with the centerline joining the centroids of the robot and the wheel.
    % gamma [º] angle of the sliding direction, referred to the yw axis of the wheel frame {w}.
    % delta = [º] angle of the wheel distribution, referred to the xb axis of the platform frame {b}.
    % R = [m] radial distance between the centroids of the robot and the wheel.
    % r = [m] radius of the wheel.
    % wk = [rad/s] angular velocity of the wheels.
    % vb = [m/s] norm of the linear velocity of the platform expressed in the platform frame {b}.
    % alpha = [º] angle of the linear velocity of the platform expressed in the platform frame {b}.
    % w = [rad/s] angular velocity of the robot.
    
    m = zeros(omr.numWheels,2); % Matrix with the elements that will be multiplied with the velocity of the platform expressed in the wheel frame to compute the angular velocities of the wheels.
    mA = zeros(omr.numWheels,3);  % Matrix to compute the angular velocities of the wheels from the linear velocity v of the platform in the platform frame. 
    
    for jj = 1:omr.numWheels 
        % The matrixes are generated row by row
        
        A_bwT = [1 0 -omr.y(jj); 0 1 omr.x(jj)]; % Translation matrix from the platform frame {b} to the wheel frame {w}.
        A_bwR = [cosd(omr.delta(jj)+90+omr.phi(jj)) sind(omr.delta(jj)+90+omr.phi(jj)); -sind(omr.delta(jj)+90+omr.phi(jj)) cosd(omr.delta(jj)+90+omr.phi(jj))]; % Rotation matrix from the robot frame robot {b} to the wheel frame {w}.
        A_bw = A_bwR*A_bwT; % Change-of-basis matrix from the platform frame {b} to the wheel frame {w}.

        m(jj,:) = [1/omr.r(jj) tand(omr.gamma(jj))/omr.r(jj)]; % Matrix to compute the angular velocities of the wheels from the linear velocity v of the platform in the wheel frame.
        mA(jj,:) = m(jj,:)*A_bw; % Matrix to compute the angular velocities of the wheels from the linear velocity v of the platform in the platform frame.
    end
        
    invmA = pinv(mA); % The inverse of matrix mA is computed as the pseudoinverse in case the matrix mA is not full rank.
    vel_b = invmA*wk; % Computation of the velocity of the robot decomposed in the platform frame: vx, vy, w [m/s; m/s; rad/s].
    
    % Computation of v, alpha.
    vx_b = vel_b(1);
    vy_b = vel_b(2);
    vb = sqrt(vx_b^2+vy_b^2); % Norm of the target linear velocity of the platform [m/s].
    % Since alpha is computed from vx and vy, if one of these values is
    % close to but not zero (for instance 1e-15), the atan is not zero. Hereafter
    % a control of such cases is performed.
    if abs(vx_b)<1e-5 && abs(vy_b)<1e-5
        alpha_out = 0;
    else
        alpha_out = atan2d(vy_b,vx_b); % Angle of the target linear velocity of the platform [º].
        alpha_out = mod(alpha_out,360);
    end
    w_out = vel_b(3); % Target angular velocity of the platform [rad/s].

    vrobot = [vb,alpha_out,w_out];% Vector with the target velocity of the robot: v,alpha,w [m/s º rad/s].

end

function [figName,axName] = representAngularVelocities(figName,axName,alphai,wki,omr)
    % Function that graphically represents the evolution of the angular
    % velocities of the wheels as a function of the direction alpha.
    %
    % INPUTS
    % figName: identifier of the figure created outside the function.
    % axName: axes of the figure.
    % alphai: range of alpha values explored.
    % wki: matrix with the angular velocities of the wheels in each
    % direction alpha. Each column will store the angular velocity of one
    % wheel for the specific direction alpha of the row.
    % omr: Structure of the platform with its dimensional parameters.
    %
    % OUTPUTS
    % figName: identifier of the figure with the representation.
    % axName: axes of the figure.

    markersList = {'o','^', 'square','o', '^', 'square', 'o'};
    LineColor = {[0.5 0.5 0.5],[0.7 0.7 0.7], [0.9 0.9 0.9], 'k', 'm', 'c','y'};
    markerColor = {[0.5 0.5 0.5],[0.7 0.7 0.7], [0.9 0.9 0.9], 'k', 'm', 'c','y'};
    legendObjs = zeros(1,omr.numWheels);
    legendTexts = cell(1,omr.numWheels);
    omegaStr = '\x03C9';
    alfaStr = '\x03B1';

    hold on
    lw = 1; % Line width of the angular velocities represented
    for wheelID = 1: omr.numWheels
        legendObjs(wheelID) = plot(axName,alphai,wki(:,wheelID),markersList{wheelID},'LineStyle','-','Color',LineColor{wheelID},'LineWidth',lw,'MarkerSize',8,'MarkerEdgeColor','k','MarkerFaceColor',markerColor{wheelID});
        % Creation of the texts that will be in the legend
        legendTexts{wheelID} = [sprintf(omegaStr),'_',num2str(wheelID)];
    end
    xlabel(axName,[sprintf(alfaStr),' of the motion (º)']);
    ylabel(axName,[sprintf(omegaStr),'_k (rad/s)']);

    margin_x = 5;
    axName.XLim = [alphai(1)-margin_x alphai(end)+margin_x];
    axName.YLim = [-10 10];

    if alphai(1) == 0
        axName.XTick = [0 45 90 135 180 225 270 315 360];
    elseif alphai(1) == -180
        axName.XTick = [-180 -135 -90 -45 0 45 90 135 180];
    end

    axName.Position = [0.0583 0.1700 0.9217 0.8000];

    figName.Position = [231 602 909 276];

    axName.Box = 'on';
    grid(axName,'on');

    legend(axName, legendObjs, legendTexts,'Location','southeast','Orientation','horizontal');
end

end