3DOF Inverse Kinematics - PseudoInverse Jacobian 코드에서 ginput 에러가

Question

minkyung Kwak 2022-6-8

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1736155-3dof-inverse-kinematics-pseudoinverse-jacobian-ginput

回答： sai charan sampara 2023-10-27

% DHmatrix.mlx

function [Mdh] = DHmatrix(theta,d,a,alpha)
%DHMATRIX Summary of this function goes here
%   Detailed explanation goes here
%   inputannya DH parameter yaa
Mdh=[cosd(theta) -sind(theta)*cosd(alpha) sind(theta)*sind(alpha) a*cosd(theta);
     sind(theta) cosd(theta)*cosd(alpha)  -cosd(theta)*sind(alpha) a*sind(theta);
     0,sind(alpha),cosd(alpha),d;
     0,0,0,1];
end

% Inversekinematics.mlx

close all; clear;
theta1=0;d1=0;a1=10;alpha1=0; %parameter link1
theta2=0;d2=0;a2=10;alpha2=0; %parameter link2
theta3=0;d3=0;a3=5;alpha3=0; %parameter link3
deltatheta1=0;deltatheta2=0; deltatheta3=0; %theta1,2,3 velocity
Xc=5;Yc=10;
a=0;b=0;
while a==0
    while b==0
        theta1=theta1+deltatheta1/2;
        theta2=theta2+deltatheta2/2;
        theta3=theta3+deltatheta3/2;%360-theta1-theta2;
        T01=DHmatrix(theta1,d1,a1,alpha1);
        T12=DHmatrix(theta2,d2,a2,alpha2);
        T23=DHmatrix(theta3,d3,a3,alpha3);
        T02=T01*T12;
        T03=T01*T12*T23;
        P0=[0 0];
        P1=transpose(T01(1:2,4));
        P2=transpose(T02(1:2,4));
        P3=transpose(T03(1:2,4));
        Q1=[P0(1,1) P1(1,1) P2(1,1) P3(1,1)];
        Q2=[P0(1,2) P1(1,2) P2(1,2) P3(1,2)];
        disp('end effector pos:');disp(T03);
        
        figure
        plot(Q1,Q2,'-o','LineWidth',4);
        axis([-31,31,-31,31]);
        grid on;
        
    
        %Linear velocity jacobian
        %Prismatic: Jv = zi-1
        %Revolute: Jv = zi-1 x (on - oi-1)
        Z0=[0;0;1];O=[0;0;0];O3=T03(1:3,4);
        Jv1=cross(Z0,(O3-O));
        Z1=T01(1:3,3);O1=T01(1:3,4);
        Jv2=cross(Z1,(O3-O1));
        Z3=T12(1:3,3);O2=T12(1:3,4);
        Jv3=cross(Z1,(O3-O2));
        Jv=[Jv1 Jv2 Jv3];
        disp('Jv:'); disp(Jv);
        
        Xinit=P3(1,1);Yinit=P3(1,2);
        Xend=Xc;Yend=Yc; 
        Xspeed=(Xend-Xinit);
        Yspeed=(Yend-Yinit);
        %Pseudoinverse Jacobian
        thetadot=pinv(Jv)*[Xspeed;Yspeed;0];
        OrinEnd=atan2d(Yend,Xend);
        Orininit=atan2d(Yinit,Xinit);
        dis_error=sqrt(Xend^2+Yend^2)- sqrt(Xinit^2+Yinit^2);
        orin_error=OrinEnd-Orininit;
        if abs(dis_error)<=0.2 && abs(orin_error)<=2
            b=1;
        end
        theta1dot=thetadot(1,1);
        theta2dot=thetadot(2,1);
        theta3dot=thetadot(3,1);
        disp('thetadot:');disp(thetadot);
       
        deltatheta1=rad2deg(theta1dot);
        deltatheta2=rad2deg(theta2dot);
        deltatheta3=rad2deg(theta3dot);
        deltatheta=[deltatheta1;deltatheta2;deltatheta3];
        disp('deltatheta');disp(deltatheta);
        text(P3(1,1),P3(1,2),['  (', num2str(P3(1,1),3), ', ', num2str(P3(1,2),3), ')']);
        text(-25,-17,'Orinerror:','Color','red','FontSize',12)
        text(-25,-20,num2str(abs(orin_error),3),'Color','red','FontSize',12)
        text(-25,-23,'diserror:','Color','red','FontSize',12)
        text(-25,-26,num2str(abs(dis_error),3),'Color','red','FontSize',12)
        pause(0.01);
    end
    if b==1
       [Xc,Yc,buttons] = ginput;
       b=0;
    end
end

end effector pos, Jv, thetadot, deltatheta 값은 잘 나오는데 반해 Inversekinematics.mlx 중 76번째 명령줄에서 "다음 사용 중 오류가 발생함 : ginput / Figure 삭제로 인해 중단되었습니다." 라는 에러가 발생하면서 좌표측에 원하는 점을 선택하기 위해 커서를 움직여 버튼을 클릭하거나 키보드를 눌러봐도 응답하지 않습니다. 혹시 이런 에러가 발생 시 어떻게 해결해야하는지 자문을 구하고 싶습니다!

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

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

Answer 1

sai charan sampara 2023-10-27

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1736155-3dof-inverse-kinematics-pseudoinverse-jacobian-ginput#answer_1341901

Hello Minkyung,

I understand that you are trying to use “ginput” to get the co-ordinates of a point selected by the cursor but getting an error.

This is because “ginput” allows you to select an unlimited number of points until you press the Return key. The co-ordinates of the points that were clicked in between the function call and the pressing of "Return" key are returned as outputs of the function Hence if the "Return" key is not pressed the execution of the program is paused until then. So, in this case once the figure appears select the desired point and then hit "Return" key to get its co-ordinates in “Xc” and “Yc”. This will give the desired result and the execution of the program is continued.

The error is showing up because “ginput” works on the existing figure and extracts co-ordinates from it. Hence if a figure is closed by the user, it shows an error that it was aborted due to figure deletion because there is no figure detected by it.

You can refer to the below documentation to learn more about this property of “ginput”:

https://www.mathworks.com/help/matlab/ref/ginput.html#:~:text=%5Bx%2Cy%5D%20%3D%20ginput%20allows%20you%20to%20select%20an%20unlimited%20number%20of%20points%20until%20you%20press%20the%20Return%20key.

Hope this helps.

Thanks,

Charan.