Symbolic Math Toolbox

Analytical Solutions of the Inverse Kinematics of a Humanoid Robot

This example derives analytical solutions for the inverse kinematics of the head chain of a humanoid robot.

This example uses the following Symbolic Math Toolbox capablities:

Contents

Motivation

This example describes the kinematics of the head-chain link (the link between the torso and the head) of the chosen robot - the Aldebaran NAO robot [1] using the Denavit-Hartenberg (DH) parameters and notations described in Kofinas et al [2]. The following transformation defines the head-chain link.

$$T = A_{Base,0} T_{0,1} T_{1,2} R_x R_y A_{2,Head}$$

Here,

  • $A_{Base,0}$ is the translation from the Base (torso) to joint or reference frame 0,
  • $T_{0,1}$ is the orientation of reference 1 relative to 0,
  • $T_{1,2}$ is the orientation of 2 relative to 1,
  • $R_x$ is the roll rotation,
  • $R_y$ is the pitch rotation,
  • $A_{2,Head}$ is the translation from 2 to the end effector point (the head).

$T(1:3,3)$ defines the coordinates px, py, pz of the head.

This example analytically solves the inverse kinematics problem by returning all orientations of individual joints in the head-chain link given a position of the head px, py, pz within the reachable-space. Then, it converts the analytical results to purely numeric functions for efficiency.

The main advantages of an analytical solution, when available, are real-time computation and avoidance of singularities that cause difficulty for numerical methods.

Define Forward Kinematics of the 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 [4].

kinChain = 'head';
dhInfo = getKinematicChain(kinChain);
T = dhInfo.T
 
T =
 
[ r11, r12, r13, px]
[ r21, r22, r23, py]
[ r31, r32, r33, pz]
[   0,   0,   0,  1]
 

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

Translation from torso (base) to joint 0.

ABase0 = dhInfo.ABase0
 
ABase0 =
 
[ 1, 0, 0,           0]
[ 0, 1, 0,           0]
[ 0, 0, 1, NeckOffsetZ]
[ 0, 0, 0,           1]
 

Transformation from joint 0 to joint 1.

T01 = dhInfo.T01
 
T01 =
 
[ cos(theta_1), -sin(theta_1), 0, 0]
[ sin(theta_1),  cos(theta_1), 0, 0]
[            0,             0, 1, 0]
[            0,             0, 0, 1]
 

Transformation from joint 1 to joint 2.

T12 = dhInfo.T12
 
T12 =
 
[  cos(theta_2 - pi/2), -sin(theta_2 - pi/2), 0, 0]
[                    0,                    0, 1, 0]
[ -sin(theta_2 - pi/2), -cos(theta_2 - pi/2), 0, 0]
[                    0,                    0, 0, 1]
 

Rx is the roll rotation.

Rx = dhInfo.Rx
 
Rx =
 
[ 1, 0,  0, 0]
[ 0, 0, -1, 0]
[ 0, 1,  0, 0]
[ 0, 0,  0, 1]
 

Ry is the pitch rotation.

Ry = dhInfo.Ry
 
Ry =
 
[  0, 0, 1, 0]
[  0, 1, 0, 0]
[ -1, 0, 0, 0]
[  0, 0, 0, 1]
 

Translation from joint 2 to head.

A2Head = dhInfo.A2Head
 
A2Head =
 
[ 1, 0, 0, CameraX]
[ 0, 1, 0,       0]
[ 0, 0, 1, CameraZ]
[ 0, 0, 0,       1]
 

The known parameters of this problem are CameraX, CameraZ, NeckOffsetZ, and the positions px, py, pz. The unknown parameters are $\theta_1$ and $\theta_2$. After finding $\theta_1$ and $\theta_2$, you can compute the individual transformations of T. Therefore, the robot can achieve the desired position px, py, pz.

Find Algebraic Solutions for $\theta_1$ and $\theta_2$

First, rearrange the LHS and RHS 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 =
 
 px*cos(theta_1) - CameraZ*sin(theta_2) - CameraX*cos(theta_2) + py*sin(theta_1)
                                               py*cos(theta_1) - px*sin(theta_1)
                  pz - NeckOffsetZ - CameraZ*cos(theta_2) + CameraX*sin(theta_2)
 

In this system of equations, there are two unknown parameters of interest: $\theta_1$ and $\theta_2$. However, the equations also imply that you cannot arbitrarily choose px, py, and pz. Therefore, py is also considered to be an unknown parameter.

The following variables are not yet visible in the script because you introduced them in the function. Create symbolic variables for them.

syms theta_1 theta_2 py real;

Also, create symbolic variables for the following parameters.

syms CameraX CameraZ NeckOffsetZ px pz real;

This example follows a typical algebraic approach for solving inverse kinematics problems (See Kendricks [3]). The idea is to get a compact representation of the solution, with each variable expressed in terms of parameters and the previously solved-for variables. As a first step, find either $\theta_1$ or $\theta_2$ in terms of the parameters. Then, express the other variable in terms of the solved variable and parameters. To do this, start by identifying the equation that has only one variable.

intersect(symvar(eqns(1)),[theta_1,theta_2,py])
intersect(symvar(eqns(2)),[theta_1,theta_2,py])
intersect(symvar(eqns(3)),[theta_1,theta_2,py])
 
ans =
 
[ py, theta_1, theta_2]
 
 
ans =
 
[ py, theta_1]
 
 
ans =
 
theta_2
 

The third equation contains only one unknown, $\theta_2$. Solve this equation first.

theta_2sol = simplify(solve(eqns(3),theta_2,'Real',true));
pretty(theta_2sol)
/  imag(#2) 2 i - 2 #2  \
|                       |
\ - 2 #1 + imag(#1) 2 i /

where

                                     2          2              2
   #1 == atan((CameraX - sqrt(CameraX  + CameraZ  - NeckOffsetZ

                             2
      + 2 NeckOffsetZ pz - pz ))/(CameraZ - NeckOffsetZ + pz))

                                     2          2              2
   #2 == atan((CameraX + sqrt(CameraX  + CameraZ  - NeckOffsetZ

                             2
      + 2 NeckOffsetZ pz - pz ))/(CameraZ - NeckOffsetZ + pz))


Now, find $\theta_1$ and py in terms of $\theta_2$ and parameters.

temp = solve(eqns(1:2),theta_1,py,'Real',true);
theta_1sol = simplify(temp.theta_1);
pretty(theta_1sol)
/  #1 \
|     |
\ -#1 /

where

   #1 == atan(sqrt((CameraX cos(theta_2) - px + CameraZ sin(theta_2))

      (px + CameraX cos(theta_2) + CameraZ sin(theta_2)))/(px

      + CameraX cos(theta_2) + CameraZ sin(theta_2))) 2


py_sol = simplify(temp.py);
pretty(py_sol)
              2             2          2             2     2
[[sqrt(CameraX  cos(theta_2)  + CameraZ  sin(theta_2)  - px

   + 2 CameraX CameraZ cos(theta_2) sin(theta_2))],

                2             2          2             2     2
  [-sqrt(CameraX  cos(theta_2)  + CameraZ  sin(theta_2)  - px

   + 2 CameraX CameraZ cos(theta_2) sin(theta_2))]]

The number of $\theta_2$ solutions is as follows.

numTheta2 = numel(theta_2sol);

For each $\theta_2$, there are numel($\theta_1$) solutions.

numTheta1PerTheta2 = numel(theta_1sol);

Verify the Solutions

Verifying the solutions of this problem requires two steps:

  1. Create numeric functions.
  2. Verify the solutions by using these numeric functions.

Starting with an arbitrary set of known $\theta_1$ and $\theta_2$ values, compute the end-effector positions px, py, pz. Then, work backwards from px, py, pz to compute all possible $\theta_1$ and $\theta_2$. Verify that one set of the reverse solutions matches the starting set of $\theta_1$ and $\theta_2$.

Set the fixed parameters for the robot. These values 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 $\theta_1$ and $\theta_2$, and then generate MATLAB function for these positions.

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

pxyzFunc = matlabFunction(subs(pxyz,[CameraX,CameraZ,NeckOffsetZ],...
    [CameraXValue,CameraZValue,NeckOffsetZValue]),...
    'vars',[theta_1,theta_2]);

Using inverse computations, generate MATLAB functions for $\theta_2$ and $\theta_2$.

theta_2IKFunc = matlabFunction(subs(theta_2sol,...
    [CameraX,CameraZ,NeckOffsetZ],...
    [CameraXValue,CameraZValue,NeckOffsetZValue]),...
    'vars',[px,py,pz]);

theta_1IKFunc = matlabFunction(subs(theta_1sol,...
    [CameraX,CameraZ,NeckOffsetZ],...
    [CameraXValue,CameraZValue,NeckOffsetZValue]),...
    'vars',[theta_2,px,py,pz]);

Now, you can use the MATLAB functions for $\theta_1$ and $\theta_2$ to verify the solutions. First, pick an arbitrary starting set of values of $\theta_1$ and $\theta_2$ and perform forward kinematics.

theta_1_known = [pi/4,pi/6,pi/8,pi/10];
theta_2_known = [pi/8,-pi/12,pi/16,-pi/10];
pxyzVal = pxyzFunc(theta_1_known,theta_2_known);
numPts = numel(theta_1_known);

Then, evaluate the inverse kinematics based ending values of $\theta_1$ and $\theta_2$.

theta_2_derived = theta_2IKFunc(pxyzVal(1,:),pxyzVal(2,:),pxyzVal(3,:));
theta_1_derived = zeros(numTheta2*numTheta1PerTheta2,numPts);

Note that for each $\theta_2$, there are numel($\theta_1$) solutions.

for k=1:numTheta2
    theta_1_derived((k-1)*numTheta1PerTheta2+(1:numTheta1PerTheta2),:) = ...
        theta_1IKFunc(theta_2_derived(k,:),pxyzVal(1,:),pxyzVal(2,:),pxyzVal(3,:));
end

To better visualize the results, create the following table.

coordinates = pxyzVal';
theta_1_known = theta_1_known';
theta_2_known = theta_2_known';
theta_2_derived = theta_2_derived';
theta_1_for_theta_2_first_derived = theta_1_derived(1:2,:)';
theta_1_for_theta_2_second_derived = theta_1_derived(3:4,:)';
myTable = table(coordinates,theta_2_known,theta_1_known,theta_2_derived,...
    theta_1_for_theta_2_first_derived,theta_1_for_theta_2_second_derived)
myTable = 

                 coordinates                  theta_2_known    theta_1_known
    ______________________________________    _____________    _____________

    5.3585e+01    5.3585e+01    3.7105e+01     3.9270e-01      7.8540e-01   
    2.9869e+01    1.7245e+01    7.4537e+01    -2.6180e-01      5.2360e-01   
    6.1079e+01    2.5300e+01    5.1080e+01     1.9635e-01      3.9270e-01   
    2.8798e+01    9.3569e+00    7.6233e+01    -3.1416e-01      3.1416e-01   


         theta_2_derived          theta_1_for_theta_2_first_derived
    __________________________    _________________________________

    -1.7346e+00     3.9270e-01    -2.3562e+00     2.3562e+00       
    -1.0801e+00    -2.6180e-01    -2.6180e+00     2.6180e+00       
    -1.5383e+00     1.9635e-01    -2.7489e+00     2.7489e+00       
    -1.0278e+00    -3.1416e-01    -2.8274e+00     2.8274e+00       


    theta_1_for_theta_2_second_derived
    __________________________________

    7.8540e-01    -7.8540e-01         
    5.2360e-01    -5.2360e-01         
    3.9270e-01    -3.9270e-01         
    3.1416e-01    -3.1416e-01         

Note that theta_2_known matches one of the columns of theta_2_derived, and theta_1_known matches a column in [theta_1_for_theta_2_first_derived,theta_1_for_theta_2_second_derived].

References

  1. Aldebaran NAO robot
  2. Complete analytical inverse kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G., 2013 13th International Conference on Autonomous Robot Systems (Robotica), 2013.
  3. Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory by Kimberly Kendricks, PhD Thesis, 2007.
  4. getKinematicChain
type getKinematicChain;
% This function returns the kinematic chain of the Aldebaran NAO humanoid
% robot represented via Denavit-Hartenberg Parameters (DH) parameters. This
% work 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

function kinChain = getKinematicChain(link)
if nargin < 1
    link = 'head';
end
% Notation: A, R, and T are respectively the translation, rotation and
% general transformation matrices.
% DH parameters for the desired end configuration
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 px py pz real;
R = [r11 r12 r13;r21 r22 r23;r31 r32 r33];
kinChain.T = [R,[px;py;pz];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 01 link and is
        % an unknown
        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 01 link and is
        % an unknown
        syms theta_2 real;
        piby2 = sym('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 = sym('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