Example 4.08:: Offset Spherical Manipulator: Algebraic Solution (MATLAB)

This example uses the algebraic method to compute the inverse kinematics for the offset spherical manipulator.

Clear All Workspace Objects and Reset All Assumptions
Structural Parameters
Position and Orientation for OXYZ6
Normal, Orientation, Approach, and Position Vectors
Right-Shoulder Solution for $\theta_{1}$
Solution for $\theta_{2}$
Solution for $d_{3}$
Solution for $\theta_{4}$ , No-Flip
Solution for $\theta_{5}$ , No-Flip
Solution for $\theta_{6}$ , No-Flip
Solution for $\theta_{4}$ , Flip
Solution for $\theta_{5}$ , Flip
Solution for $\theta_{6}$ , Flip

Clear All Workspace Objects and Reset All Assumptions

clear all

Structural Parameters

d2 = 18; %cm

Position and Orientation for OXYZ6

Given information.

T6N = [ 0  0  1  18
        0  1  0  50
       -1  0  0   0
        0  0  0   1 ];

Normal, Orientation, Approach, and Position 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);

px = T6N(1,4);
py = T6N(2,4);
pz = T6N(3,4);

Right-Shoulder Solution for $\theta_{1}$

theta1R = atan2d(py, px) + atan2d(d2, sqrt(px*px + py*py - d2*d2)); % deg
theta1 = theta1R % deg

theta1 =

    90

Solution for $\theta_{2}$

theta2 = atan2d(-pz*cosd(theta1) - py*sind(theta1), pz) % deg

theta2 =

   -90

Solution for $d_{3}$

V114 = px*cosd(theta1) + py*sind(theta1);
d3 = -V114*sind(theta2) + pz*cosd(theta2) % cm

d3 =

    50

Solution for $\theta_{4}$ , No-Flip

V113 = ax*cosd(theta1) + ay*sind(theta1);
V133 = ax*sind(theta1) - ay*cosd(theta1);
V213 = az*sind(theta2) + V113*cosd(theta2);
theta4NF = atan2d(V133, -V213);
theta4 = theta4NF % deg

theta4 =

    90

Solution for $\theta_{5}$ , No-Flip

V233 = az*cosd(theta2) - V113*sind(theta2);
theta5NF = atan2d(V133*sind(theta4) - V213*cosd(theta4), V233);
theta5 = theta5NF % deg

theta5 =

    90

Solution for $\theta_{6}$ , No-Flip

V112 = ox*cosd(theta1) + oy*sind(theta1);
V132 = ox*sind(theta1) - oy*cosd(theta1);
V212 = oz*sind(theta2) + V112*cosd(theta2);
V232 = oz*cosd(theta2) - V112*sind(theta2);
V412 = V212*cosd(theta4) - V132*sind(theta4);
V432 = V212*sind(theta4) + V132*cosd(theta4);
theta6NF = atan2d(-V232*sind(theta5) - V412*cosd(theta5), - V432);
theta6 = theta6NF % deg

theta6 =

   -90

Solution for $\theta_{4}$ , Flip

theta4F = atan2d(-V133, V213);
theta4 = theta4F % deg

theta4 =

   -90

Solution for $\theta_{5}$ , Flip

theta5F = atan2d(V133*sind(theta4) - V213*cosd(theta4), V233); % deg
theta5 = theta5F % deg

theta5 =

   -90

Solution for $\theta_{6}$ , Flip

V412 = V212*cosd(theta4) - V132*sind(theta4);
V432 = V212*sind(theta4) + V132*cosd(theta4);
theta6F = atan2d(-V232*sind(theta5) - V412*cosd(theta5), - V432); % deg
theta6 = theta6F % deg

theta6 =

    90

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.

Example 4.08:: Offset Spherical Manipulator: Algebraic Solution (MATLAB)

Contents

Clear All Workspace Objects and Reset All Assumptions

Structural Parameters

Position and Orientation for OXYZ6

Normal, Orientation, Approach, and Position Vectors

Right-Shoulder Solution for

Solution for

Solution for

Solution for , No-Flip

Solution for , No-Flip

Solution for , No-Flip

Solution for , Flip

Solution for , Flip

Solution for , Flip

Right-Shoulder Solution for $\theta_{1}$

Solution for $\theta_{2}$

Solution for $d_{3}$

Solution for $\theta_{4}$ , No-Flip

Solution for $\theta_{5}$ , No-Flip

Solution for $\theta_{6}$ , No-Flip

Solution for $\theta_{4}$ , Flip

Solution for $\theta_{5}$ , Flip

Solution for $\theta_{6}$ , Flip