2
$\begingroup$

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

Aim is to go from <span class=$x_0$ to $x_f$ within this toroid region">

$\endgroup$
  • 4
    $\begingroup$ 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. $\endgroup$ – High Performance Mark Dec 20 '18 at 16:26
  • $\begingroup$ 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? $\endgroup$ – spektr Dec 20 '18 at 21:01
  • $\begingroup$ @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. $\endgroup$ – Talha Yousuf Dec 21 '18 at 15:00

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.