potential function motion planner in matlab

main.m
function potential_field_planner(start, goal, obstacles)
    q  = start; %initial configuration
    qg = goal; %final configuration
    k_attractive = 2; %attractive potential hyperparameter
    k_repulsive  = 100; %repulsive potential hyperparameter
    q_stepsize   = 0.1; %stepsize for configuration update
    goal_tol     = 0.1; %tolerance to determine if the goal is reached
    max_iter     = 10000; %maximum number of configurations to try

    num_obstacles = size(obstacles,1); %number of obstacles

    %main loop
    for i=1:max_iter
        %check if the goal is reached
        if norm(q-qg)<=goal_tol
            break;
        end

        %compute attractive potential
        Fatt = k_attractive*(qg-q);

        %compute repulsive potential
        Frep = zeros(2,1);
        for j=1:num_obstacles
            qo = obstacles(j,:)';
            rho = norm(q-qo); %distance to obstacle
            direction = (q-qo)/rho; %normalized direction to obstacle
            if rho > 5 %ignore obstacles too far away
                continue;
            end
            Frep = Frep + k_repulsive*(1/rho - 1/5)*(1/rho^2)*direction;
        end

        %compute total force
        F = Fatt + Frep;

        %update with a step along the direction of the total force
        q = q + q_stepsize * F;

        %check if the new configuration is in collision with the obstacles
        for j=1:num_obstacles
            qo = obstacles(j,:)';
            if norm(q-qo)<=1
                %new configuration is in collision, go back to the previous one
                q = q - q_stepsize * F;
                break;
            end
        end
    end

    if i==max_iter
        disp('Max iterations reached, could not find a plan!');
    else
        disp('Goal reached!');
    end
end
1793 chars
57 lines

main.m
start = [0;0];
goal = [10;10];
obstacles = [2 2; 3 3; 5 5; 7 7];
potential_field_planner(start, goal, obstacles);
114 chars
5 lines