Robot Mechanics and Control Robot Mechanics and Control

Example 4.06:: Spherical Wrist: Geometric Solution (MATLAB)

3R Spherical Wrist

This example uses a geometric method to compute the inverse kinematics for a spherical wrist attached to the offset articulate regional structure.

Contents

Clear All Workspace Objects and Reset All Assumptions

clear all

Structural Parameters

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

Position and Orientation for OXYZ6

Given information.

T6N = [ 0.5736  -0.8192  0.0000  18
        0.5792   0.4056  0.7071  43
       -0.5792  -0.4056  0.7071  43
        0       0        0        1 ];

Solution for Regional Structure

This solution comes from Example 4.4.

theta1 = 90; % deg
theta2 = 0; % deg
theta3 = 0; % deg
theta23 = theta2 + theta3; % deg

Normal, Orientation, and Approach Vectors

Extract these vectors from T6N.

nx = T6N(1,1);
ny = T6N(2,1);
nz = T6N(3,1);

ox = T6N(1,2);
oy = T6N(2,2);
oz = T6N(3,2);

ax = T6N(1,3);
ay = T6N(2,3);
az = T6N(3,3);

No-Flip Solution

Here, we solve for $\theta_{4}$, $\theta_{5}$, and $\theta_{6}$ using the equations for the no-flip configuration.

% Solution for theta4
A = ax*sind(theta1)*sind(theta23) - ay*cosd(theta1)*sind(theta23);
B = ax*cosd(theta23) + az*cosd(theta1)*sind(theta23);
C = -ay*cosd(theta23) - az*sind(theta1)*sind(theta23);
mag_kW_cross_a = sqrt(A*A + B*B + C*C);
sintheta4 = (ax*sind(theta1) - ay*cosd(theta1))/mag_kW_cross_a;
costheta4 = (-az*sind(theta23) - ay*sind(theta1)*cosd(theta23) ...
           - ax*cosd(theta1)*cosd(theta23))/mag_kW_cross_a;
theta4 = atan2d(sintheta4, costheta4); % deg
theta4NF = theta4 % deg

% Solution for theta5
sintheta5 = ax*(sind(theta1)*sind(theta4) - cosd(theta1)*cosd(theta23)*cosd(theta4)) ...
            - ay*(cosd(theta1)*sind(theta4) + sind(theta1)*cosd(theta23)*cosd(theta4)) ... ...
			- az*sind(theta23)*cosd(theta4);
costheta5 = -ax*cosd(theta1)*sind(theta23) - ay*sind(theta1)*sind(theta23) + az*cosd(theta23);
theta5 = atan2d(sintheta5, costheta5); % deg
theta5NF = theta5 % deg

% Solution for theta6
sintheta6 = -nx*(sind(theta1)*cosd(theta4) + cosd(theta1)*cosd(theta23)*sind(theta4)) ...
           + ny*(cosd(theta1)*cosd(theta4) - sind(theta1)*cosd(theta23)*sind(theta4)) ...
							  -nz*sind(theta23)*sind(theta4);
costheta6 = -ox*(sind(theta1)*cosd(theta4) + cosd(theta1)*cosd(theta23)*sind(theta4)) ...
			+ oy*(cosd(theta1)*cosd(theta4) - sind(theta1)*cosd(theta23)*sind(theta4)) ...
			- oz*sind(theta23)*sind(theta4);
theta6 = atan2d(sintheta6, costheta6); % deg
theta6NF = theta6 % deg
theta4NF =

   180


theta5NF =

    45


theta6NF =

  145.0005

Flip Solution

Here, we use the relations for the flip configuration.

theta4F = theta4NF - 180 % deg
theta5F = -theta5NF % deg
theta6F = theta6NF - 180 % deg
theta4F =

     0


theta5F =

   -45


theta6F =

  -34.9995

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.