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
Best Answer