RTJADEMO Jacobian demo
0001 %RTJADEMO Jacobian demo 0002 0003 % Copyright (C) 1993-2002, by Peter I. Corke 0004 echo off 0005 % 6/99 change arg to maniplty() to 'yoshikawa' 0006 % $Log: not supported by cvs2svn $ 0007 % Revision 1.2 2002-04-01 11:47:17 pic 0008 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn 0009 % references, clarification of functions. 0010 % 0011 % $Revision: 1.1 $ 0012 echo on 0013 % 0014 % Jacobian and differential motion demonstration 0015 % 0016 % A differential motion can be represented by a 6-element vector with elements 0017 % [dx dy dz drx dry drz] 0018 % 0019 % where the first 3 elements are a differential translation, and the last 3 0020 % are a differential rotation. When dealing with infinitisimal rotations, 0021 % the order becomes unimportant. The differential motion could be written 0022 % in terms of compounded transforms 0023 % 0024 % transl(dx,dy,dz) * trotx(drx) * troty(dry) * trotz(drz) 0025 % 0026 % but a more direct approach is to use the function diff2tr() 0027 % 0028 D = [.1 .2 0 -.2 .1 .1]'; 0029 diff2tr(D) 0030 pause % any key to continue 0031 % 0032 % More commonly it is useful to know how a differential motion in one 0033 % coordinate frame appears in another frame. If the second frame is 0034 % represented by the transform 0035 T = transl(100, 200, 300) * troty(pi/8) * trotz(-pi/4); 0036 % 0037 % then the differential motion in the second frame would be given by 0038 0039 DT = tr2jac(T) * D; 0040 DT' 0041 % 0042 % tr2jac() has computed a 6x6 Jacobian matrix which transforms the differential 0043 % changes from the first frame to the next. 0044 % 0045 pause % any key to continue 0046 0047 % The manipulator's Jacobian matrix relates differential joint coordinate 0048 % motion to differential Cartesian motion; 0049 % 0050 % dX = J(q) dQ 0051 % 0052 % For an n-joint manipulator the manipulator Jacobian is a 6 x n matrix and 0053 % is used is many manipulator control schemes. For a 6-axis manipulator like 0054 % the Puma 560 the Jacobian is square 0055 % 0056 % Two Jacobians are frequently used, which express the Cartesian velocity in 0057 % the world coordinate frame, 0058 0059 q = [0.1 0.75 -2.25 0 .75 0] 0060 J = jacob0(p560, q) 0061 % 0062 % or the T6 coordinate frame 0063 0064 J = jacobn(p560, q) 0065 % 0066 % Note the top right 3x3 block is all zero. This indicates, correctly, that 0067 % motion of joints 4-6 does not cause any translational motion of the robot's 0068 % end-effector. 0069 pause % any key to continue 0070 0071 % 0072 % Many control schemes require the inverse of the Jacobian. The Jacobian 0073 % in this example is not singular 0074 det(J) 0075 % 0076 % and may be inverted 0077 Ji = inv(J) 0078 pause % any key to continue 0079 % 0080 % A classic control technique is Whitney's resolved rate motion control 0081 % 0082 % dQ/dt = J(q)^-1 dX/dt 0083 % 0084 % where dX/dt is the desired Cartesian velocity, and dQ/dt is the required 0085 % joint velocity to achieve this. 0086 vel = [1 0 0 0 0 0]'; % translational motion in the X direction 0087 qvel = Ji * vel; 0088 qvel' 0089 % 0090 % This is an alternative strategy to computing a Cartesian trajectory 0091 % and solving the inverse kinematics. However like that other scheme, this 0092 % strategy also runs into difficulty at a manipulator singularity where 0093 % the Jacobian is singular. 0094 0095 pause % any key to continue 0096 % 0097 % As already stated this Jacobian relates joint velocity to end-effector 0098 % velocity expressed in the end-effector reference frame. We may wish 0099 % instead to specify the velocity in base or world coordinates. 0100 % 0101 % We have already seen how differential motions in one frame can be translated 0102 % to another. Consider the velocity as a differential in the world frame, that 0103 % is, d0X. We can write 0104 % d6X = Jac(T6) d0X 0105 % 0106 T6 = fkine(p560, q); % compute the end-effector transform 0107 d6X = tr2jac(T6) * vel; % translate world frame velocity to T6 frame 0108 qvel = Ji * d6X; % compute required joint velocity as before 0109 qvel' 0110 % 0111 % Note that this value of joint velocity is quite different to that calculated 0112 % above, which was for motion in the T6 X-axis direction. 0113 pause % any key to continue 0114 % 0115 % At a manipulator singularity or degeneracy the Jacobian becomes singular. 0116 % At the Puma's `ready' position for instance, two of the wrist joints are 0117 % aligned resulting in the loss of one degree of freedom. This is revealed by 0118 % the rank of the Jacobian 0119 rank( jacobn(p560, qr) ) 0120 % 0121 % and the singular values are 0122 svd( jacobn(p560, qr) ) 0123 pause % any key to continue 0124 % 0125 % When not actually at a singularity the Jacobian can provide information 0126 % about how `well-conditioned' the manipulator is for making certain motions, 0127 % and is referred to as `manipulability'. 0128 % 0129 % A number of scalar manipulability measures have been proposed. One by 0130 % Yoshikawa 0131 maniplty(p560, q, 'yoshikawa') 0132 % 0133 % is based purely on kinematic parameters of the manipulator. 0134 % 0135 % Another by Asada takes into account the inertia of the manipulator which 0136 % affects the acceleration achievable in different directions. This measure 0137 % varies from 0 to 1, where 1 indicates uniformity of acceleration in all 0138 % directions 0139 maniplty(p560, q, 'asada') 0140 % 0141 % Both of these measures would indicate that this particular pose is not well 0142 % conditioned. 0143 pause % any key to continue 0144 0145 % An interesting class of manipulators are those that are redundant, that is, 0146 % they have more than 6 degrees of freedom. Computing the joint motion for 0147 % such a manipulator is not straightforward. Approaches have been suggested 0148 % based on the pseudo-inverse of the Jacobian (which will not be square) or 0149 % singular value decomposition of the Jacobian. 0150 % 0151 echo off