# How to find two points within defined region in this constrained optimization problem?

I am doing a project related to robotics where I am using fmincon function from matlab to minimize the distance between the points x_0 and x_f. Basically task is to make manipulator move from x_0 to x_f such that it never goes out of torus region. So I think it is a constrained optimization problem. I am having difficulty in figuring out what is the strategy to find the two intermediate points (as shown in figure below) so that given x_0 and x_f, it gives us two points [x1,y1,z1] and [x2,y2,z2] to use as via points.

Can you please figure out how should I write the objective function. Code so far is below:

    function [x, fval, history] = solver( x0, xn, R, r )

% x0 = initial position, xn = final position, R = distance from the center of the tube to the center of the torus
% r is the radius of the tube.
% plotting torus
th=linspace(0,2*pi,60); % e.g. 60 partitions along perimeter of the tube
phi=linspace(0,pi,60); % e.g. 60 partitions along azimuth of torus
[Phi,Th]=meshgrid(phi,th);
x=0+(R+r.*cos(Th)).*cos(Phi);
y=0+(R+r.*cos(Th)).*sin(Phi);
z=0+r.*sin(Th);
s=surface(x,y,z);
s.EdgeColor='none'
axis equal;
daspect([1 1 1 ])
hold off
alpha(s,0.2);
% defining constraints and objective function
history = [];
options = optimset('OutputFcn', @myoutput);
[x, fval] = fmincon(@(x)objfun(x,xn),x0,[],[],[],[],[],[],@noncol,options);

function stop = myoutput(x,optimvalues,state)
stop = false;
if isequal(state,'iter')
history = [history; x];
end
end

function z = objfun(x,p)
z = sqrt((p(1)-x(1))^2+(p(2)-x(2))^2+(p(3)-x(3))^2);
end
function [y, yeq] = noncol( x ) % x ==== [ x(1)   x(2)    x(3) ]
y = (sqrt(x(1)^2+x(2)^2)-R)^2 + x(3)^2-r^2;
yeq =[];
end
end $x_0$ to $$x_f$$ within this toroid region">

• I won't say you can't solve it that way, but do you need to ? If it's a simple torus (circular cross sections) aren't you just looking for a tangent from xf to the circle defined by the inner radius of the torus (which you can flatten into an annulus for calculations), then an arc around the circle to the tangent point of the line from xo ? I expect I'm missing something blindingly obvious. – High Performance Mark Dec 20 '18 at 16:26
• Are you assuming a linear path from any point to another? Your figure looks like you’re allowing curved surfaces? If you’re allowing curved surfaces, why do the two intermediate points matter? – spektr Dec 20 '18 at 21:01
• @HighPerformanceMark The torus is actually a task space for robotic arm. x0 and xf can be any where within torus. So for close points, it might go out of this constrained region or take longer path. – Talha Yousuf Dec 21 '18 at 15:00