Quasi newton method for NLP code problem

3 views (last 30 days)
HN
HN on 2 Nov 2020
Edited: HN on 4 Nov 2020
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

Accepted Answer

Matt J
Matt J on 2 Nov 2020
Is there any built in function to solve PRSopt_QN with quasi-newton ?
Yes, fminunc is a quasi-newton solver,
  21 Comments
HN
HN on 3 Nov 2020
Edited: HN on 3 Nov 2020
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 on 4 Nov 2020
Edited: HN on 4 Nov 2020
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

Sign in to comment.

More Answers (0)

Community Treasure Hunt

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

Start Hunting!