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

3 views (last 30 days)
mukund shah
mukund shah on 11 Dec 2021
Answered: Jianxin Sun on 20 May 2022
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

Answers (1)

Jianxin Sun
Jianxin Sun on 20 May 2022

Tags

Products


Release

R2021b

Community Treasure Hunt

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

Start Hunting!