Robot Mechanics and Control Robot Mechanics and Control

Example 5.08:: Offset Articulate Manipulator: Joint Rate Computation (MATLAB)

This example uses the inverse Jacobian for the offset articulate manipulator to compute the joint rates that correspond to a given Cartesian twist $^{6}\overline{\mathbf T}_{6/0}$.

Contents

Clear All Workspace Objects and Reset All Assumptions

clear all

Joint Values

theta1 = 90; % deg
theta2 = 90; % deg
theta3 = -180; % deg
theta23 = theta2 + theta3; % deg
theta4 = 90; % deg
theta5 = 90; % deg
theta6 = -90; % deg

Structural Parameters

a2 = 43; % cm
d3 = 18; % cm
d4 = 43; % cm

Given Cartesian Twist $^{6}\overline{\mathbf T}_{6/0}$

Twist6Rel0Res6 = [0; 0; 0; 0; 0; 25]; % [rad/s; rad/s; rad/s; cm/s; cm/s; cm/s]

Local Variables From U-Matrices

U511 = cosd(theta5)*cosd(theta6);
U512 = -cosd(theta5)*sind(theta6);
U521 = sind(theta5)*cosd(theta6);
U522 = -sind(theta5)*sind(theta6);

U411 = U511*cosd(theta4) - sind(theta4)*sind(theta6);
U412 = U512*cosd(theta4) - sind(theta4)*cosd(theta6) ;
U413 = -cosd(theta4)*sind(theta5);
U421 = U511*sind(theta4) + cosd(theta4)*sind(theta6);
U422 = U512*sind(theta4) + cosd(theta4)*cosd(theta6);
U423 = -sind(theta4)*sind(theta5);

U311 = U411*cosd(theta3) - U521*sind(theta3);
U312 = U412*cosd(theta3) - U522*sind(theta3);
U313 = U413*cosd(theta3) - sind(theta3)*cosd(theta5);
U314 = -d4*sind(theta3);
U321 = U411*sind(theta3) + U521*cosd(theta3);
U322 = U412*sind(theta3) + U522*cosd(theta3);
U323 = U413*sind(theta3) + cosd(theta3)*cosd(theta5);
U324 = d4*cosd(theta3);

U211 = U411*cosd(theta23) - U521*sind(theta23);
U212 = U412*cosd(theta23) - U522*sind(theta23);
U213 = U413*cosd(theta23) - sind(theta23)*cosd(theta5);
U214 = a2* cosd(theta2) - d4*sind(theta23);
U221 = U411*sind(theta23) + U521*cosd(theta23);
U222 = U412*sind(theta23) + U522*cosd(theta23);
U223 = U413*sind(theta23) + cosd(theta23)*cosd(theta5);
U224 = a2*sind(theta2) + d4*cosd(theta23);

$\mathbf{J}_{11}$

J11 = [U221,  -U421,  -U421;
       U222,  -U422,  -U422;
       U223,  -U423,  -U423];

$[\mathbf{J}_{12}]^{-1}$

invJ12 = -(1/sind(theta5))*[ -cosd(theta6), sind(theta6), 0;
                             -U522,         U521,         0;
                              U511,         U512,        -sind(theta5)];

$[\mathbf{J}_{21}]^{-1}$

C1 = [d3*U211 + U214*U421;
      d3*U212 + U214*U422;
      d3*U213 + U214*U423];

C2 = [U214*U221 - U211*U224;
      U214*U222 - U212*U224;
      U214*U223 - U213*U224];

C3 = [U314*U321 - U311*U324;
      U314*U322 - U312*U324;
      U314*U323 - U313*U324];

 invJ21 = (1/dot(C1, cross(C2, C3)))*[transpose(cross(C2, C3));
                                      transpose(cross(C3, C1));
                                      transpose(cross(C1, C2))];

Inverse Jacobian: $[^{6}\mathbf{J}_{6}]^{-1}$

invJ6Res6 = [zeros(3,3),  invJ21;
             invJ12,     -invJ12*J11*invJ21]

invJ6Res6 =

         0         0         0         0         0   -0.0233
         0         0         0         0   -0.0233   -0.0097
         0         0         0   -0.0233    0.0233    0.0097
         0    1.0000         0         0         0         0
    1.0000         0         0         0         0   -0.0233
         0         0    1.0000    0.0233         0         0

Joint Rates $\dot{\mathbf \Theta} = \{ \dot{\theta}_{1}, \dot{\theta}_{2}, \ldots, \dot{\theta}_{6} \}^{\mathrm T}$ in rad/s

ThetaDot = invJ6Res6*Twist6Rel0Res6 % [rad/s]

ThetaDot =

   -0.5814
   -0.2434
    0.2434
         0
   -0.5814
         0

Joint Rates $\dot{\mathbf \Theta} = \{ \dot{\theta}_{1}, \dot{\theta}_{2}, \ldots, \dot{\theta}_{6} \}^{\mathrm T}$ in deg/s

ThetaDotDeg = (180/pi)*ThetaDot % [deg/s]

ThetaDotDeg =

  -33.3115
  -13.9443
   13.9443
         0
  -33.3115
         0

This MATLAB example illustrates a computation from the textbook Fundamentals of Robot Mechanics by G. L. Long, Quintus-Hyperion Press, 2015. See http://www.RobotMechanicsControl.info for additional relevant files.


Published with MATLAB® R2014a