Good evening,
I am currently working on a parallelipiped driving simulator with 3-DoF (heave, roll and pitch) that has 4 actuators on the corners, so I cannot use inverse kinematic to obtain the length of the four actuators (z1 is the rear right actuator, z2 front right and so on anti clockwise). So I decided to use Maple to write the kinematic solution (the varibles with subscript g) and the constraints, and to use a constrained minimization to find the length of actuators. However, if I use a single target to try the optimization(theta_t, phi_t, z_t), Simulink does not signal any error while using a trajectory of more points, Simulink gives me the following error "Error calling ‘Opt/mycon’. This call-site expects more outputs than this function can supply. P-code function ‘checkNonlinearInputs.p’ produced an error.". I leave the code of the MATLAB function block below, which receives as input the two angles and the heave and has as output the four actuators length. How can I solve this problem to obtain a workink real-time optimization? Thanks in advance.

function [z1, z2, z3, z4] = Opt(theta_t, phi_t, z_t)
l = 0.82;
L = 1.6;
% theta_t = 1.5*pi/180;
% phi_t = 1.5*pi/180;
% z_t = 0.05;
%theta_g = atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
%phi_g = -atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l));
% x_g = ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
% y_g = ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
% z_g = L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
fun = @(z)120*(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – theta_t)^2 + 40*(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – phi_t)^2 …
+ 1*(L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) +1*(l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))) – z_t)^2;
lb = [0, 0, 0, 0];
ub = [0.1, 0.1, 0.1, 0.1];
if theta_t < 0.4*pi/180
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-17, ‘StepTolerance’, 1e-10);
else
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-18, ‘StepTolerance’, 1e-10);
end
function [c,ceq] = mycon(z)
l = 0.82;
L = 1.6;
c = [atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – 2.5*pi/180;
-(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) – 2.5*pi/180];
ceq = [-sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(2) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(3) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(4) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
];
end
Z = fmincon(fun, [0.05, 0.05, 0.05, 0.05], [], [], [], [], lb, ub , @mycon, options);
z1 = Z(1);
z2 = Z(2);
z3 = Z(3);
z4 = Z(4);
endGood evening,
I am currently working on a parallelipiped driving simulator with 3-DoF (heave, roll and pitch) that has 4 actuators on the corners, so I cannot use inverse kinematic to obtain the length of the four actuators (z1 is the rear right actuator, z2 front right and so on anti clockwise). So I decided to use Maple to write the kinematic solution (the varibles with subscript g) and the constraints, and to use a constrained minimization to find the length of actuators. However, if I use a single target to try the optimization(theta_t, phi_t, z_t), Simulink does not signal any error while using a trajectory of more points, Simulink gives me the following error "Error calling ‘Opt/mycon’. This call-site expects more outputs than this function can supply. P-code function ‘checkNonlinearInputs.p’ produced an error.". I leave the code of the MATLAB function block below, which receives as input the two angles and the heave and has as output the four actuators length. How can I solve this problem to obtain a workink real-time optimization? Thanks in advance.

function [z1, z2, z3, z4] = Opt(theta_t, phi_t, z_t)
l = 0.82;
L = 1.6;
% theta_t = 1.5*pi/180;
% phi_t = 1.5*pi/180;
% z_t = 0.05;
%theta_g = atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
%phi_g = -atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l));
% x_g = ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
% y_g = ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
% z_g = L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
fun = @(z)120*(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – theta_t)^2 + 40*(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – phi_t)^2 …
+ 1*(L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) +1*(l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))) – z_t)^2;
lb = [0, 0, 0, 0];
ub = [0.1, 0.1, 0.1, 0.1];
if theta_t < 0.4*pi/180
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-17, ‘StepTolerance’, 1e-10);
else
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-18, ‘StepTolerance’, 1e-10);
end
function [c,ceq] = mycon(z)
l = 0.82;
L = 1.6;
c = [atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – 2.5*pi/180;
-(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) – 2.5*pi/180];
ceq = [-sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(2) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(3) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(4) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
];
end
Z = fmincon(fun, [0.05, 0.05, 0.05, 0.05], [], [], [], [], lb, ub , @mycon, options);
z1 = Z(1);
z2 = Z(2);
z3 = Z(3);
z4 = Z(4);
end Good evening,
I am currently working on a parallelipiped driving simulator with 3-DoF (heave, roll and pitch) that has 4 actuators on the corners, so I cannot use inverse kinematic to obtain the length of the four actuators (z1 is the rear right actuator, z2 front right and so on anti clockwise). So I decided to use Maple to write the kinematic solution (the varibles with subscript g) and the constraints, and to use a constrained minimization to find the length of actuators. However, if I use a single target to try the optimization(theta_t, phi_t, z_t), Simulink does not signal any error while using a trajectory of more points, Simulink gives me the following error "Error calling ‘Opt/mycon’. This call-site expects more outputs than this function can supply. P-code function ‘checkNonlinearInputs.p’ produced an error.". I leave the code of the MATLAB function block below, which receives as input the two angles and the heave and has as output the four actuators length. How can I solve this problem to obtain a workink real-time optimization? Thanks in advance.

function [z1, z2, z3, z4] = Opt(theta_t, phi_t, z_t)
l = 0.82;
L = 1.6;
% theta_t = 1.5*pi/180;
% phi_t = 1.5*pi/180;
% z_t = 0.05;
%theta_g = atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
%phi_g = -atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l));
% x_g = ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
% y_g = ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
% z_g = L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
fun = @(z)120*(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – theta_t)^2 + 40*(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – phi_t)^2 …
+ 1*(L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) +1*(l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))) – z_t)^2;
lb = [0, 0, 0, 0];
ub = [0.1, 0.1, 0.1, 0.1];
if theta_t < 0.4*pi/180
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-17, ‘StepTolerance’, 1e-10);
else
options = optimoptions(‘fmincon’,’Display’,’iter’,’Algorithm’,’sqp’, ‘ConstraintTolerance’, 1e-18, ‘StepTolerance’, 1e-10);
end
function [c,ceq] = mycon(z)
l = 0.82;
L = 1.6;
c = [atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)) – 3.5*pi/180;
-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)) – 2.5*pi/180;
-(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) – 2.5*pi/180];
ceq = [-sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + ((-2*z(3) + 2*z(4))*z(1) + L^2)/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(2) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(3) – 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(4) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) + 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) – 0.8000000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + ((-z(3)^2 + (-2*z(1) + z(2) + z(4))*z(3) + l^2 + 2*z(1)*z(2) – z(2)*z(4))*L^2 + l^2*(z(3) – z(4))^2)/(2*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l);
-cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*cos(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)))*z(1) + 0.8000000000*cos(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l)))*sin(atan(-(z(3) – z(4))/sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))) – 0.4100000000*sin(-atan(L*(z(2) – z(3))/(sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*l))) + L*(z(2) + z(4))/(2*sqrt(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2)*sqrt(((L^2 + l^2)*z(3)^2 + (-2*L^2*z(2) – 2*l^2*z(4))*z(3) + (l^2 + z(2)^2)*L^2 + l^2*z(4)^2)/(l^2*(L^2 + z(3)^2 – 2*z(3)*z(4) + z(4)^2))));
];
end
Z = fmincon(fun, [0.05, 0.05, 0.05, 0.05], [], [], [], [], lb, ub , @mycon, options);
z1 = Z(1);
z2 = Z(2);
z3 = Z(3);
z4 = Z(4);
end real time optimization MATLAB Answers — New Questions

​