clone

Create deep clone of controllerTEB object

Since R2023a

Syntax

controller2 = clone(controller1)

Description

controller2 = clone(controller1) creates a deep clone of the controllerTEB object controller1.

Examples

Compute Velocity Commands and Optimal Trajectory for Differential-Drive Robot Using Timed Elastic Band Algorithm

Set Up Parking Lot Environment

Create an occupancyMap object from a parking lot map and set the map resolution to 3 cells per meter.

load parkingMap.mat;
resolution = 3;
map = occupancyMap(map,resolution);

Visualize the map. The map contains the floor plan of a parking lot with some parking slots already occupied.

show(map)
title("Parking Lot Map")
hold on

Set Up and Run Global Planner

Create a validatorOccupancyMap state validator using the stateSpaceSE2 definition. Specify the map and the distance for interpolating and validating path segments.

validator = validatorOccupancyMap(stateSpaceSE2,Map=map);
validator.ValidationDistance = 0.1;

Create an RRT* path planner. Increase the maximum connection distance.

rrtstar = plannerRRTStar(validator.StateSpace,validator);
rrtstar.MaxConnectionDistance = 0.2;

Set the start and goal states.

start = [2 9 0];
goal = [27 18 -pi/2];

Plan a path with default settings.

rng(42,"twister") % Set random number generator seed for repeatable result.
route = plan(rrtstar,start,goal);
refpath = route.States;

RRT* uses a random orientation, which can cause unnecessary turns.

headingToNextPose = headingFromXY(refpath(:,1:2));

Align the orientation to the path, except for at the start and goal states.

refpath(2:end-1,3) = headingToNextPose(2:end-1);

Visualize the path.

plot(refpath(:,1),refpath(:,2),"r-",LineWidth=2)
hold off

Set Up and Run Local Planner

Create a local occupancyMap object with a width and height of 15 meters and the same resolution as the global map.

localmap = occupancyMap(15,15,map.Resolution);

Create a controllerTEB object by using the reference path generated by the global planner and the local map.

teb = controllerTEB(refpath,localmap);

Specify the properties of the controllerTEB object.

teb.LookAheadTime = 10;         % sec
teb.ObstacleSafetyMargin = 0.4; % meters

% To generate time-optimal trajectories, specify a larger weight value,
% like 100, for the cost function, Time. To follow the reference path
% closely, keep the weight to a smaller value like 1e-3.
teb.CostWeights.Time = 100;

Create a deep clone of the controllerTEB object.

teb2 = clone(teb);

Initialize parameters.

curpose = refpath(1,:);
curvel = [0 0];
simtime = 0;
% Reducing timestep can lead to more accurate path tracking.
timestep = 0.1;
itr = 0;
goalReached = false;

Compute velocity commands and optimal trajectory.

while ~goalReached && simtime < 200
    % Update map to keep robot in the center of the map. Also update the
    % map with new information from the global map or sensor measurements.
    moveMapBy = curpose(1:2) - localmap.XLocalLimits(end)/2;
    localmap.move(moveMapBy,FillValue=0.5)
    syncWith(localmap,map)

    if mod(itr,10) == 0 % every 1 sec
        % Generate new vel commands with teb
        [velcmds,tstamps,curpath,info] = step(teb,curpose,curvel);
        goalReached = info.HasReachedGoal;
        feasibleDriveDuration = tstamps(info.LastFeasibleIdx);
        % If robot is far from goal and only less than third of trajectory
        % is feasible, then an option is to re-plan the path to follow to
        % reach the goal.
        if info.ExitFlag == 1 && ...
                feasibleDriveDuration < (teb.LookAheadTime/3)
            route = plan(rrtstar,curpose,[27 18 -pi/2]);
            refpath = route.States;
            headingToNextPose = headingFromXY(refpath(:,1:2));
            refpath(2:end-1,3) = headingToNextPose(2:end-1);
            teb.ReferencePath = refpath;
        end
        timestamps = tstamps + simtime;

        % Show the updated information input to or output
        % from controllerTEB
        clf
        show(localmap)
        hold on
        plot(refpath(:,1),refpath(:,2),".-",Color="#EDB120", ...
             DisplayName="Reference Path")
        quiver(curpath(:,1),curpath(:,2), ...
               cos(curpath(:,3)),sin(curpath(:,3)), ...
               0.2,Color="#A2142F",DisplayName="Current Path")
        quiver(curpose(:,1),curpose(:,2), ...
               cos(curpose(:,3)),sin(curpose(:,3)), ...
               0.5,"o",MarkerSize=20,ShowArrowHead="off", ...
               Color="#0072BD",DisplayName="Start Pose")
    end

    simtime = simtime+timestep;
    % Compute the instantaneous velocity to be sent to the robot from the
    % series of timestamped commands generated by controllerTEB
    velcmd = velocityCommand(velcmds,timestamps,simtime);
    % Very basic robot model, should be replaced by simulator.
    statedot = [velcmd(1)*cos(curpose(3)) ...
                velcmd(1)*sin(curpose(3)) ...
                velcmd(2)];
    curpose = curpose + statedot*timestep;

    if exist("hndl","var")
        delete(hndl)
    end
    hndl = quiver(curpose(:,1),curpose(:,2), ...
                  cos(curpose(:,3)),sin(curpose(:,3)), ...
                  0.5,"o",MarkerSize=20,ShowArrowHead="off", ...
                  Color="#D95319",DisplayName="Current Robot Pose");
    itr = itr + 1;
    drawnow
end
legend

Input Arguments

controller1 — TEB controller

controllerTEB object

TEB controller, specified as a controllerTEB object.

Output Arguments

controller2 — Clone of TEB controller

controllerTEB object

Clone of TEB controller, returned as a controllerTEB object.

Version History

Introduced in R2023a