gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/mrsDiffDrivePurePursuit.m
%% EXAMPLE: Differential drive vehicle following waypoints using the % Pure Pursuit algorithm (robotics.PurePursuit) % % Copyright 2018 The MathWorks, Inc. %% Define Vehicle R = 0.1; % Wheel radius [m] L = 0.5; % Wheelbase [m] dd = DifferentialDrive(R,L); %% Simulation parameters sampleTime = 0.05; % Sample time [s] tVec = 0:sampleTime:15; % Time array initPose = [0;0;0]; % Initial pose (x y theta) pose = zeros(3,numel(tVec)); % Pose matrix pose(:,1) = initPose; % Define waypoints waypoints = [0,0; 2,2; 4,2; 2,4; 0.5,3]; % Create visualizer viz = Visualizer2D; viz.hasWaypoints = true; %% Pure Pursuit Controller controller = robotics.PurePursuit; controller.Waypoints = waypoints; controller.LookaheadDistance = 0.35; controller.DesiredLinearVelocity = 0.75; controller.MaxAngularVelocity = 1.5; %% Simulation loop close all r = robotics.Rate(1/sampleTime); for idx = 2:numel(tVec) % Run the Pure Pursuit controller and convert output to wheel speeds [vRef,wRef] = controller(pose(:,idx-1)); [wL,wR] = inverseKinematics(dd,vRef,wRef); % Compute the velocities [v,w] = forwardKinematics(dd,wL,wR); velB = [v;0;w]; % Body velocities [vx;vy;w] vel = bodyToWorld(velB,pose(:,idx-1)); % Convert from body to world % Perform forward discrete integration step pose(:,idx) = pose(:,idx-1) + vel*sampleTime; % Update visualization viz(pose(:,idx),waypoints) waitfor(r); end