I am getting a 3d path using rrt in Se3 statespace ,i ant to smoothen this path how can i do this?

I am getting a path in 3d map using rrt ,how can i smoothen it,by giving certain radius of curvature etc such that there are no sharp turns?Can we do spline interpolation?

code:

planner = plannerRRT(space,statevalidator);

planner.MaxConnectionDistance = 5;

planner.MaxIterations = 1000;

planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5);

planner.GoalBias = 0.1;

start = [40 180 25 0.7 0.2 0 0.1];

goal = [96 140 144 0.3 0 0.1 0.6];

[pthObj,solnInfo] = plan(planner,start,goal);

pthObj.States

isValid = isStateValid(statevalidator,pthObj.States)

isPathValid = zeros(size(pthObj.States,1)-1,1,'logical');

for i = 1:size(pthObj.States,1)-1

[isPathValid(i),~] = isMotionValid(statevalidator,pthObj.States(i,:),...

pthObj.States(i+1,:));

end

isPathValid

show(omap)

hold on

scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state

scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled') % draw goal state

plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),...

'r-','LineWidth',2) % draw path

Answer 1

[Jianxin Sun]

https://www.mathworks.com/help/uav/ug/motion-planning-with-rrt-for-fixed-wing-uav.html may help you.