Setting the robot dimensions for the RRT* algorithm

13 views (last 30 days)
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

Answers (2)

Ashu
Ashu on 19 May 2023
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'
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'
Hope it helps.

Ajay Pattassery
Ajay Pattassery on 29 Nov 2023
A very simple approach would be to infate the map.
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.

Products


Release

R2022a

Community Treasure Hunt

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

Start Hunting!