// key:e159ad4c7fb8ed9f786ac4d9548af4b2 // Potential field navigation for a mobile robot // based on ideas from m-files of Remy Blank // Copyright 1999-2000, Yves Piguet. // 23.12.1999-18.1.2000 // Demonstration for ISR (EPFL) only // All positions are complex, to simplify and speed up everything variable pGoal // [p, k] variable circles // [centers, radii, R0, k] variable segments // [p1, p2, R0, k] variable pos0 // initial position of the robot variable dispForceField // true if the force field is displayed // handlers init [pGoal, circles, segments, pos0, dispForceField] = init menu "Display Force Field" _checkmark(dispForceField) dispForceField = toggle(dispForceField) figure "Potential Field (demonstration for ISR-DMT-EPFL)" draw drawPotField(pGoal, circles, segments, pos0, dispForceField) mousedrag [pGoal, circles, segments, pos0, _msg] = dragPot(pGoal, circles, segments, pos0, _id, _nb, _ix, _z1) mouseover [_cursor, _msg] = overPot(pGoal, circles, segments, _id, _nb, _z) functions {@ function [pGoal, circles, segments, pos0, dispForceField] = init pGoal = [650+500j, 3e-5]; circles = [270+140j, 32, 300, 1e4]; segments = [300+450j, 450+370j, 200, 1e4]; pos0 = 20 + 20j; dispForceField = true; subplots('Potential Field (demonstration for ISR-DMT-EPFL)'); function b = toggle(b) b = ~b; function n = setTo(n) % empty % --- graphics function drawPotField(pGoal, circles, segments, pos0, dispForceField) scale('equal', [0, 700, 0, 600]); sc = scale; n = 80; range = (0:n) / n; z = repmat(sc(1) + (sc(2) - sc(1)) * range, length(range), 1) ... + 1j * repmat(sc(3) + (sc(4) - sc(3)) * range', 1, length(range)); [u1, f1] = goalPot(z, pGoal(1), pGoal(2)); [u2, f2] = circlePot(z, circles(:,1), circles(:,2), circles(:,3), circles(:,4)); [u3, f3] = segmentPot(z, segments(:,1), segments(:,2), segments(:,3), segments(:,4)); p = min(u1 + u2 + u3, 1e3); contour(p, sc, exp((0:10)/10*log(100)), 'b'); if dispForceField f = f1 + f2 + f3; f = f(1:2:end, 1:2:end); % discard 3/4 of arrows z = z(1:2:end, 1:2:end); maxf = 0.9 * min(abs(z(1,1) - z(1,2)), abs(z(1,1) - z(2,1))); f = maxf * sign(f); vec = [z(:), z(:)+f(:)]; plot(real(vec), imag(vec), 'm'); end plot(real(pGoal(1)), imag(pGoal(1)), 'xr', 1); plot(real(circles(:,1)), imag(circles(:,1)), '.r', 2); circle(real(circles(:,1)), imag(circles(:,1)), circles(:,2), 'rf', 3); circle(real(circles(:,1)), imag(circles(:,1)), circles(:,2) / 10, 'wf'); circle(real(circles(:,1)), imag(circles(:,1)), circles(:,3) + circles(:,2), 'y', 4); plot(real(segments(:,1:2)), imag(segments(:,1:2)), 'r', 5); circle(real(segments(:,1)), imag(segments(:,1)), segments(:,3), 'y', 6); circle(real(segments(:,2)), imag(segments(:,2)), segments(:,3), 'y', 7); plot(real(pos0), imag(pos0), '[', 10); traj = calcTraject(pos0, pGoal, circles, segments, 5, 10, 1000); plot(real(traj), imag(traj), 'g'); function [pGoal, circles, segments, pos0, msg] = dragPot(pGoal, circles, segments, pos0, id, nb, ix, z) if isempty(id) cancel; end msg = ''; switch id case 1 pGoal(1) = z; case 2 circles(nb, 1) = z; case 3 circles(nb, 2) = abs(z - circles(nb, 1)); case 4 circles(nb, 3) = abs(z - circles(nb, 1)) - circles(nb, 2); case 5 segments(nb, ix) = z; case 6 segments(nb, 3) = abs(z - segments(nb, 1)); case 7 segments(nb, 3) = abs(z - segments(nb, 2)); case 10 pos0 = z; end function [cursor, msg] = overPot(pGoal, circles, segments, id, nb, z) cursor = ~isempty(id); p = goalPot(z, pGoal(1), pGoal(2)) ... + circlePot(z, circles(:,1), circles(:,2), circles(:,3), circles(:,4)); msg = sprintf('Potential: %g', p); if ~isempty(id) switch id case 1 msg = sprintf('Goal (weight: %k)', pGoal(2)); case [2,3] msg = sprintf('Obstacle (weight: %k)', circles(nb, 4)); case 5 msg = sprintf('Obstacle (weight: %k)', segments(nb, 4)); case [4,6,7] msg = 'Obstacle visibility'; case 10 msg = 'Starting point'; end end % --- calculate potential and force for elementary obstacles and goals function [u, f] = goalPot(z, p, k) u = 0.5 * k * abs(z - p).^2; f = k * (p - z); function [u, f] = circlePot(z, p, r, r0, k) d = abs(z - p) - r; u = inf * ones(size(z)); f = zeros(size(z)); sel = find(d > 0 & d <= r0); u(sel) = 0.5 * k * (1./d(sel) - 1/r0).^2; f(sel) = k * (1./d(sel) - 1/r0) ./ d(sel).^2 .* sign(z(sel) - p); sel = find(d >= r0); u(sel) = 0; function [u, f] = segmentPot(z, p1, p2, r0, k) u = inf * ones(size(z)); f = zeros(size(z)); tmp = (z - p1) / (p2 - p1); sel = real(tmp) < 0; % side of p1 d = abs(z - p1); sel1 = find(sel & d > 0 & d <= r0); u(sel1) = 0.5 * k * (1./d(sel1) - 1/r0).^2; f(sel1) = k * (1./d(sel1) - 1/r0) ./ d(sel1).^2 .* sign(z(sel1) - p1); sel1 = find(sel & d >= r0); u(sel1) = 0; sel = real(tmp) > 1; % side of p2 d = abs(z - p2); sel1 = find(sel & d > 0 & d <= r0); u(sel1) = 0.5 * k * (1./d(sel1) - 1/r0).^2; f(sel1) = k * (1./d(sel1) - 1/r0) ./ d(sel1).^2 .* sign(z(sel1) - p2); sel1 = find(sel & d >= r0); u(sel1) = 0; sel = real(tmp) >= 0 & real(tmp) <= 1; % between p1 and p2 d = abs(imag(tmp)) * abs(p2 - p1); sel1 = find(sel & d > 0 & d <= r0); u(sel1) = 0.5 * k * (1./d(sel1) - 1/r0).^2; f(sel1) = -k * (1./d(sel1) - 1/r0) ./ d(sel1).^2 .* sign(imag(tmp(sel1)) * (1j * (p1 - p2))); sel1 = find(sel & d >= r0); u(sel1) = 0; % --- trajectory integration function traj = calcTraject(z0, pGoal, circles, segments, radius, prec, maxlength) % calculate the trajectory from z0 to goal with the specified radius % in the "force" field of pGoal, circles, segments (momentum is ignored) % each step has a length of prec; stop after maxlength traj = z0; while sum(abs(diff(traj))) < maxlength & abs(traj(end) - pGoal(1)) > radius [u1, f1] = goalPot(traj(end), pGoal(1), pGoal(2)); [u2, f2] = circlePot(traj(end), circles(:,1), circles(:,2), circles(:,3), circles(:,4)); [u3, f3] = segmentPot(traj(end), segments(:,1), segments(:,2), segments(:,3), segments(:,4)); f = f1 + f2 + f3; traj(end + 1) = traj(end) + prec * sign(f); end @}