clone

Create deep clone of controllerTEB object

Since R2023a

collapse all in page

    Syntax

    controller2 = clone(controller1)

    Description

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

    example

    Examples

    collapse all

    Open Live Script

    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

    Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains an object of type image.

    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

    Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains 2 objects of type image, line.

    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

    Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, quiver. These objects represent Reference Path, Current Path, Start Pose, Current Robot Pose.

    Input Arguments

    collapse all

    TEB controller, specified as a controllerTEB object.

    Output Arguments

    collapse all

    Clone of TEB controller, returned as a controllerTEB object.

    Version History

    Introduced in R2023a

    See Also

    Objects

    Functions