Quasi newton method for NLP code problem

4 次查看（过去 30 天）
HN
HN 2020-11-2
编辑： HN 2020-11-4
Hello, Can some one tell whats wrong with the following program ? I tried to change x and evaluate V but I got some difficulties to run. Is there any built in function to solve PRSopt_QN with quasi-newton ?
function V = PRSopt_QN(radius,alpha,beta)
L=1;
z=0.7071068;
ts=1/2;
t=0:ts:1;
for k=1:length(t)
xi_2=beta(k);
rp=radius(k);
for j=1:length(t)
xi_1=0;
xi_2=alpha(j);
xi_3=beta(j);
th(j)=-0.2*cos(2*pi*t(j));
psi(k)=0.2*sin(2*pi*t(k));
phi(j)=atan2(sin(psi(k))*sin(th(j)),(cos(psi(k))+cos(th(j))));
R=Rot('y',th(j))*Rot('x',psi(k))*Rot('z',phi(j));
x(k)=1/2*rp*(-cos(phi(j))*cos(psi(k))+cos(phi(j))*cos(th(j))+sin(phi(j))*sin(psi(k))*sin(th(j)));
y(k)=-rp*cos(psi(k))*sin(phi(j));
zc(k)=z;
delta(k)=sqrt(x(k)^2+y(k)^2)
p=[x(k);y(k);zc(k)];
a1(:,k)=R*[rp;0;0];
a2(:,k)=R*[rp*cos(xi_2);rp*sin(xi_2);0];
a3(:,k)=R*[rp*cos(xi_3);rp*sin(xi_3);0];
r1=p+R*[rp;0;0];
r2=p+R*[rp*cos(xi_2);rp*sin(xi_2);0];
r3=p+R*[rp*cos(xi_3);rp*sin(xi_3);0];
P=1/3*(r1+r2+r3);
g1=inv(Rot('z',0))*r1;
g2=inv(Rot('z',xi_2))*r2;
g3=inv(Rot('z',xi_3))*r3;
% leg length
b1(k)=g1(1)+sqrt(L^2-g1(3)^2);
b2(k)=g2(1)+sqrt(L^2-g2(3)^2);
b3(k)=g3(1)+sqrt(L^2-g3(3)^2);
% passive koint value
sin_th21=(g1(1)-b1(k))/L;
cos_th21=g1(3)/L;
th21(k)=atan2(sin_th21,cos_th21);
sin_th22=(g2(1)-b2(k))/L;
cos_th22=g2(3)/L;
th22(k)=atan2(sin_th22,cos_th22);
sin_th23=(g3(1)-b3(k))/L;
cos_th23=g3(3)/L;
th23(k)=atan2(sin_th23,cos_th23);
pr1(k)=norm(r1-p);
pr2(k)=norm(r3-p);
pr3(k)=norm(r3-p);
r12(k)=norm(r1-r2);
r23(k)=norm(r3-r2);
r31(k)=norm(r3-r1);
Link1(k)=norm(r1-[b1(k);0;0]);
Link2(k)=norm(r2-Rot('z',xi_2)*[b2(k);0;0]);
Link3(k)=norm(r3-Rot('z',xi_3)*[b3(k);0;0]);
s21=[0;1;0];
s22=[-sin(xi_2);cos(xi_2);0];
s23=[-sin(xi_3);cos(xi_3);0];
Constraint1(k)=(p+a1(:,k))'*s21;
Constraint2(k)=(p+a2(:,k))'*s22;
Constraint3(k)=(p+a3(:,k))'*s23;
Gc=[s21',cross(s21,a1(:,k))';s22',cross(s22,a2(:,k))';s23',cross(s23,a3(:,k))'];
M=[eye(6)-transpose(Gc)*pinv((transpose(Gc)))];
st(:,k)=[0.2*cos(2*pi*t(k))*0;0.2*sin(2*pi*t(k))*0;2*cos(2*pi*t(k))*0;2*cos(2*pi*t(k));2*sin(2*pi*t(k));-0.02*cos(2*pi*t(k))*0]; % Desired task velocity
stm(:,k)=M*st(:,k)
wx(k)=st(4,k);
wy(k)=st(5,k);
vz(k)=stm(3,k);
C1=[-sin(xi_1) cos(xi_1) -(a1(1)*cos(xi_1)+a1(2)*sin(xi_1)) ; -sin(xi_2) cos(xi_2) -(a2(1)*cos(xi_2)+a2(2)*sin(xi_2)) ; -sin( xi_3) cos( xi_3) -(a3(1)*cos( xi_3)+a3(2)*sin( xi_3)) ];
C2=[-a1(3)*cos(xi_1) -a1(3)*sin(xi_1) ;-a2(3)*cos(xi_2) -a2(3)*sin(xi_2);-a3(3)*cos( xi_3) -a3(3)*sin( xi_3)]
V(:,k)=(inv(C1)*C2)*[wx(k);wy(k)];
Stm(:,k)=M*[V(1,k);V(2,k);vz(k);wx(k);wy(k);V(3,k)]
constraint2(:,k)=Gc*Stm(:,k)
constraint1(:,k)=Gc*stm(:,k)
con1(k)=constraint1(1,k);
con2(k)=constraint1(2,k);
con3(k)=constraint1(3,k);
end
end
return;
end
%%
function Rotation = Rot(char,a)
c=cos(a); s=sin(a);
switch char
case 'x'
Rotation =[1, 0, 0;
0, c, -s;
0, s, c];
case 'y'
Rotation = [c, 0, s;
0, 1, 0;
-s, 0, c];
case 'z'
Rotation = [c, -s, 0;
s, c, 0;
0, 0, 1];
end

请先登录，再回答此问题。

采纳的回答

Matt J
Matt J 2020-11-2
Is there any built in function to solve PRSopt_QN with quasi-newton ?
Yes, fminunc is a quasi-newton solver,
https://www.mathworks.com/help/optim/ug/fminunc.html#responsive_offcanvas
  21 个评论
显示 19更早的评论
HN
HN 2020-11-3
编辑：HN 2020-11-3
Matt J,
I took the a verage value of V and computed the miniimum value. Below, xOptimal is a vector that shows the three optmizatation variables. So,is v=1.0913e-16 computed at xOptimal =[0.35 ;2.6180 ;-2.6180] ? Thank you
Local minimum possible.
fminunc stopped because it cannot decrease the objective function
along the current search direction.
<stopping criteria details>
xOptimal =
Columns 1 through 9
0.2000 0.2150 0.2300 0.2450 0.2600 0.2750 0.2900 0.3050 0.3200
2.0944 2.1468 2.1991 2.2515 2.3038 2.3562 2.4086 2.4609 2.5133
-2.0944 -2.1468 -2.1991 -2.2515 -2.3038 -2.3562 -2.4086 -2.4609 -2.5133
Columns 10 through 11
0.3350 0.3500
2.5656 2.6180
-2.5656 -2.6180
V =
1.0913e-16
HN
HN 2020-11-4
编辑：HN 2020-11-4
Matt J ,
Would you please help in the following issues I am experiancing ?
  1. How can I set a range of values for optimization variables as a search domain?
  2. No iterations while running the optimization. why is that ?
  3. How can I plot the output in countrour or 3D to visiualize if the out is must be scalar ?
Thank you
First-order
Iteration Func-count f(x) Step-size optimality
0 34 1.6549e-16 0.332
Local minimum possible.
fminunc stopped because it cannot decrease the objective function
along the current search direction.
<stopping criteria details>
xOptimal =
Columns 1 through 9
0.1000 0.1550 0.2100 0.2650 0.3200 0.3750 0.4300 0.4850 0.5400
2.0944 2.1468 2.1991 2.2515 2.3038 2.3562 2.4086 2.4609 2.5133
-2.0944 -2.1468 -2.1991 -2.2515 -2.3038 -2.3562 -2.4086 -2.4609 -2.5133
Columns 10 through 11
0.5950 0.6500
2.5656 2.6180
-2.5656 -2.6180
V =
1.6549e-16

请先登录，再进行评论。

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Solver Outputs and Iterative Display 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by