Home > demos > rtikdemo.m

rtikdemo

PURPOSE ^

RIKNDEMO Inverse kinematics demo

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

RIKNDEMO Inverse kinematics demo

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003