I want to modify how inbuilt function plannerRRT takes nearest neighbours with some kinematic constraints

This is the code of the function in plannerRRT in matlab ,i want to modify this function such that it chooses nearest neighbours which satisfy a kinematic constraint,

function [newNodeId, statusCode, treeInternal] = extend(obj, randState, treeInternal)
%extend The RRT "Extend" routine
statusCode = obj.InProgress;
newNodeId = nan;
idx = treeInternal.nearestNeighbor(randState);
nnState = treeInternal.getNodeState(idx);

d = obj.StateSpace.distance(randState, nnState);
newState = randState;

here is the function now how do i change this as its using property of treeinternal as nearest neighbors?

Answer 1

[Cameron Stabile]

Hi Mukund,

The distance and connection method between states in plannerRRT is controlled by the nav.StateSpace that it accepts. If the planner is given a stateSpaceSE2 object, then states are connected via linear interpolation, whereas a planner constructed with stateSpaceDubins object connects two states with a Dubins path.

In both cases, the distance method of the state-space will return the length of the segment connecting the two states, and the connection-mechanism inside the interpolate method determines the shape of the segment, and is also responsible for evaluating the segment at provided lengths.

One important assumption with these Geometric Planners is that the state-space must be able to connect any two states given to it. This means the segment connecting two states should inherently enforce any kinematic constraints you have defined (similar to how the Dubins/ReedsShepp curves enforce a MinTurningRadius constraint).

If you don't mind me asking, what kind of constraints are you trying to enforce? Do you have a method for connecting states analytically which implicitly honors your kinematic constraint, akin to the Dubins example above?

Thanks in advance,

Cameron

[mukund shah]

@Cameron Stabile what i want is that in SE3 statespace only i get a trajectory which follows kinematics of a steerable needle i.e trajectory should not be greater than a min turning radius ,also the curves should be smooth,how can i achieve this? what should i change? Here is my current code which i am using, i want to modify for above purposes now

space = stateSpaceSE3([1 256;

1 256;

1 256;

inf inf;

inf inf;

inf inf;

inf inf]);

omap = map3D;

statevalidator = validatorOccupancyMap3D(space);

statevalidator.Map = omap;

statevalidator.ValidationDistance = 1;

planner = plannerRRT(space,statevalidator);

planner.MaxConnectionDistance = 10;

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),...

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

[Cameron Stabile]

Thank you for the clarification Mukund.

Do you by any chance have access to the UAV Toolbox? The feature you are requesting sounds quite similar to an example-helper they created in the Motion Planning with RRT for Fixed-Wing UAV example.

That state-space uses the uavDubinsConnection under the hood, which is effectively a 3D version of the dubinsConnection found in NAV. It should produce C1 continuous curves and allow you to provide kinematic constraints in the form of MinTurnRadius, MaxRollAngle, and FlightPathAngleLimit.

My suggestion would be to try out that helper file and see if it suits your needs, and perhaps build off that example if you have additional requirements that aren't being met.

I have also submitted an enhancement request to make this an official feature. The development team will review it alongside other such requests and may choose to expose this in a future release.

Best,

Cameron