Documentation

# cart2frenet

Convert Cartesian states to Frenet states

## Syntax

``cart2frenet(planner,cartesianStates)``

## Description

example

````cart2frenet(planner,cartesianStates)` converts a 6-element vector of Cartesian states ```[x, y, theta, kappa, speed, acceleration]``` to a 6-element vector of Frenet states ```[s, ds, dds, l, dl, ddl]```, where s is arc length from the first point in reference path, and l is normal distance from the closest point at s on the reference path.```

## Examples

collapse all

This example shows how to plan an optimal trajectory using a `trajectoryOptimalFrenet` object.

Create and Assign Map to State Validator

Create a state validator object for collision checking.

`stateValidator = validatorOccupancyMap;`

Create an obstacle grid map.

```grid = zeros(700,700); grid(296:305,150:175) = 1; grid(286:295,300:325) = 1; grid(306:315,300:325) = 1; grid(296:305,450:475) = 1; grid(286:295,600:625) = 1; grid(306:315,600:625) = 1;```

Create a `binaryOccupancyMap` with the grid map.

`map = binaryOccupancyMap(grid);`

Assign the map to the state validator.

`stateValidator.Map = map;`

Plan and Visualize Trajectory

Create a reference path for the planner to follow.

`refPath = [10,400;700,400];`

Declare the cost function handle to prioritize left lane changes.

`leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);`

Initialize the planner object with the reference path, the state validator, and the custom cost function.

`plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);`

Assign lateral deviation values.

`plannerObj.TerminalStates.Lateral = -10:10:10;`

Trajectory Generation

Initialize the variables for the replanning loop.

```previousDeviationValue = 0; deviationValue = 0; initState = zeros(1,6);```

Replan until final waypoint is closest to the terminal state.

```for j = 1:150 % Deviation from the current reference path if initState(4) > 5 deviationValue = 10; elseif initState(4) < -5 deviationValue = -10; end % Move reference path to the current lane if previousDeviationValue ~= deviationValue % Shift the waypoints by the deviation value plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue]; % Shift the terminal states such that they remain fixed in world frame plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue; % Store the deviation value previousDeviationValue = deviationValue; % Update initState variable with the new planner object initState = cart2frenet(plannerObj,trajectory(10,1:6)); end % Generate a trajectory from initState trajectory = plan(plannerObj,initState); % Use 10th state of the current trajectory as a starting point for % replanning initState = cart2frenet(plannerObj,trajectory(10,1:6)); % Visualize every 5th iteration if mod(j,5)==0 show(map) hold on; show(plannerObj,'TrajectoryColor','none'); drawnow end end```

## Input Arguments

collapse all

Optimal trajectory planner in frenet space, specified as a `trajectoryOptimalFrenet` object.

Vector of Cartesian states, specified as a 1-by-6 vector, ```[x, y, theta, kappa, speed, acceleration]```. x and y specifies the position in meters, theta specifies the orientation angle in radians, kappa specifies the curvature in `m-1`, speed specifies the velocity in `m/s`, and acceleration specify the acceleration in `m/s2`.

Example: `[110 110 pi/4 0 0 0]`

Data Types: `double`