Robot Mechanics and Control Robot Mechanics and Control

Example 5.10:: Offset Spherical Manipulator: Joint Rate Computation (MATLAB)

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

Contents

Clear All Workspace Objects and Reset All Assumptions

clear all

Structural Parameters

d2 = 18; % cm

Joint Values

theta1 = 90; % deg
theta2 = -90; % deg
d3 = 50; % cm
theta4 = 90; % deg
theta5 = 90; % deg
theta6 = -90; % deg

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

Twist6Rel0Res0 = [0; 0; 0; 10; 0; 0]; % [rad/s; rad/s; rad/s; cm/s; cm/s; cm/s]

Local Variables From U- and T-Matrices

U214 = -d3*sind(theta2);
U224 = d3*cosd(theta2);

T211 = cosd(theta1)*cosd(theta2);
T213 = -cosd(theta1)*sind(theta2);
T214 = d2*sind(theta1);
T221 = sind(theta1)*cosd(theta2);
T223 = -sind(theta1)*sind(theta2);
T224 = -d2*cosd(theta1);

T314 = d3*T213 + T214;
T324 = d3*T223 + T224;

T411 = T211*cosd(theta4) - sind(theta1)*sind(theta4);
T413 = sind(theta1)*cosd(theta4) + T211*sind(theta4);
T421 = cosd(theta1)*sind(theta4) + T221*cosd(theta4);
T423 = T221*sind(theta4) - cosd(theta1)*cosd(theta4);
T431 = sind(theta2)*cosd(theta4);
T433 = sind(theta2)*sind(theta4);

T511 = T213*sind(theta5) + T411*cosd(theta5);
T521 = T223*sind(theta5) + T421*cosd(theta5);
T531 = cosd(theta2)*sind(theta5) + T431*cosd(theta5);

$\mathbf{J}_{11}$

J11 = [0,  sind(theta1), 0;
       0, -cosd(theta1), 0;
       1,  0,            0];

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

invJ12 = -(1/sind(theta5))*[ -T511,              -T521,              -T531;
                             -T413*sind(theta5), -T423*sind(theta5), -T433*sind(theta5);
                              T411,               T421,               T431];

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

invJ21 = -(1/(d3*d3*sind(theta2)))*[ -d3*sind(theta1),    d3*cosd(theta1),    0;
                                     -T314*cosd(theta2), -T324*cosd(theta2), -U214*sind(theta2);
                                      T314*U214,          T324*U214,          U214*U224];

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

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

invJ6Res0 =

         0         0         0   -0.0200         0         0
         0         0         0         0         0    0.0200
         0         0         0    0.3600    1.0000         0
         0    1.0000         0         0         0         0
         0         0   -1.0000   -0.0200         0         0
    1.0000         0         0         0         0   -0.0200

Joint Rates $\dot{\mathbf \Theta} = \{ \dot{\theta}_{1}, \dot{\theta}_{2}, \dot{d}_{3}, \dot{\theta}_{4}, \dot{\theta}_{5}, \dot{\theta}_{6} \}^{\mathrm T}$ in [rad/s; rad/s; cm/s; rad/s; rad/s; rad/s]

ThetaDot = invJ6Res0*Twist6Rel0Res0 % [rad/s; rad/s; cm/s; rad/s; rad/s; rad/s]

ThetaDot =

   -0.2000
         0
    3.6000
         0
   -0.2000
         0

Joint Rates $\dot{\mathbf \Theta} = \{ \dot{\theta}_{1}, \dot{\theta}_{2}, \dot{d}_{3}, \dot{\theta}_{4}, \dot{\theta}_{5}, \dot{\theta}_{6} \}^{\mathrm T}$ in [deg/s; deg/s; cm/s; deg/s; deg/s; deg/s]

radToDeg = 180/pi;
ThetaDotDeg = [radToDeg*ThetaDot(1);
               radToDeg*ThetaDot(2);
               ThetaDot(3);
               radToDeg*ThetaDot(4);
               radToDeg*ThetaDot(5);
               radToDeg*ThetaDot(6)] % [deg/s; deg/s; cm/s; deg/s; deg/s; deg/s]

ThetaDotDeg =

  -11.4592
         0
    3.6000
         0
  -11.4592
         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