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=ABase,0T0,1T1,2RxRyA2,Head

where:

  • ABase,0 is the translation from the base (torso) to the joint or reference frame 0

  • T0,1 is the orientation of reference 1 relative to 0

  • T1,2 is the orientation of reference 2 relative to 1

  • Rx is the roll rotation

  • Ry is the pitch rotation

  • A2,Head is the translation from reference 2 to the end-effector point (the head)

T(1:3,3) defines the coordinates of the head, which are xc, yc, zc.

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 = 

(r11r12r13xcr21r22r23ycr31r32r33zc0001)

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 = 

(10000100001NeckOffsetZ0001)

Specify the transformation matrix from joint 0 to joint 1.

T01 = dhInfo.T01
T01 = 

(cos(θ1)-sin(θ1)00sin(θ1)cos(θ1)0000100001)

Specify the transformation matrix from joint 1 to joint 2.

T12 = dhInfo.T12
T12 = 

(cos(θ2-π2)-sin(θ2-π2)000010-sin(θ2-π2)-cos(θ2-π2)000001)

Specify the roll rotation matrix.

Rx = dhInfo.Rx
Rx = 

(100000-1001000001)

Specify the pitch rotation matrix.

Ry = dhInfo.Ry
Ry = 

(00100100-10000001)

Specify the translation matrix from joint 2 to the head.

A2Head = dhInfo.A2Head
A2Head = 

(100CameraX0100001CameraZ0001)

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 = 

(xccos(θ1)-CameraZsin(θ2)-CameraXcos(θ2)+ycsin(θ1)yccos(θ1)-xcsin(θ1)zc-NeckOffsetZ-CameraZcos(θ2)+CameraXsin(θ2))

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 = (θ1θ2yc)
intersect(symvar(eqns(2)),[theta_1,theta_2,yc])
ans = (θ1yc)
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 = 

(2πk-2atan(CameraX-CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc)2πk-2atan(CameraX+CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc))

theta_2_params = k
theta_2_conds = 

(CameraZ+zcNeckOffsetZNeckOffsetZ-zc2CameraX2+CameraZ2kZCameraZ+zcNeckOffsetZNeckOffsetZ-zc2CameraX2+CameraZ2kZ)

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 = 

(-2atan(CameraX-CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc)-2atan(CameraX+CameraX2+CameraZ2-NeckOffsetZ2+2NeckOffsetZzc-zc2CameraZ-NeckOffsetZ+zc))

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 = 

(2πk-σ1σ2+2πk2πk-σ22πk-π2σ1+2πk2atan(x)+2πkπ2+2πk2πk-π2π2+2πk)where  σ1=2atan(CameraX+xcCameraX-xcCameraX-xc)  σ2=2atan(σ3σ5+1+σ5σ3σ5+1σ4)  σ3=-xc-CameraX+CameraXσ5+xcσ5-2CameraZtan(θ22)σ4  σ4=CameraX+xc-CameraXσ5+xcσ5+2CameraZtan(θ22)  σ5=tan(θ22)2

yc_sol
yc_sol = 

(CameraX+xcCameraX-xcσ1-σ1CameraX-CameraX+xcCameraX-xc0σ2-σ2-CameraX)where  σ1=-xc-CameraX+σ3+xctan(θ22)2-σ4CameraX+xc-σ3+xctan(θ22)2+σ4tan(θ22)2+1  σ2=-σ3+σ4+CameraXtan(θ22)2+1  σ3=CameraXtan(θ22)2  σ4=2CameraZtan(θ22)

The solutions depend on two parameters, k and x.

yc_theta_1_params
yc_theta_1_params = (kx)

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 = 

(-2atan(CameraX+xcCameraX-xcCameraX-xc)σ1-σ1-π22atan(CameraX+xcCameraX-xcCameraX-xc)0π2-π2π2)where  σ1=2atan(CameraXcos(θ2)-xc+CameraZsin(θ2)xc+CameraXcos(θ2)+CameraZsin(θ2)xc+CameraXcos(θ2)+CameraZsin(θ2))

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.

  1. 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".

  2. 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.

  3. 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.

  4. 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 numerical match
                    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

  1. SoftBank Robotics. NAO. https://www.softbankrobotics.com/emea/en/nao.

  2. 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.

  3. 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