Setting the robot dimensions for the RRT* algorithm
6 次查看(过去 30 天)
显示 更早的评论
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
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1351144/image.png)
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
0 个评论
回答(2 个)
Ashu
2023-5-19
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.
0 个评论
Ajay Pattassery
2023-11-29
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.
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Motion Planning 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!