i want plan a path using A* algorithm,Below is my code and when im running it im gettinh the folowing eroor.How can i correct this?

clear

clc

start_0 = [1, 1, 1];

goal_0 = [50, 50, 50];

C_0= zeros(50,50,50);

A = Astar3D(start_0,goal_0,C_0)

function [path, cost] = Astar3D(start, goal, C)

% Astar3D: A* algorithm for 3D configuration space

% Inputs:

% start: 1x3 vector representing start point (x,y,z)

% goal: 1x3 vector representing goal point (x,y,z)

% C: x*y*z matrix representing configuration space, where 0 is free space and 1 is an obstacle

% Outputs:

% path: Nx3 matrix representing path from start to goal

% cost: cost of the path

% Define heuristic function

heuristic = @(x,y,z) sqrt((x-goal(1))^2 + (y-goal(2))^2 + (z-goal(3))^2);

% Initialize start node

startNode = struct('pos', start, 'f', 0, 'g', 0, 'h', 0, 'parent', []);

% Initialize open and closed sets

openSet = startNode;

closedSet = [];

% Loop until goal is found or open set is empty

while ~isempty(openSet)

% Find node with lowest f score

[~, idx] = min([openSet.f]);

currentNode = openSet(idx);

% If goal is reached, construct path and return

if isequal(currentNode.pos, goal)

path = reconstructPath(currentNode);

cost = currentNode.f;

return;

end

% Move current node to closed set

openSet(idx) = [];

closedSet(end+1) = currentNode;

% Generate neighbors of current node

neighbors = generateNeighbors(currentNode.pos, C);

% Loop through neighbors

for i = 1:size(neighbors,1)

neighbor = neighbors(i,:);

% If neighbor is in closed set, skip to next neighbor

if any(ismember([closedSet.pos], neighbor, 'rows'))

continue;

end

% Calculate g score (distance from start to neighbor)

tentativeG = currentNode.g + norm(currentNode.pos-neighbor);

% If neighbor is not in open set, add it

neighborNode = struct('pos', neighbor, 'f', 0, 'g', 0, 'h', 0, 'parent', []);

neighborNode.parent = currentNode;

if ~any(ismember([openSet.pos], neighbor, 'rows'))

neighborNode.h = heuristic(neighbor(1), neighbor(2), neighbor(3));

neighborNode.g = tentativeG;

neighborNode.f = neighborNode.g + neighborNode.h;

openSet(end+1) = neighborNode;

else

% If neighbor is in open set, update its g score if tentative g score is better

idx = find(ismember([openSet.pos], neighbor, 'rows'));

if tentativeG < openSet(idx).g

openSet(idx).g = tentativeG;

openSet(idx).f = openSet(idx).g + openSet(idx).h;

openSet(idx).parent = currentNode;

end

end

end

end

% If open set is empty and goal is not found, return empty path and cost

path = [];

cost = [];

end

function path = reconstructPath(node)

% Reconstruct path from start to given node

path = [node.pos];

while ~isempty(node.parent)

node = node.parent;

path = [node.pos;path];

end

end

function neighbors = generateNeighbors(pos, C)

% Generate neighbors of given position in 3D configuration space

x = pos(1);

y = pos(2);

z = pos(3);

neighbors = [];

for i = -1:1

for j = -1:1

for k = -1:1

if i == 0 && j == 0 && k == 0

continue;

end

if x+i < 1 || x+i > size(C,1) || y+j < 1 || y+j > size(C,2) || z+k < 1 || z+k > size(C,3)

continue;

end

if C(x+i,y+j,z+k)

continue;

end

neighbors(end+1,:) = [x+i, y+j, z+k];

end

end

end

end

error:

Conversion to double from struct is not possible.

Error in untitled5>Astar3D (line 44)

closedSet(end+1) = currentNode;

Error in untitled5 (line 7)

A = Astar3D(start_0,goal_0,C_0)

Answer 1

[Walter Roberson]

closedSet = [];

That is a double(). You cannot assign a struct to that by indexing.

Use

closedSet = startNode(false) ;

which will create a struct with no entries.