I want to create a rrt planner using custom kinematic constraints in a 3d map ,how to proceed with this problem?

i have 3d occupancy grid map and now how can i define my custom kinematic constraints and apply rrt to this problem?

Answer 1

[Sandip Kumar]

This example showcases how to use custom validator with stateSpaceSE2 object inside plannerRRT: https://www.mathworks.com/help/nav/ug/moving-furniture-in-a-cluttered-room-with-rrt.html

This should give you inspiration of how to do it.

ValidatorOccupancyMap3D uses the occupancyMap3D: https://www.mathworks.com/help/nav/ref/validatoroccupancymap3d.html --> there is an example on this page which shows how to use plannerRRT

Read more about create custom state spaces here: https://www.mathworks.com/help/nav/ref/nav.statespace-class.html

Read about creating custom validator here: https://www.mathworks.com/help/nav/ref/nav.statevalidator-class.html

Other motion planning example: https://www.mathworks.com/help/nav/examples.html?category=motion-planning&s_tid=CRUX_topnav (note only some of them use plannerRRT)

[mukund shah]

but these all dont use kinematic constraints ,i dont need custom state space for my problem SE3 is sufficient but i need to add kinematic constraints to it,so that rrt gives path that follows those constraints

[Karsh Tharyani]

Hi Mukund,

I think you have kinematic constraints associated with your state space which is derived from SE3. You might want to consider overriding the implementation of interpolate. I have a sample skeleton of what such a class (called ConstrainedSE3) might look like.

classdef ConstrainedSE3 < stateSpaceSE3

methods

function obj=ConstrainedSE3()

obj@stateSpaceSE3();

end

function interpolatedStates=interpolate(obj,s1,s2,ratios)

interpolatedStates=interpolate@stateSpaceSE3(s1,s2,ratios);

obj.constrainState(interpolatedStates);

end

end

methods(Access=private)

function constrainedStates=constrainState(obj,states)

%Replace below with your constrain implementation

constrainedStates=states;

end

end

end

Your implementation of constrainState should be used to kinematically constrain a state transition which is carried out by interpolate. You might also want to override distance in a similar fashion if the basic measure of stateSpaceSE3 is not the true measure of distance in your constrained state space.

We would also be glad to hear from you on your specific use case so that we can enhance this in a future release of MATLAB. For the same, please reach out to Technical Support https://www.mathworks.com/support/contact_us.html

Hope this helps!

Best,

Karsh

[mukund shah]

Can i somehow make it into a control problem which kinematically constrains the output?

[Karsh Tharyani]

plannerRRT is a geometric planner and since the state doesn't have any notion of time it is hard to encode it in the planner. Have you looked at plannerControlRRT?

[mukund shah]

Yes but i dont know to edit the state propogator, I want to make a state propogator in Se3 space for a 3d bicycle model, I have the rotation and translation matrix but I am not getting how to edit nav. State propogator for this task