Analytical Solutions of the Inverse Kinematics of a Humanoid Robot

This example shows how to derive analytical solutions for the inverse kinematics of the head chain of a humanoid robot.

Step 1: Define Parameters

Describe the kinematics of the head-chain link (the link between the torso and the head) of the NAO humanoid robot [1] using the Denavit-Hartenberg (DH) parameters and notations based on a study by Kofinas et al. [2]. The following transformation defines the head-chain link.

$T = A_{B a s e, 0} T_{0, 1} T_{1, 2} R_{x} R_{y} A_{2, H e a d}$

where:

$A_{B a s e, 0}$ is the translation from the base (torso) to the joint or reference frame 0
$T_{0, 1}$ is the orientation of reference 1 relative to 0
$T_{1, 2}$ is the orientation of reference 2 relative to 1
$R_{x}$ is the roll rotation
$R_{y}$ is the pitch rotation
$A_{2, H e a d}$ is the translation from reference 2 to the end-effector point (the head)

$T (1 : 3, 4)$ defines the coordinates of the head, which are $x c$ , $y c$ , $z c$ .

In this example, you analytically solve the inverse kinematics problem by returning all orientations of individual joints in the head-chain link given head coordinates of xc, yc, and zc within the reachable space. Then, you convert the analytical results to purely numeric functions for efficiency.

Solving analytically (when doing so is possible) lets you perform computations in real time and avoid singularities, which cause difficulty for numerical methods.

Step 2: Define Forward Kinematics of Head Chain Using DH Parameters

The function getKinematicChain returns the specific kinematic chain for the NAO robot in terms of symbolic variables. For details on getKinematicChain, see the Helper Functions section.

kinChain = 'head';
dhInfo = getKinematicChain(kinChain);
T = dhInfo.T

T = 
 $(\begin{array}{c} r_{11} & r_{12} & r_{13} & xc \\ r_{21} & r_{22} & r_{23} & yc \\ r_{31} & r_{32} & r_{33} & zc \\ 0 & 0 & 0 & 1 \end{array})$

Express the forward kinematics transformation matrix T as the following sequence of products: T = ABase0*T01*T12*Rx*Ry*A2Head. Define the individual matrices as follows.

Specify the translation matrix from the torso (base) to joint 0.

ABase0 = dhInfo.ABase0

ABase0 = 
 $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & NeckOffsetZ \\ 0 & 0 & 0 & 1 \end{array})$

Specify the transformation matrix from joint 0 to joint 1.

T01 = dhInfo.T01

T01 = 
 $(\begin{array}{c} \cos (θ_{1}) & - \sin (θ_{1}) & 0 & 0 \\ \sin (θ_{1}) & \cos (θ_{1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the transformation matrix from joint 1 to joint 2.

T12 = dhInfo.T12

T12 = 
 $(\begin{array}{c} \cos (θ_{2} - \frac{π}{2}) & - \sin (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ - \sin (θ_{2} - \frac{π}{2}) & - \cos (θ_{2} - \frac{π}{2}) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the roll rotation matrix.

Rx = dhInfo.Rx

Rx = 
 $(\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the pitch rotation matrix.

Ry = dhInfo.Ry

Ry = 
 $(\begin{array}{c} 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 \\ - 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array})$

Specify the translation matrix from joint 2 to the head.

A2Head = dhInfo.A2Head

A2Head = 
 $(\begin{array}{c} 1 & 0 & 0 & CameraX \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & CameraZ \\ 0 & 0 & 0 & 1 \end{array})$

The known parameters of this problem are CameraX, CameraZ, NeckOffsetZ, and the positions xc, yc, and zc. The unknown parameters are $θ_{1}$ and $θ_{2}$ . After finding $θ_{1}$ and $θ_{2}$ , you can compute the individual transformations of T. The robot can then achieve the desired position xc, yc, zc.

Although you can see these parameters in the transformation matrices, they do not exist as variables in the MATLAB base workspace. This is because these parameters originate from a function. Functions do not use the base workspace. Each function workspace is separate from the base workspace and all other workspaces to protect the data integrity. Thus, to use these variables outside of the function getKinematicChain, use syms to create them.

syms CameraX CameraZ NeckOffsetZ xc yc zc theta_1 theta_2 real;

Step 3: Find Algebraic Solutions for $θ_{1}$ and $θ_{2}$

Rewrite the equation T = ABase0*T01*T12*Rx*Ry*A2Head, separating the terms that describe the torso and head movements of the robot: inv(T01)*inv(ABase0)*T = T12*Rx*Ry*A2Head. Simplify the left and right sides of the equation, and define equations of interest matching the expressions for coordinate positions.

LHS = simplify(inv(T01)*inv(ABase0)*T);
RHS = simplify(T12*Rx*Ry*A2Head);
eqns = LHS(1:3,end) - RHS(1:3,end)

eqns = 
 $(\begin{array}{c} xc \cos (θ_{1}) - CameraZ \sin (θ_{2}) - CameraX \cos (θ_{2}) + yc \sin (θ_{1}) \\ yc \cos (θ_{1}) - xc \sin (θ_{1}) \\ zc - NeckOffsetZ - CameraZ \cos (θ_{2}) + CameraX \sin (θ_{2}) \end{array})$

This system of equations contains two variables for which you want to find solutions, $θ_{1}$ and $θ_{2}$ . However, the equations also imply that you cannot arbitrarily choose xc, yc, and zc. Therefore, also consider yc as a variable. All other unknowns of the system are symbolic parameters.

This example follows a typical algebraic approach for solving inverse kinematics problems [3]. The idea is to get a compact representation of the solution, where the expression of each variable is in terms of parameters and variables for which you already solved the equations. As a first step, find either $θ_{1}$ or $θ_{2}$ in terms of the parameters. Then, express the other variable in terms of the known variable and parameters. To do this, start by identifying the equation that has only one variable.

intersect(symvar(eqns(1)),[theta_1,theta_2,yc])

ans =  $(\begin{array}{c} θ_{1} & θ_{2} & yc \end{array})$

intersect(symvar(eqns(2)),[theta_1,theta_2,yc])

ans =  $(\begin{array}{c} θ_{1} & yc \end{array})$

intersect(symvar(eqns(3)),[theta_1,theta_2,yc])

ans =  $θ_{2}$

The third equation contains only one variable, $θ_{2}$ . Solve this equation first.

[theta_2_sol,theta_2_params,theta_2_conds] = ...
    solve(eqns(3),theta_2,'Real',true,'ReturnConditions',true)

theta_2_sol = 
 $(\begin{array}{c} 2 π k - 2 atan (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ 2 π k - 2 atan (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

theta_2_params =  $k$

theta_2_conds = 
 $(\begin{array}{c} k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \\ k \in Z \land CameraZ + zc \neq NeckOffsetZ \land {(NeckOffsetZ - zc)}^{2} \leq {CameraX}^{2} + {CameraZ}^{2} \end{array})$

The solutions have an additive $2 π k$ term with the parameter $k$ . Without loss of generalization, you can set $k$ equal to 0 in the solutions and conditions.

theta_2_sol = subs(theta_2_sol,theta_2_params,0)

theta_2_sol = 
 $(\begin{array}{c} - 2 atan (\frac{CameraX - \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \\ - 2 atan (\frac{CameraX + \sqrt{{CameraX}^{2} + {CameraZ}^{2} - {NeckOffsetZ}^{2} + 2 NeckOffsetZ zc - {zc}^{2}}}{CameraZ - NeckOffsetZ + zc}) \end{array})$

for p = 1:numel(theta_2_conds)
    theta_2_conds(p) = simplify(subs(theta_2_conds(p),theta_2_params,0));
end

Now solve for the variables $θ_{1}$ and yc in terms of the variable $θ_{2}$ and other symbolic parameters.

[theta_1_sol,yc_sol,yc_theta_1_params,yc_theta_1_conds] = ...
    solve(eqns(1:2),theta_1,yc,'Real',true,'ReturnConditions',true);

Display the solutions for $θ_{1}$ and yc.

theta_1_sol

theta_1_sol = 
 $\begin{array}{l} (\begin{array}{c} 2 π k - σ_{1} \\ σ_{2} + 2 π k \\ 2 π k - σ_{2} \\ 2 π k - \frac{π}{2} \\ σ_{1} + 2 π k \\ 2 atan (x) + 2 π k \\ \frac{π}{2} + 2 π k \\ 2 π k - \frac{π}{2} \\ \frac{π}{2} + 2 π k \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{2} = 2 atan (\frac{\frac{σ_{3}}{σ_{5} + 1} + \frac{σ_{5} σ_{3}}{σ_{5} + 1}}{σ_{4}}) \\ σ_{3} = \sqrt{- (xc - CameraX + CameraX σ_{5} + xc σ_{5} - 2 CameraZ \tan (\frac{θ_{2}}{2})) σ_{4}} \\ σ_{4} = CameraX + xc - CameraX σ_{5} + xc σ_{5} + 2 CameraZ \tan (\frac{θ_{2}}{2}) \\ σ_{5} = {\tan (\frac{θ_{2}}{2})}^{2} \end{array}$

yc_sol

yc_sol = 
 $\begin{array}{l} (\begin{array}{c} \sqrt{(CameraX + xc) (CameraX - xc)} \\ σ_{1} \\ - σ_{1} \\ CameraX \\ - \sqrt{(CameraX + xc) (CameraX - xc)} \\ 0 \\ σ_{2} \\ - σ_{2} \\ - CameraX \end{array}) \\ where \\ σ_{1} = \frac{\sqrt{- (xc - CameraX + σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} - σ_{4}) (CameraX + xc - σ_{3} + xc {\tan (\frac{θ_{2}}{2})}^{2} + σ_{4})}}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{2} = \frac{- σ_{3} + σ_{4} + CameraX}{{\tan (\frac{θ_{2}}{2})}^{2} + 1} \\ σ_{3} = CameraX {\tan (\frac{θ_{2}}{2})}^{2} \\ σ_{4} = 2 CameraZ \tan (\frac{θ_{2}}{2}) \end{array}$

The solutions depend on two parameters, $k$ and $x$ .

yc_theta_1_params

yc_theta_1_params =  $(\begin{array}{c} k & x \end{array})$

The solutions have an additive $2 π k$ term. Without loss of generalization, you can set $k$ equal to 0 in the solution for theta_1_sol and the conditions yc_theta_1_conds.

theta_1_sol = simplify(subs(theta_1_sol,yc_theta_1_params,[0,0]))

theta_1_sol = 
 $\begin{array}{l} (\begin{array}{c} - 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ σ_{1} \\ - σ_{1} \\ - \frac{π}{2} \\ 2 atan (\frac{\sqrt{(CameraX + xc) (CameraX - xc)}}{CameraX - xc}) \\ 0 \\ \frac{π}{2} \\ - \frac{π}{2} \\ \frac{π}{2} \end{array}) \\ where \\ σ_{1} = 2 atan (\frac{\sqrt{(CameraX \cos (θ_{2}) - xc + CameraZ \sin (θ_{2})) (xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2}))}}{xc + CameraX \cos (θ_{2}) + CameraZ \sin (θ_{2})}) \end{array}$

for p = 1:numel(yc_theta_1_conds)
    yc_theta_1_conds(p) = simplify(subs(yc_theta_1_conds(p),yc_theta_1_params,[0,0]));
end

A similar substitution is not required for yc_sol since there is no dependency on the parameters.

intersect(symvar(yc_sol),yc_theta_1_params)

 
ans =
 
Empty sym: 1-by-0

Step 4: Verify the Solutions

Starting with an arbitrary set of known numeric values for $θ_{1}$ and $θ_{2}$ , compute the numeric values of the end-effector positions xc, yc, and zc with forward kinematics. Then, work backwards from xc, yc, and zc to compute all possible numeric values for $θ_{1}$ and $θ_{2}$ using the inverse kinematics expressions from the previous computation. Verify that one set of the inverse solutions matches the starting set of numeric values for $θ_{1}$ and $θ_{2}$ .

Set the fixed parameters for the robot. The values in this example are for illustration purposes only.

CameraXValue = 53.9; 
CameraZValue = 67.9; 
NeckOffsetZValue = -5;

Using forward computations, find the end-effector positions corresponding to the values $θ_{1}$ and $θ_{2}$ .

Tfk = ABase0*T01*T12*Rx*Ry*A2Head;
xyz = Tfk(1:3,end);

Create a symbolic function for these positions.

xyzFunc = symfun(subs(xyz,[CameraX,CameraZ,NeckOffsetZ], ...
  [CameraXValue,CameraZValue,NeckOffsetZValue]),[theta_1,theta_2]);

For forward kinematics, specify two variables theta_1_known and theta_2_known that contain an arbitrary starting set of values for $θ_{1}$ and $θ_{2}$ .

theta_1_known = [pi/4,pi/6,pi/8,pi/10];
theta_2_known = [pi/8,-pi/12,pi/16,-pi/10];
numPts = numel(theta_1_known);
num_theta_2 = numel(theta_2_sol);
num_theta_1 = numel(theta_1_sol);

Note that there are potentially num_theta_1 solutions for each num_theta_2 solution.

Use the following sequence of operations to verify the solutions.

Loop over (theta_1_known,theta_2_known). For each point, compute the end positions x_known, y_known, and z_known, which are then "known".
Loop over the solutions for $θ_{2}$ corresponding to x_known and z_known. For each solution, check to see if the corresponding condition cond_theta_2 is valid. If the condition is valid, compute the corresponding numeric solution theta_2_derived.
Loop over the solutions for $θ_{1}$ corresponding to x_known, z_known, and theta_2_derived. For each solution, check to see if the corresponding condition cond_theta_1 is valid. If the condition is valid, check to see if y_derived numerically matches y_known within relative and absolute tolerances through the condition cond_yc. If this condition is valid, then compute theta_1_derived.
Collect the results in a table T for display purposes.

T = table([],[],[],[],'VariableNames',{'theta_1_known','theta_2_known',...
    'theta_1_derived','theta_2_derived'});

for ix1 = 1:numPts
xyz_known = double(xyzFunc(theta_1_known(ix1),theta_2_known(ix1)));
x_known = xyz_known(1);
y_known = xyz_known(2);
z_known = xyz_known(3);
for ix2 = 1:num_theta_2 % theta_2 loop
  cond_theta_2 = subs(theta_2_conds(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],...
                 [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]);
  if isAlways(cond_theta_2) % if theta_2 is valid
    theta_2_derived = subs(theta_2_sol(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],...
                      [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]);
    theta_2_derived = double(theta_2_derived);
    for ix3 = 1:num_theta_1 % theta_1 loop
      cond_theta_1 = subs(yc_theta_1_conds(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
              [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
      if isAlways(cond_theta_1) % if theta_1 is valid
        y_derived = subs(yc_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
                [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
        y_derived = double(y_derived);
        cond_yc = abs(y_known - y_derived) < max(100*eps,1e-6*abs(y_known)); % check rounding
        if isAlways(cond_yc) % if yc is valid
          theta_1_derived = subs(theta_1_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],...
                  [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]);
          theta_1_derived = double(theta_1_derived);
          T = vertcat(T,table(theta_1_known(ix1),theta_2_known(ix1),theta_1_derived,...
              theta_2_derived,'VariableNames',T.Properties.VariableNames));
        end
      end
    end
  end
end
end
T

T=8×4 table
    theta_1_known    theta_2_known    theta_1_derived    theta_2_derived
    _____________    _____________    _______________    _______________

        0.7854           0.3927            0.7854             0.3927    
        0.7854           0.3927           -2.3562            -1.7346    
        0.5236          -0.2618            0.5236            -0.2618    
        0.5236          -0.2618            -2.618            -1.0801    
        0.3927          0.19635            0.3927            0.19635    
        0.3927          0.19635           -2.7489            -1.5383    
       0.31416         -0.31416           0.31416           -0.31416    
       0.31416         -0.31416           -2.8274            -1.0278

Observe that there are two possible solution pairs (theta_1_derived,theta_2_derived) obtained using inverse kinematics for each pair of starting angles (theta_1_known,theta_2_known). One set of the inverse solutions matches the starting angles.

References

SoftBank Robotics. NAO. https://www.aldebaran.com/en/nao.
Kofinas, N., E. Orfanoudakis, and M. G. Lagoudakis. "Complete Analytical Inverse Kinematics for NAO." In 2013 13th International Conference on Autonomous Robot Systems. Lisbon, Portugal: Robotica, 2013.
Kendricks, K. "Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory." Ph.D. Thesis. Auburn University, Auburn, AL, 2007.

Helper Functions

function kinChain = getKinematicChain(link)
% This function returns the kinematic chain of the NAO humanoid robot
% represented using Denavit-Hartenberg (DH) parameters.
% The function uses as a reference the paper: Complete analytical inverse
% kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G.,
% 2013 13th International Conference on Autonomous Robot Systems
% (Robotica), Publication Year: 2013.
    if nargin < 1
        link = 'head';
    end
    % Notation: A, R, and T are the translation, rotation, and DH
    % transformation matrices, respectively.
    % Specify DH parameters for the desired end configuration.
    syms r11 r12 r13 r21 r22 r23 r31 r32 r33 xc yc zc real;
    R = [r11 r12 r13;r21 r22 r23;r31 r32 r33];
    kinChain.T = [R,[xc;yc;zc];0,0,0,1];
    switch link
        case 'head' % Kinematic chain from torso (base) to head.
            syms CameraX CameraZ NeckOffsetZ real;
            % Translation from torso (base) to joint 0.
            ABase0 = getA([0 0 NeckOffsetZ]);
            % Translation from joint 2 to head.
            A2Head = getA([CameraX 0 CameraZ]);
            % Transformation from joint 0 to joint 1.
            % theta_1 is the rotation angle corresponding to the 0-1 link.
            syms theta_1 real;
            alpha1 = 0; a1 = 0; d1 = 0;
            T01 = getT(a1,alpha1,d1,theta_1);
            % Transformation from joint 1 to joint 2.
            % theta_2 is the rotation angle corresponding to the 1-2 link.
            syms theta_2 real;
            piby2 = str2sym('pi/2');
            d2 = 0; a2 = 0; alpha2 = -piby2;
            T12 = getT(a2,alpha2,d2,theta_2-piby2);
            % Rx is the roll rotation.
            Rx = getR('x',piby2);
            % Ry is the pitch rotation.
            Ry = getR('y',piby2);
            % Capture the kinematic chain as a string.
            % The transformation is from the base to the head.
            kinChain.DHChain = 'ABase0*T01*T12*Rx*Ry*A2Head';
            kinChain.ABase0 = ABase0;
            kinChain.T01 = T01;
            kinChain.T12 = T12;
            kinChain.Rx = Rx;
            kinChain.Ry = Ry;
            kinChain.A2Head = A2Head;
        case 'lefthand' % Kinematic chain from torso to left hand.
            syms ShoulderOffsetY ElbowOffsetY ShoulderOffsetZ real;
            ABase0 = getA([0 (ShoulderOffsetY+ElbowOffsetY) ShoulderOffsetZ]);
            syms HandOffsetX LowerArmLength real;
            AEnd4 = getA([(HandOffsetX+LowerArmLength) 0 0]);
            piby2 = str2sym('pi/2');
            syms theta_1 real;
            alpha1 = -piby2; a1 = 0; d1 = 0;
            T01 = getT(a1,alpha1,d1,theta_1);
            syms theta_2 real;
            d2 = 0; a2 = 0; alpha2 = piby2;
            T12 = getT(a2,alpha2,d2,theta_2-piby2);
            syms UpperArmLength theta_3 real;
            d3 = UpperArmLength; a3 = 0; alpha3 = -piby2;
            T32 = getT(a3,alpha3,d3,theta_3);
            syms theta_4 real;
            d4 = 0; a4 = 0; alpha4 = piby2;
            T43 = getT(a4,alpha4,d4,theta_4);
            Rz = getR('z',piby2);
            kinChain.DHChain = 'ABase0*T01*T12*T32*T43*Rz*AEnd4';
            kinChain.ABase0 = ABase0;
            kinChain.T01 = T01;
            kinChain.T12 = T12;
            kinChain.T32 = T32;
            kinChain.T43 = T43;
            kinChain.Rz = Rz;
            kinChain.AEnd4 = AEnd4;
    end
end

function A = getA(vec)
% This function returns the translation matrix.
    A = [1 0 0 vec(1);
        0 1 0 vec(2);
        0 0 1 vec(3);
        0 0 0 1];
end

function R = getR(orientation,theta)
% This function returns the rotation matrix.
    switch orientation
        case 'x'
            R = [1 0 0 0;
                0 cos(theta) -sin(theta) 0;
                0 sin(theta) cos(theta) 0
                0 0 0 1];
        case 'y'
            R = [cos(theta) 0 sin(theta) 0;
                0 1 0 0;
                -sin(theta) 0 cos(theta) 0;
                0 0 0 1];
        case 'z'
            R = [cos(theta) -sin(theta) 0 0;
                sin(theta) cos(theta) 0 0;
                0 0 1 0;
                0 0 0 1];
    end
end

function T = getT(a,alpha,d,theta)
% This function returns the Denavit-Hartenberg (DH) matrix.
    T = [cos(theta),-sin(theta),0,a;
         sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d;
         sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d;
         0,0,0,1];
end