gusucode.com > Mobile Robotics Simulation Toolbox > src/kinematics/FourWheelMecanum.m
classdef FourWheelMecanum < handle % FOURWHEELMECANUM Utilities for four-wheeled vehicle with individually % driven Mecanum wheels (also known as Swedish or Ilon wheels). % The ordering of the wheels is Front Left, Front Right, Rear Left, % Rear Right. % Source: http://research.ijcaonline.org/volume113/number3/pxc3901586.pdf % % For more information, see <a href="matlab:edit mrsDocFourWheelMecanum">the documentation page</a> % % Copyright 2018 The MathWorks, Inc. properties wheelRadius = 0.1; % Wheel radius [m] wheelBase = 0.4; % Wheelbase [m] wheelTrack = 0.5; % Wheel track [m] 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 = FourWheelMecanum(wheelRadius,wheelBase,wheelTrack) % FOURWHEELMECANUM Construct an instance of this class % Unpack parameters obj.wheelRadius = wheelRadius; obj.wheelBase = wheelBase; obj.wheelTrack = wheelTrack; % 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 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 forward 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 R = (obj.wheelBase + obj.wheelTrack)/2; % Create forward kinematics matrix (wheel speeds to body speeds) obj.fwdMatrix = [ 1, 1, 1, 1; ... -1, 1, 1,-1; ... -1/R, 1/R, -1/R, 1/R ] * (obj.wheelRadius/4); % Create inverse kinematics matrix (body speeds to wheel speeds) obj.invMatrix = [ 1, -1, -R; ... 1, 1, R; ... 1, 1, -R; ... 1, -1, R ] / obj.wheelRadius; end end end