I am doing a project related to robotics where I am using
fmincon function from matlab to minimize the distance between the points
x_f. Basically task is to make manipulator move from
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_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