Quasi newton method for NLP code problem
3 views (last 30 days)
Show older comments
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
0 Comments
Accepted Answer
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
More Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!