scm_menu2.m

针对移动机械臂运动学

%SCM_MENU2 Auxiliary: Demonstration of the SCM Method (Inverse Kinematics)

%July ~ December 2005, by Reinaldo M. do Nascimento

clc
Step='Step 1';
while 1
     selec3 = menu('Inverse Kinematics',...
                    Step,...
                   'Exit');
     switch selec3
           case 1
               switch Step
                     case 'Step 1'
                         selec4=menu('Get (numeric joint variables) ==> q = QRAND(manipulator,q_input) or q = QRAND(manipulator)',...
                                     'by User',...
                                     'by Program');
                         switch selec4
                               case 1
                                   q=input('Destination. Type [q1 ... qn] ==>   ');
                                   Step='Step 2';
                                   try
                                      q=qrand(manipulator,q)
                                   catch
                                        Step='Step 1';
                                   end
                               case 2
                                   q=qrand(manipulator)
                                   Step='Step 2';
                         end
                     case 'Step 2'
                         selec4=menu('Get (numeric homogeneous transformation) ==> D0 = S2D(S0,q)',...
                                     'Go');
                         switch selec4
                               case 1
                                   switch exist('S0')
                                         case 0
                                             S0=scm(manipulator,0)
                                         case 1
                                   end
                                   D0=s2d(S0,q)
                                   Step='Step 3';
                         end
                     case 'Step 3'
                         selec4=menu('Get (inverse kinematics) ==> Si = SCM(manipulator, D0) or Si = SCM(manipulator, D0, q_origin)',...
                                     'q_origin == 0',...
                                     'q_origin ~= 0');
                         switch selec4
                               case 1
                                   clear q_origin;
                                   try
                                      Si=scm(manipulator,D0)
                                      Step='Check';
                                   catch
                                        disp('Solution wouldn''t converge')
                                        Step='Step 1';
                                   end
                               case 2
                                   q_origin=input('Origin. Type [q1 ... qn]:   ');
                                   try,q_test=qrand(manipulator,q_origin);catch,clear q_origin;Step='Step 3';end
                                   switch exist('q_test')
                                         case 0
                                         case 1
                                             try
                                                Si=scm(manipulator,D0,q_origin)
                                                Step='Check';
                                             catch
                                                  disp('Solution wouldn''t converge')
                                                  Step='Step 1';  
                                             end
                                   end
                         end
                     case 'Check'
                         while 1
                              selec4=menu('Check ==> q_rad = IK2Q(Si,''rad''), q_deg = IK2Q(Si,''deg''), q_calculated = IK2Q(Si)',...
                                          'Desired Values (M,Rad)',...
                                          'Calculated Values (M,Rad)',...
                                          'q_rad_pi (Calculated)',...
                                          'q_deg_180 (Calculated)',...
                                          'Si',...
                                          'Exit');
                              switch selec4
                                    case 1
                                        switch exist('q_origin')
                                              case 0
                                                  q_origin=zeros(1,length(manipulator.chain.variables))
                                              case 1
                                                  q_origin
                                        end
                                        q_desired=q.q
                                        H_desired=D0
                                    case 2
                                        switch exist('q_c')
                                            case 0
                                                q_calculated=ik2q(Si)
                                                q_c=qrand(manipulator,q_calculated);
                                                H_calculated=s2d(S0,q_c)
                                            case 1
                                                q_calculated
                                                H_calculated
                                        end
                                    case 3
                                        q_rad_pi=ik2q(Si,'rad')
                                    case 4
                                        q_deg_180=ik2q(Si,'deg')
                                    case 5
                                        Si
                                    case 6
                                        Step='Reset';
                                        break
                              end
                         end
                     case 'Reset'
                         clear q*
                         Step='Step 1';
               end
            case 2
                break
     end
end
clear selec3 selec4 Step* q* H_*