RIKNDEMO Inverse kinematics demo
0001 %RIKNDEMO Inverse kinematics demo 0002 0003 % Copyright (C) 1993-2002, by Peter I. Corke 0004 0005 % $Log: not supported by cvs2svn $ 0006 % Revision 1.3 2002-04-02 12:26:49 pic 0007 % Handle figures better, control echo at end of each script. 0008 % Fix bug in calling ctraj. 0009 % 0010 % Revision 1.2 2002/04/01 11:47:17 pic 0011 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn 0012 % references, clarification of functions. 0013 % 0014 % $Revision: 1.1 $ 0015 figure(2) 0016 echo on 0017 % 0018 % Inverse kinematics is the problem of finding the robot joint coordinates, 0019 % given a homogeneous transform representing the last link of the manipulator. 0020 % It is very useful when the path is planned in Cartesian space, for instance 0021 % a straight line path as shown in the trajectory demonstration. 0022 % 0023 % First generate the transform corresponding to a particular joint coordinate, 0024 q = [0 -pi/4 -pi/4 0 pi/8 0] 0025 T = fkine(p560, q); 0026 % 0027 % Now the inverse kinematic procedure for any specific robot can be derived 0028 % symbolically and in general an efficient closed-form solution can be 0029 % obtained. However we are given only a generalized description of the 0030 % manipulator in terms of kinematic parameters so an iterative solution will 0031 % be used. The procedure is slow, and the choice of starting value affects 0032 % search time and the solution found, since in general a manipulator may 0033 % have several poses which result in the same transform for the last 0034 % link. The starting point for the first point may be specified, or else it 0035 % defaults to zero (which is not a particularly good choice in this case) 0036 qi = ikine(p560, T); 0037 qi' 0038 % 0039 % Compared with the original value 0040 q 0041 % 0042 % A solution is not always possible, for instance if the specified transform 0043 % describes a point out of reach of the manipulator. As mentioned above 0044 % the solutions are not necessarily unique, and there are singularities 0045 % at which the manipulator loses degrees of freedom and joint coordinates 0046 % become linearly dependent. 0047 pause % any key to continue 0048 % 0049 % To examine the effect at a singularity lets repeat the last example but for a 0050 % different pose. At the `ready' position two of the Puma's wrist axes are 0051 % aligned resulting in the loss of one degree of freedom. 0052 T = fkine(p560, qr); 0053 qi = ikine(p560, T); 0054 qi' 0055 % 0056 % which is not the same as the original joint angle 0057 qr 0058 pause % any key to continue 0059 % 0060 % However both result in the same end-effector position 0061 fkine(p560, qi) - fkine(p560, qr) 0062 pause % any key to continue 0063 0064 % Inverse kinematics may also be computed for a trajectory. 0065 % If we take a Cartesian straight line path 0066 t = [0:.056:2]; % create a time vector 0067 T1 = transl(0.6, -0.5, 0.0) % define the start point 0068 T2 = transl(0.4, 0.5, 0.2) % and destination 0069 T = ctraj(T1, T2, length(t)); % compute a Cartesian path 0070 0071 % 0072 % now solve the inverse kinematics. When solving for a trajectory, the 0073 % starting joint coordinates for each point is taken as the result of the 0074 % previous inverse solution. 0075 % 0076 tic 0077 q = ikine(p560, T); 0078 toc 0079 % 0080 % Clearly this approach is slow, and not suitable for a real robot controller 0081 % where an inverse kinematic solution would be required in a few milliseconds. 0082 % 0083 % Let's examine the joint space trajectory that results in straightline 0084 % Cartesian motion 0085 subplot(3,1,1) 0086 plot(t,q(:,1)) 0087 xlabel('Time (s)'); 0088 ylabel('Joint 1 (rad)') 0089 subplot(3,1,2) 0090 plot(t,q(:,2)) 0091 xlabel('Time (s)'); 0092 ylabel('Joint 2 (rad)') 0093 subplot(3,1,3) 0094 plot(t,q(:,3)) 0095 xlabel('Time (s)'); 0096 ylabel('Joint 3 (rad)') 0097 0098 pause % hit any key to continue 0099 0100 % This joint space trajectory can now be animated 0101 plot(p560, q) 0102 pause % any key to continue 0103 echo off