gusucode.com > Mobile Robotics Simulation Toolbox > src/kinematics/GenericOmniwheel.m

    classdef GenericOmniwheel < handle
    % GENERICOMNIWHEEL Utilities for omniwheel vehicles with generic
    % number, position, and orientation of wheels.
    %
    % For more information, see <a href="matlab:edit mrsDocGenericOmniwheel">the documentation page</a>
    %
    % Copyright 2018 The MathWorks, Inc.
    
    properties
        wheelRadius = 0.1;           % Wheel radius [m]
        wheelPositions = [1 1;1 -1]; % Wheel (X,Y) positions relative to CG [m]
        wheelAngles = [0;0];         % Wheel angles relative to CG [rad]
    end
    properties(Access=private)
        fwdMatrix = zeros(3,4); % Transformation matrix from wheel speeds to body speeds
        invMatrix = zeros(4,3); % Transformation matrix from body speeds to wheel speeds
    end
    
    methods
        
        function obj = GenericOmniwheel(wheelRadius,wheelPositions,wheelAngles)
            % GENERICOMNIWHEEL Construct an instance of this class
            
            % Unpack parameters
            obj.wheelRadius = wheelRadius;
            obj.wheelPositions = wheelPositions;
            obj.wheelAngles = wheelAngles;
            
            % Create forward and inverse matrices
            computeMatrices(obj);
            
        end
        
        function bodySpeeds = forwardKinematics(obj,wheelSpeeds)
            % Calculates linear and angular velocities [vx;vy;w],
            % in the *body* frame, from wheel speeds [w1;w2;w3;w4]
            bodySpeeds = obj.fwdMatrix * wheelSpeeds;
        end
        
        function wheelSpeeds = inverseKinematics(obj,bodySpeeds)
            % Calculates wheel speeds [w1;w2;w3;w4] from linear and 
            % angular velocities [vx;vy;w] in the *body* frame
            % NOTE: This uses the pseudoinverse so it may not be accurate
            wheelSpeeds = obj.invMatrix * bodySpeeds;
        end
              
        function M = getForwardMatrix(obj)
            % Returns forward kinematics matrix (wheel speeds to body speeds)
            M = obj.fwdMatrix;
        end
        
        function M = getInverseMatrix(obj)
            % Returns inverse kinematics matrix (wheel speeds to body speeds)
            M = obj.invMatrix;
        end
        
        function computeMatrices(obj)
            % Creates the forward and inverse matrices given the object's
            % kinematic properties
            
            % Initialize the forward kinematics matrix
            numWheels = size(obj.wheelPositions,1);
            M = zeros(3,numWheels);
            
            % If the wheel angles property is scalar, replicate it to all
            % the wheels on the vehicle.
            if isscalar(obj.wheelAngles) && numWheels > 1
                alpha = repmat(obj.wheelAngles,[numWheels 1]);
            else
                alpha = obj.wheelAngles; 
            end
            
            % Loop through each wheel and calculate the forward dynamics
            for idx = 1:numWheels
               
                % Linear components
                M(1,idx) = cos(alpha(idx)); % X direction
                M(2,idx) = sin(alpha(idx)); % Y direction
                
                % Angular component
                % If the line is close to vertical, slope is infinite so
                % this is a special case of the calculation that only
                % requires the X distance
                if abs( abs(alpha(idx)) - pi/2 ) < 1e-3
                    M(3,idx) = obj.wheelPositions(idx,1);
                % Else, do the full calculation with line equation
                % (slope + intercept)
                else
                    slope = tan(alpha(idx));
                    intercept = obj.wheelPositions(idx,2) - slope*obj.wheelPositions(idx,1);
                    M(3,idx) = abs(intercept)/sqrt(1+slope^2);
                end
                
            end
            
            % Multiply by wheel radius to convert from angular to linear 
            % speed, and average over the number of wheels
            obj.fwdMatrix = M * obj.wheelRadius / numWheels;
            
            % Use pseudoinverse to *approximate* the inverse kinematics matrix
            obj.invMatrix = pinv(obj.fwdMatrix);
            
        end
        
    end
end