Path following with optimalTrajectoryFrenet

1 view (last 30 days)
Hello
how could I add an actor (with a local coordinate system) to follow a planned trajectory from frenet states? I only see the first planned trajectories in the simulation and would like the algorithm to replan the trajectory (follow the specified path) after execution by updating the initState values and repeating the process. Thank you in advance!
Code:
validator = validatorOccupancyMap;
grid = zeros(1000,1000);
map = binaryOccupancyMap(grid);
setOccupancy(map, [170 180], 1);
setOccupancy(map, [169.6 156.2], 1);
validator.Map = map;
refPath = [0 ,0; 400 400];
initState = [150 110 pi/4 0 10 0];
xy = grid2local(map, [initState(1) initState(2)]);
planner = trajectoryOptimalFrenet(refPath, validator, 'Time',1,'Deviation',0,'LateralSmoothness',3,'LongitudinalSmoothness',5);
initFrenetState = cart2frenet(planner, initState);
trajectory = plan(planner, initFrenetState);
show(planner,"Trajectory","all","TrajectoryColor","velocity")

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!