Setting the robot dimensions for the RRT* algorithm

Hey

I use the RRT* algorithm to find a path for my robot.

I transfer the resulting route to an external simulation program where my robot is located.

My question is how to define the dimensions of the robot in a simple way?

The route obtained from the RRT, as you can see in the picture below, is very close to the obstacles. A situation is created in which the robot overcomes the obstacles

I would appreciate your help, thank you very much!

This is my code :

% Generate a random 2-D maze map.

map = mapMaze(5,MapSize=[25 25],MapResolution=1);

mapData = occupancyMatrix(map);

% Define start and goal poses.

startPose = [2.1712 1.75 pi/2];

goalPose = [22 20 0];

%Plan the path for the specified start pose, and goal pose, and map.

[path,treeData] = codegenRRTStar1(mapData,startPose,goalPose);

%Visualize the computed path.

show(occupancyMap(mapData))

hold on

% Start state

scatter(startPose(1,1),startPose(1,2),"g","filled")

% Goal state

scatter(goalPose(1,1),goalPose(1,2),"r","filled")

% Tree expansion

plot(treeData(:,1),treeData(:,2),"c.-")

% Path

plot(path(:,1),path(:,2),"r-",LineWidth=2)

legend("Start Pose","Goal Pose","Tree expansion","MATLAB Generated Path")

legend(Location='northwest')

% -------------- MATLAB - BLENDER CONNECTION :-----------------

% MATLAB script to send data to Blender server

% Set up a TCP/IP socket

host = 'localhost';

port = 12345;

tcp_client = tcpclient(host, port);

for k=1:length(path)

% Create example data

example_data = path(k,:); % Send 3 variables to blender- x,y,theta

% Convert the example data to JSON format

json_data = struct('custom_properties', {example_data});

json_string = jsonencode(json_data);

% Send the JSON-formatted data to Blender

write(tcp_client, json_string);

pause(0.1)

end

% Close the socket

clear tcp_client;

% -------------- MATLAB - BLENDER CONNECTION :----------------

function [path,treeData] = codegenRRTStar1(mapData,startPose,goalPose)

%#codegen

% Create an occupancy map.

omap = occupancyMap(mapData);

% Create a state space object.

stateSpace = stateSpaceSE2;

% Update state space bounds to be the same as map limits.

stateSpace.StateBounds = [omap.XWorldLimits;omap.YWorldLimits;[-pi pi]];

% Construct a state validator object using the statespace and map

% object.

stateValidator = validatorOccupancyMap(stateSpace,Map=omap);

% Set the validation distance for the validator.

stateValidator.ValidationDistance = 0.01;

% Create RRT* path planner and allow further optimization after goal is

% reached. Reduce the maximum iterations and increase the maximum

% connection distance.

ContinueAfterGoalReached=true;

MaxIterations=4000;

MaxConnectionDistance=1;

planner = plannerRRTStar(stateSpace,stateValidator);

% Compute a path for the given start and goal poses.

[pathObj,solnInfo] = plan(planner,startPose,goalPose);

% Extract the path poses from the path object.

path = pathObj.States;

% Extract the tree expansion data from the solution information

% structure.

treeData = solnInfo.TreeData;

end

Answer 1

[Ashu]

Hi Yovel,

I understand that you want you define dimensions of your robot so that it doesn't cross the obstacles.

To ensure that the robot does not cross the obstacles you can use the function 'isMotionValid' in the 'validatorOccupancyMap'.

[isValid,lastValid] = isMotionValid(validator,state1,state2)

Please refer to the following document to learn more about 'validatorOccupancyMap' and how to use 'isMotionValid'

https://www.mathworks.com/help/nav/ref/validatoroccupancymap.html

You can also introduce 'inflationCollisionChecker' to maintain distance from the walls.

map.CollisionChecker = inflationCollisionChecker("InflationRadius",1);

Please refer to the following document to learn about 'inflationCollisionChecker'

https://www.mathworks.com/help/nav/ref/validatoroccupancymap.ismotionvalid.html

Hope it helps.

Answer 2

[Ajay Pattassery]

A very simple approach would be to infate the map.

omap = inflate(map,radius);

A radius of 1, will make sure the robot keeps 1m away from the obstacles.

Note: This assumes that the robot is of circular shape with radius r.