Quasi newton method for NLP code problem

3 ビュー (過去 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,
  21 件のコメント
HN
HN 2020 年 11 月 3 日
編集済み: HN 2020 年 11 月 3 日
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 日
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 ExchangeSolver 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