Main Content

step

Compute velocity commands and optimal trajectory for subsequent time steps

Since R2023a

collapse all in page

    Syntax

    [velcmds,timestamps,optPath] = step(controller,curState,curVel)
    [___,extraInfo] = step(___)

    Description

    [velcmds,timestamps,optPath] = step(controller,curState,curVel) computes the linear and angular velocity commands velcmds, with their corresponding timestamps and corresponding optimized path optPath, for the specified current pose curState and current velocity curVel of a robot.

    example

    [___,extraInfo] = step(___) returns extra information, extraInfo, to evaluate the solution, in addition to all arguments from the previous syntax.

    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.

    Current pose of the robot, specified as a three-element vector of the form [x y theta]. x and y specify the robot position in meters. theta specifies the robot orientation in radians.

    Data Types: single | double

    Current velocity of the robot, specified as a two-element vector of the form [v w]. v specifies the linear velocity of the robot in meters per second. w specifies the angular velocity of the robot in radians per second.

    Data Types: single | double

    Output Arguments

    collapse all

    Velocity commands, returned as an N-by-2 matrix. The first column is the linear velocity in meters per second, and the second column is the angular velocity in radians per second.

    Data Types: double

    Timestamps corresponding to velocity commands, returned as an N-element column vector.

    Data Types: double

    Optimized path, returned as an N-by-3 matrix. Each row is of the form [x y theta], which defines the xy-position and orientation angle theta at a point in the path.

    N is affected by the ReferenceDeltaTime and LookAheadTime properties of controller. The algorithm tries to keep the difference between any two consecutive timestamps close to ReferenceDeltaTime. If the gap between a pair of consecutive timestamps is greater than ReferenceDeltaTime, the function adds poses and timestamps to the path. If the gap is less than ReferenceDeltaTime, the function removes poses and timestamps from the path. In addition, the algorithm tries to keep the final value of timestamps close to LookAheadTime, so increasing LookAheadTime increases N as well.

    Data Types: double

    Extra information, returned as a structure. The fields of the structure are:

    FieldDescription
    LastFeasibleIdx

    Index specifying an element in the optimized path and timestamp outputs until which the trajectory is feasible. Beyond this index, the value of ExitFlag will be greater than zero. This implies one or more scenarios corresponding to ExitFlag > 0 has occurred.

    DistanceFromStartPose

    Distance of each pose in optPath from the first pose in optPath. The value of curState is always the first pose in optPath.

    HasReachedGoal

    Indicates whether the robot has successfully reached the last pose in the ReferencePath within a tolerance, and returns as true if successful. Otherwise, this value returns false.

    TrajectoryCost

    Cost of optimized trajectory for cost functions in the Timed Elastic Band algorithm.

    ExitFlag

    Scalar value indicating the exit condition of the step function.

    • 0 — Indicates that the step function returned feasible velocity commands and trajectory.

    • 1 — Indicates that the trajectory output returned by the step function is collision free only until the index value specified using LastFeasibleIdx.

    • 2 — Indicates that the poses corresponding to indices after the LastFeasibleIdx violates the obstacle safety margin constraint of the TEB controller by more than 10%.

    • 3 — Indicates that the turning radius of the vehicle while moving from a pose at LastFeasibleIdx to a subsequent pose violates the minimum turning radius constraint by more than 10%.

    • 4 — Indicates that one or more time stamps returned by the step function are older than the previous time stamp.

    • 5 — Indicates that the robot will not reach the reference path within a specified look-ahead time even if it travels at a maximum linear velocity from its current position.

    Data Types: struct

    Version History

    Introduced in R2023a

    expand all

    See Also

    Objects

    Functions