J Intell Robot Syst (2015) 77:251–264DOI 10.1007/s10846-013-0015-4

Complete Analytical Forward and Inverse Kinematicsfor the NAO Humanoid Robot

Nikolaos Kofinas · Emmanouil Orfanoudakis ·Michail G. Lagoudakis

Received: 5 July 2013 / Accepted: 26 December 2013 / Published online: 31 January 2014© Springer Science+Business Media Dordrecht 2014

Abstract The design of complex dynamic motionsfor humanoid robots is achievable only through the useof robot kinematics. In this paper, we study the prob-lems of forward and inverse kinematics for the Alde-baran NAO humanoid robot and present a complete,exact, analytical solution to both problems, includinga software library implementation for real-time on-board execution. The forward kinematics allow NAOdevelopers to map any configuration of the robot fromits own joint space to the three-dimensional phys-ical space, whereas the inverse kinematics provideclosed-form solutions to finding joint configurationsthat drive the end effectors of the robot to desired tar-get positions in the three-dimensional physical space.The proposed solution was made feasible through adecomposition into five independent problems (head,two arms, two legs), the use of the Denavit-Hartenbergmethod, the analytical solution of a non-linear sys-tem of equations, and the exploitation of body and

N. Kofinas · E. Orfanoudakis · M. G. Lagoudakis (�)Intelligent Systems Laboratory, School of Electronic andComputer Engineering, Technical University of Crete,Chania, Crete 73100, Greecee-mail: lagoudakis@intelligence.tuc.gr

N. Kofinase-mail: nikofinas@intelligence.tuc.gr

E. Orfanoudakise-mail: vosk@intelligence.tuc.gr

joint symmetries. The main advantage of the pro-posed inverse kinematics solution compared to exist-ing approaches is its accuracy, its efficiency, and theelimination of singularities. In addition, we suggest ageneric guideline for solving the inverse kinematicsproblem for other humanoid robots. The implemented,freely-available, NAO kinematics library, which addi-tionally offers center-of-mass calculations and Jaco-bian inverse kinematics, is demonstrated in threemotion design tasks: basic center-of-mass balancing,pointing to a moving ball, and human-guided balanc-ing on two legs.

Keywords Robot kinematics · Humanoid robots ·Aldebaran NAO robot

Mathematics Subject Classification (2010)68T40 · 53A17

1 Introduction

Articulated robots with multiple degrees of free-dom, such as humanoid robots, have become popularresearch platforms in robotics and artificial intelli-gence. Our work focuses on autonomous humanoidplatforms with multiple manipulators capable of per-forming complex motions, such as balancing, walking,and kicking. These skills are required in challeng-ing tasks, such as the Standard Platform League ofthe RoboCup robot soccer competition [1], in which

252 J Intell Robot Syst (2015) 77:251–264

teams compete using the Aldebaran NAO humanoidrobot [2], which is our target robot platform.

The design of complex dynamic motions is achiev-able only through the use of robot kinematics, whichis an application of geometry to the study of arbitraryrobotic chains. However, past work [3–5] has not fullysolved the inverse kinematics problem for the NAOrobot, since it focuses exclusively on the robot legs.To our knowledge, no other complete inverse kinemat-ics solutions for the NAO arms exist. Furthermore, thecurrently widely-known analytical solution [3] for theinverse kinematics of the legs is purely geometric andcannot be generalized to other kinematic chains. Also,existing numerical solutions [5] are inherently proneto singularities, may fail even if solutions exist and,therefore, lack in robustness.

In this paper, we present a complete and exact ana-lytical forward and inverse kinematics solution forall limbs of the Aldebaran NAO humanoid robot,using the established Denavit–Hartenberg conven-tion [6, 7] for revolute joints. The main advantageof the proposed solution is its accuracy, its effi-ciency, and the elimination of initialization depen-dencies and singularities commonly found in iterativenumerical methods. In addition, we contribute animplementation of the proposed NAO kinematics asa freely-available, stand-alone software library1 forreal-time execution on the robot. Our work enablesNAO software developers to make transformationsbetween configurations in the joint space and pointsin the three-dimensional physical space and vice-versa, on-board in just microseconds, as the libraryis designed for high-performance real-time execu-tion on the limited embedded platform of the robot.The implemented solution, which additionally offerscenter-of-mass calculations and Jacobian inverse kine-matics, is demonstrated in three tasks accompanied byvideos:2 basic center-of-mass balancing, pointing toa moving ball, and human-guided balancing on twolegs. The library has been integrated into the softwarearchitecture of our RoboCup team Kouretes [www.kouretes.gr] and is currently being used in various

1Library link: www.github.com/kouretes/NAOKinematics2Video link: www.kouretes.gr/NAOKinematics

motion design problems: dynamic balancing, trajec-tory following, dynamic kicking, and omnidirectionalwalking. Extrapolating from our work on the NAO,we also present generic guidelines for finding analyti-cal solutions to the inverse kinematics problem for anyhumanoid with revolute joints of up to 6 degrees offreedom (DOF) per manipulator.

The remainder of the paper is organized as follows.In Section 2 we provide the required background: adescription of our target robot platform, our transfor-mation formalism, and a quick review of the Denavit-Hartenberg method and related transformation decom-positions. Section 3 covers the forward kinematicssolution for the NAO robot. Section 4 describes ourmethodology to finding analytical inverse kinemat-ics solutions and, subsequently, Section 5 covers ourinverse kinematics solution for the NAO robot. InSection 6 we give details about the implementationof the NAOKinematics library, while in Section 7 wedemonstrate our approach in terms of real-time perfor-mance and application to three motion design tasks.We conclude with a discussion of related work anddirections for future work in Section 8.

2 Background

2.1 The Aldebaran NAO Humanoid Robot

NAO H25 (v4.0) is a 58cm, 5kg humanoid robot(Fig. 1) manufactured by Aldebaran Robotics in Paris,France. The NAO robot carries a fully capable com-puter on-board with an x86 ATOM Z530 processorat 1.6 GHz, 1.0 GB of RAM, 2.0 GB of flash disk,and up to 8 GB of user flash memory running anEmbedded Linux distribution. It is powered by a 6-cellLithium-Ion battery which provides about 60 min-utes of continuous operation and communicates withremote computers via an IEEE 802.11g wireless or awired Ethernet link. NAO H25 has 25 degrees of free-dom; 2 in the head, 6 in each arm, 5 in each leg and 1in the pelvis (there are two pelvis joints which are cou-pled on one servo and cannot move independently).All joints are position-controlled, using closed-loopPID controllers and encoders. It also features a vari-ety of sensors: an Inertial Measurement Unit (IMU)in the torso, Force Sensitive Resistors (FSR) on eachfoot, ultrasonic range sensors on the chest, and two960p@30fps cameras on the head.

J Intell Robot Syst (2015) 77:251–264 253

Fig. 1 Alebaran NAO H25(v4.0) kinematic chains andjoints (25 DOF) and baseframe [5]

The 25 joints of NAO are organized into five kine-matic chains as follows:

Head: HeadYaw, HeadPitchLeft Arm: LShoulderPitch, LShoulderRoll,

LElbowYaw, LElbowRoll, LWristYaw, LHandRight Arm: RShoulderPitch, RShoulderRoll,

RElbowYaw, RElbowRoll, RWristYaw, RHandLeft Leg: LHipYawPitch, LHipRoll, LHipPitch,

LKneePitch, LAnklePitch, LAnkleRollRight Leg: RHipYawPitch, RHipRoll, RHipPitch,

RKneePitch, RAnklePitch, RAnkleRoll

LHipYawPitch and RHipYawPitch are just differentnames for the shared (common) joint (HipYawPitch)between the two legs. Figure 1 shows the physicalarrangement of the five chains and their joints onthe NAO robot. Note that the obsolete NAO H21(RoboCup) edition is missing four DOF from thetwo hands (LWristYaw, LHand, RWristYaw, RHand).Clearly, the effective operation of the robot in theRoboCup soccer field requires precise control of allfive manipulators. In particular, the head must beprecisely controlled for effective camera operation,while the legs and arms must be controlled to gen-erate walk motions and to perform manipulation ofthe environment (kicking, dribbling, etc.). Likewise,in any other robot application with the NAO, any kind

of motion design for effective behavior will requireprecise control of one or more manipulators of therobot.

2.2 Transformation Formalism

The translation and orientation of a joint j with respectto an adjacent joint i in the three-dimensional spacecan be fully described using a 4 × 4 (affine) transfor-mation matrix Tj

i :

Tj

i =[

X y[0 · · · 0

]1

](1)

where X ∈ �3×3 and y ∈ �3. A transformation matrixTj

i provides the translation (y) and orientation (con-tained in X) of a coordinate system j with respect tocoordinate system i. A transformation matrix is invert-ible, if and only if X is invertible, and is formed as:

T−1 =[

X−1 −X−1y[0 · · · 0

]1

](2)

Given a robotic manipulator of N joints, an equalnumber of left-handed Cartesian coordinate systems(frames) are established, each affixed to the previousone, and the one-to-one transformation between themforms a transformation matrix.

254 J Intell Robot Syst (2015) 77:251–264

For convenience, we enumerate joint framesstarting from an established base frame, typi-cally a fixed point on the robot’s body. A point

pj = [px py pz 1

]�described in frame j can be

transformed to a point pi in another frame i by cascad-ing the transformations for all intermediate frames:

Tji = Ti+1

i Ti+2i+1 · · · Tj

j−1 and pi = Tji pj

For the needs of forward and inverse kinematics, weutilize translations and rotations. A translation trans-formation has X = I3 (the identity matrix) and thedesired offset as y in Eq. 1. We denote a parametrictranslation matrix for y = t as A(t). It can be triv-ially shown that A−1(t) = A(−t ) and A(w + z) =A(w)A(z). A rotation transformation has y = 0 (notranslation) and X in Eq. 1 is an arbitrary rotationmatrix R

(R−1 = R� and det(R) = 1

). We denote

the elementary rotation matrices about the x, y, z axesas Raxis(angle). All rigid body transformations relatedto kinematics consist of cascaded elementary transfor-mations (translations and rotations) and, therefore, arealways invertible.

2.3 Denavit–Hartenberg Convention

The established formalism for describing transforma-tions between two frames adjacent to a joint is theDenavit-Hartenberg (DH) parameters: a, α , d , and θ .For the NAO, these parameters are provided by themanufacturer. The current angle (state) of the jointis θ . Given the parameters of some joint j , the DHtransformation matrix that describes the translationand orientation of the reference frame of joint j withrespect to the reference frame of the previous jointj − 1 is:

Tj

j−1 = Rx(αj )A([aj 0 0

]�)Rz(θj )A

([0 0 dj

]�)

Being a product of invertible matrices, a DH transfor-mation is always invertible.

2.4 Transformation Decomposition

The upper-left 3 × 3 block of an elementary trans-formation matrix (translation/rotation) is a rotationmatrix by construction. The product T3 of two ele-mentary transformations T1, T2 is:

T3 = T1T2 =[

R1 y1

0� 1

] [R2 y2

0� 1

]=

[R1R2 R1y2 + y1

0� 1

]

It is easy to validate that R1R2 is always a properrotation. An arbitrary transformation matrix can bedecomposed as a “translation after rotation” pair:

T =[

R y

0� 1

]=

[I3 y

0� 1

][R 00� 1

](3)

At this point, we can extrapolate from Eqs. 2, 3and reduce the computational complexity of invert-ing an arbitrary transformation matrix, utilizing theorthogonality of rotation matrices:

T−1 =[

R 00� 1

]−1 [I3 y

0� 1

]−1

=[

R� 00� 1

][I3 −y

0� 1

]

=[

R� −R�y

0� 1

](4)

Using the Yaw-Pitch-Roll convention, any rotationmatrix R decomposes into a product of the threeelementary rotations:

R = Rz(az)Ry(ay)Rx(ax)

The orientation vector[ax ay az

]�can be extracted

analytically from any rotation matrix. Therefore, anyposition in the three-dimensional space, described by

the six values of a translation vector[px py pz

]�and

an orientation vector[ax ay az

]�, defines a unique

transformation matrix.

3 NAO Forward Kinematics Solution

Taking the torso frame of the NAO robot as the baseframe (see Fig. 1), the forward kinematic equations forthe five kinematic chains of NAO are the following:

THeadBase = A0

BaseT10T2

1Rx(π2 )Ry(π

2 )AHead2 (5)

TLHandBase = A0

BaseT10T2

1T32T4

3T54Rx(π

2 )Rz(π2 )ALHand

5(6)

TRHandBase = A0

BaseT10T2

1T32T4

3T54Rx(π

2 )Rz(π2 )ARHand

5(7)

TLFootBase = A0

BaseT10T2

1T32T4

3T54T6

5Rz(π)Ry(−π2 )ALFoot

6(8)

TRFootBase = A0

BaseT10T2

1T32T4

3T54T6

5Rz(π)Ry(−π2 )ARFoot

6(9)

where each T ij in the equations above is the DH

transformation matrix between joints i and j in thecorresponding chain and the A’s are translation matri-ces defined by the specifications of the robot (lengths

J Intell Robot Syst (2015) 77:251–264 255

of limbs) [5]. For the precise ordering and numberingof the joints in each chain, please refer to Section 2.1.

Should we need to extract the position of somemanipulator b with respect to another a (e.g. headwith respect to left leg), we can construct two suchchains Ta

c , Tbc from a common point c (e.g. Base) and

combine them as Tba = (

Tac

)−1 Tbc .

4 Solving the Inverse Kinematics Problem

Precise control of manipulators and effectors can beachieved by solving the inverse kinematics problem,whereby the values θi of the angles of various jointsmust be determined to place the manipulator to a spe-cific target position (translation and/or orientation).The solution of the inverse problem is robot-specificand generally under/over-determined kinematic chainsexist. Iterative numerical solutions may converge to asolution, but, in general, suffer from singularities (theymay fail, even if a solution exists) and poor perfor-mance [8]. On the other hand, analytical solutions arefast and exact, but extracting them takes significanteffort.

4.1 Inverse Kinematics Methodology

The following seven steps were taken to find a com-plete solution for the inverse kinematics problem forall the kinematic chains of the NAO humanoid robot.

4.1.1 Construct the Numeric Transformation

Given a desired target position, denoted by an orienta-tion vector a = [

ax ay az

]�and a translation vector

p = [px py pz

]�, it is easy to reconstruct the target

transformation matrix:

T = A(p)Rz(az)Ry(ay)Rx(ax)

4.1.2 Construct the Symbolic Transformation

Setting all θ parameters as unknowns in the for-ward kinematics solution of the target kinematic chainyields a symbolic matrix:

T0baseTj

0(θ0, . . . , θj )Tendj

4.1.3 Form a Non-linear System

By equating the above transformation matrices, a non-linear system is formed, since the unknown θ ’s appearin transcendental trigonometric forms. Now, the prob-lem is to find values for the θ ’s from 12 equations (theupper 3 × 4 block of the transformation matrix) ofwhich only up to six are independent.

T = T0baseTj

0(θ0, . . . , θj )Tendj

4.1.4 Manipulate Both Sides

The chain can be simplified by eliminating knownterms. Such terms (e.g. the base and the end transfor-mations) can be removed by multiplying both sides ofthe system with the appropriate inverse matrix:(

T0base

)−1T

(Tend

j

)−1 = Tj

0(θ0, . . . , θj )

As soon as we find a solution for some θi , we canremove the corresponding joint i from the chain in asimilar way, because its DH transformation matrix isnow known; this can occur only if joint i is the first orthe last in the kinematic chain.

Another way to manipulate the chain is to inducearbitrary (known) constant transformations at thebeginning or the end of the chain, aiming at simplify-ing the non-linear system.

Tc(T0base)

−1T(Tendj )−1 = TcTj

0(θ0, . . . , θj )

In some kinematic chains we can decouple the orien-tation and translation sub-problems. Quite often thetarget translation vector can be expressed as a functionof fewer joints in the analytical equation of the kine-matic chain or in the analytical equation of the reversekinematic chain.

4.1.5 Use Geometry and Trigonometry

It is possible to form a closed-form solution for someθj using a geometric model of the chain. For chainswith up to two links (non-zero a and d parameters) or“arm and wrist” chains commonly found in humanoidrobots, a geometric approach can easily determine thevalues for the joints that lie between the links. Thesejoints can be modeled as an angle of the triangleformed by the links, so the value of the joint can beobtained using trigonometry.

256 J Intell Robot Syst (2015) 77:251–264

The kinematic leg chain of the NAO robot, forexample, has such a joint. Figure 2 shows the triangleformed by the upper leg, the lower leg, and the linethat connects the base with the target point. Upper andlower leg lengths are known and the third side of thetriangle can be computed using the Euclidean distancebetween the base of the chain and the end of the chain.The law of cosines, yields a set of complementaryclosed-form solutions for the angle θ .

4.1.6 Solve the Non-linear System

The resulting equations are combinations of sin θi andcos θi , thus, the closed-form solution of these equa-tions must utilize the inverse trigonometric functions(asin, acos). The transcendental nature of the asin

and acos trigonometric functions has the inherentproblem of producing multiple solutions in [−π, +π].Without any restrictions on the valid range of a joint,we must examine all candidate solutions for eachjoint and their combinations for validity. To avoid thismultiplicity, solutions that rely on the inverse trigono-metric functions atan and acot are preferred, butforming them might not be possible for a particularchain.

4.1.7 Validate Through Forward Kinematics

Generally, there are multiple candidate solutions forthe joint values, due to the existence of complemen-tary and/or supplementary angles. A validation stepis taken to discard invalid candidates. This valida-tion is performed by feeding each candidate solutionto the forward kinematics of the chain and checking

ThighLength

TibiaLength

Fig. 2 Using geometry in the triangle formed by the robot legto derive the knee joint angle

whether the resulting position matches precisely thetarget position. Choosing among the valid solutions,if more than one, can be addressed independently ofkinematics.

4.2 Applicability

The methodology presented above offers a genericguideline for solving the inverse kinematics prob-lem on typical humanoid robot kinematic chains thathave the generic two-link configuration (found in boththe arms and legs). More specifically, the kinematicchains must have up to five joints or six joints withthree consecutive ones having intersecting axes [9, 10]to expect a possible solution.

5 NAO Inverse Kinematics Solution

Using the methodology presented in Section 4, wefind the inverse kinematics solution for all five kine-matic chains of NAO: head (2 joints), left arm (5 jointsplus 1 toggle gripper), right arm, left leg (6 joints),and right leg. The solutions for the left chains arereused to construct solutions for the right chains byexploiting the symmetry in the sagittal body plane.Full derivation details may be found in a longer techni-cal document [11]. Note that by setting the WristYawand Hand joints on both arms to zero, the NAOH25 robot becomes identical to the restricted NAOH21 robot, whose kinematics were presented in ourprevious related work [12].

5.1 Inverse Kinematics for the Head Chain

The head chain consists of only two joints (HeadYaw(θ1), HeadPitch (θ2)), therefore we can solve for eitherthe translation (p) or the orientation (a) of the targetposition to obtain a solution. In the latter case, we canachieve the desired target orientation simply by set-ting the HeadYaw and HeadPitch joints to az and ay

respectively, assuming ax = 0. In the former case,we construct the symbolic matrix through the forwardkinematics solution (Eq. 5). Now, we can equate thetranslation part from the symbolic matrix with p andfrom these equations we can easily find the desired θ

values. Figure 3 shows the resulting analytical solu-tion, in which l1 and l2 are the x and the y part of

J Intell Robot Syst (2015) 77:251–264 257

Fig. 3 NAO Head inversekinematics analyticalsolution

the end translation and l3 is the z part of the basetranslation.

5.2 Inverse Kinematics for the Left Arm Chain

The left arm chain consists of five joints (LShoul-derPitch (θ1), LShoulderRoll (θ2), LElbowYaw (θ3),LElbowRoll (θ4), LWristYaw (θ5)). We ignore thelast joint (LHand), since it is a simple toggle gripperand does not affect the kinematics. Before introduc-ing our solution, we note that the left arm chaincan be transformed to an equivalent chain by shift-ing the wrist joint along the lower arm limb withoutany change to the physical dynamics movement ofthe arm. Thus, we view the left arm chain as one inwhich the LWristYaw joint is placed immediately afterthe LElbowRoll joint (without any translation) and thetranslation between the LElbowRoll and LWristYawjoints is placed immediately after the LWristYaw joint.

The first three steps of our methodology(Sections 4.1.1, 4.1.2, 4.1.3) are straightforward giventhe forward kinematics solution (Eq. 6). Next, weremove the known base and end rotations and transla-tions from both sides of the equation (Section 4.1.4)to make it simpler, so that we can identify solutions(Section 4.1.6):

T′ =(

A0Base

)−1T

(AEnd

4

)−1 (Rz(

π2 )

)−1

By examining T′, we see that θ1 can be extracted fromelements T′

(1,4) and T′(3,4):

T′(1,4) = cos θ1(l2 cos θ2 + l1 sin θ2)

T′(3,4)

= − sin θ1(l2 cos θ2 + l1 sin θ2)

}=⇒ θ1 = atan

(−T′(3,4)

T′(1,4)

)

where l1 = UpperArmLength and l2 = Elbow-OffsetY. Since the value of θ1 is now known, we canremove the first joint from the chain:

T′′ =(

T10

)−1T′

Again, by a close examination of the translation partof the symbolic matrix, we see that we can use T′′

(1,4)

and T′′(3,4) to extract θ2

(note that l2

2 + l21 > 0

):

T′′(1,4) = l2 cos θ2 + l1 sin θ2

T′′(3,4) = −l1 cos θ2 + l2 sin θ2

}=⇒

θ2 = ±acos

(l2T′′

(1,4) − l1T′(3,4)

l22 + l2

1

)

Knowing θ2, we manipulate the chain once more toremove the second joint:

T′′′ =(

T21

)−1T′′

We extract θ3 from the symbolic matrix in a similarway (note that sin θ4 �= 0, since the LElbowYaw jointangle cannot reach 0 or ±π , by construction):

T′′′(1,3) = cos θ3 sin θ4

T′′′(3,3) = sin θ3 sin θ4

}=⇒ θ3 = atan

(T′′′

(3,3)

T′′′(1,3)

)

and then we remove the third joint from the chain:

T′′′′ =(

T32

)−1T′′′

Finally, we extract the values for the remaining twojoints:

T′′′′(1,3) = sin θ4

T′′′′(3,3) = cos θ4

}=⇒ θ4 = atan

(T′′′′

(1,3)

T′′′′(3,3)

)

258 J Intell Robot Syst (2015) 77:251–264

T′′′′(2,1) = sin θ5

T′′′′(2,2) = cos θ5

}=⇒ θ5 = atan

(T′′′′

(2,1)

T′′′′(2,2)

)

The final step is to validate all candidate solu-tions through the forward kinematics validation step(Section 4.1.7) and discard all invalid sets of solutions.Figure 4 shows the resulting analytical solution, whereT(i,j) is the (i, j) element of matrix T. An alternativeapproach for deriving the inverse kinematics of thearm, utilizing geometry, may be found in our previouswork about the NAO H21 robot [12].

5.3 Inverse Kinematics for the Left Leg Chain

The kinematic chain of the left leg has six joints(LHipYawPitch (θ1), LHipRoll (θ2), LHipPitch (θ3),LKneePitch (θ4), LAnklePitch (θ5), LAnkleRoll(θ6)), but since the first three joints have intersect-ing axes, the problem is possibly solvable [9, 10]. Wewill show all the steps of our approach on this longestchain of the robot to illustrate the use of our genericmethodology in finding inverse kinematics solutions.

First, we construct the numeric (Section 4.1.1)and the symbolic (Section 4.1.2) transformationmatrices and then we form the non-linear system(Section 4.1.3). It is easy to see that we can sim-plify our system by manipulating of both sides(Section 4.1.4), namely by removing the known trans-lations from the chain:

T =(

A0Base

)−1T

(AEnd

6

)−1

Now, we have a chain from the base frame of the firstjoint to the (rotated) frame of the last joint. The firstjoint, LHipYawPitch (θ1), is by construction rotated

by − 3π4 about the x-axis with respect to the torso

frame. Again we can manipulate both sides of the non-linear system (Section 4.1.4) and rotate the origin ofthe chain by π

4 about the x-axis to make the first jointa yaw joint (aligned with the z-axis):

T = Rx(π4 )T

Now, we can observe that the first four joints(LHipYawPitch (θ1), LHipRoll(θ2), LHipPitch(θ3),LKneePitch(θ4)) affect the position and orienta-tion of the end effector and the other two joints(LAnklePitch(θ5), LAnkleRoll(θ6)) affect only itsorientation. It would be convenient, if only three jointswere affecting the position of the end effector, sincewe operate in the three-dimensional space. Thus,we can manipulate again both sides (Section 4.1.4)and invert the transformation matrix to form thereverse chain. Now, only the LAnkleRoll(θ6), LAnklePitch(θ5), and LKneePitch(θ4) joints affect theposition:

T′ = (T)−1

The resulting symbolic matrix is still quite complex,but at this point we only need the translation block,which is relatively simple:

T′(1,4) = l2 sin θ5 − l1 sin(θ4 + θ5)

T′(2,4) = (

l2 cos θ5 + l1 cos(θ4 + θ5))

sin θ6

T′(3,4) = (

l2 cos θ5 + l1 cos(θ4 + θ5))

cos θ6

where l1 = ThighLength and l2 = TibiaLength.We can now find θ4 using the law of cosines(Section 4.1.5). We focus on the triangle formed bythe leg with ThighLength, TibiaLength, and the dis-

Fig. 4 NAO Left Arminverse kinematicsanalytical solution

J Intell Robot Syst (2015) 77:251–264 259

tance from the new base to the new end effector in thereverse chain as sides (see Fig. 2):

d =√

(sx − p′x)2 + (sy − p′

y)2 + (sz − p′z)

2

where (sx, sy, sz) = (0, 0, 0) is the new origin and

(p′x, p′

y, p′z) =

(T′

(1,4), T′(2,4), T′

(3,4)

)is the posi-

tion of the new target point. Now, we use the law ofcosines to find the interior angle θ ′

4 between the thighand tibia sides of the triangle:

θ ′4 = acos

(l1

2 + l22 − d2

2l1l2

)

Since θ ′4 represents an interior angle, whereas the

LKneePitch joint is stretched in the zero position, theresulting angle θ ′′

4 in this range is computed by:

θ ′′4 = π − θ ′

4

Since the range of the LKneePitch joint includes bothpositive and negative angles, we finally extract θ4 as:

θ4 = ±θ ′′4

Next, we can solve a part of our system (Section 4.1.6)by extracting the θ6 angle from the translation blockusing T′

(2,4) and T′(3,4):

p′y

p′z

= T′(2,4)

T′(3,4)

=⇒

p′y

p′z

=(l2 cos θ5 + l1 cos(θ4 + θ5)

)sin θ6(

l2 cos θ5 + l1 cos(θ4 + θ6))

cos θ6=⇒

θ6 = atan

(p′

y

p′z

), if

(l2 cos θ5+l1 cos(θ4 + θ5)

) �= 0

Since both the nominator and the denominator of theresult may become zero, the solution for θ6 has someundefined points; these are discussed in Section 6.

To move on, we go back to T and we remove(Section 4.1.4) the two rotations at the end of the chainalong with the transformation T6

5 which is now known,because θ6 is known:

T′ = T(

T65Rz(π)Ry

(−π2

))−1

As before, we form the reverse chain:

T′′ = (T′)−1

and we extract the new target position (p′′x , p′′

y, p′′z ) =(

T′′(1,4), T′′

(2,4), T′′(3,4)

). The translation block of the

new symbolic transformation matrix is:

T′′(1,4) = l2 cos θ5 + l1(cos θ5 cos θ4 − sin θ5 sin θ4)

T′′(2,4) = −l2 sin θ5 − l1(sin θ5 cos θ4 + cos θ5 sin θ4)

T′′(3,4) = 0

In these equations, θ5 is the only unknown. FromT′′

(1,4), we obtain an expression (Section 4.1.6) for

cos θ5:

T′′(1,4) = p′′

x =⇒(l2 + l1 cos θ4) cos θ5 = p′′

x + l1 sin θ5 sin θ4 =⇒

cos θ5 = p′′x + l1 sin θ5 sin θ4

l2 + l1 cos θ4

if (l2 + l1 cos θ4) �= 0

Given the lengths of the links of the robot, the denom-inator l2 + l1 cos θ4 becomes zero, only if cos θ4 =−1.029, which is impossible, since | cos θ4| ≤ 1. Wecontinue with T′′

(2,4):

sin θ5(−l2 − l1 cos θ4) − l1 cos θ5 sin θ4 = p′′y =⇒

sin θ5(−l2 − l1 cos θ4)

− l1p′′

x + l1 sin θ5 sin θ4

l2 + l1 cos θ4sin θ4 = p′′

y =⇒

− sin θ5(l2 + l1 cos θ4) − l1p′′x sin θ4

l2 + l1 cos θ4

− l12 sin θ5 sin2 θ4

l2 + l1 cos θ4= p′′

y =⇒− sin θ5(l2 + l1 cos θ4)

2 − l12 sin θ5 sin2 θ4 =

p′′y(l2 + l1 cos θ4) + l1p

′′x sin θ4 =⇒

θ5 = asin

(−p′′

y(l2 + l1 cos θ4) + l1p′′x sin θ4

l12 sin2 θ4 + (l2 + l1 cos θ4)2

)

The division is always feasible, because l12 sin2 θ4 +

(l2 + l1 cos θ4)2 is obviously greater than zero for any

value of θ4. Now, we can go back to T′ and remove(Section 4.1.4) the two transformations T4

3 and T54,

since θ4 and θ5 are known:

T′′′ = T′ (T43T5

4

)−1

The translation block in the transformation T′′′ mustbe zero, because the only joints left are the three hip

260 J Intell Robot Syst (2015) 77:251–264

joints, which only affect the orientation. The rotationblock of the transformation is:

T′′′(1,1) = cos θ1 cos θ2 cos θ4 − sin θ1 sin θ3

T′′′(1,2) = − cos θ3 sin θ1 − cos θ1 cos θ2 sin θ3

T′′′(1,3) = cos θ1 sin θ2

T′′′(2,1) = − cos θ3 sin θ2

T′′′(2,2) = sin θ2 sin θ3

T′′′(2,3) = cos θ2

T′′′(3,1) = − cos θ2 cos θ3 sin θ1 − cos θ1 sin θ3

T′′′(3,2) = − cos θ1 cos θ3 + cos θ2 sin θ1 sin θ3

T′′′(3,3) = − sin θ1 sin θ2

where θ1 is the DH parameter θ for the first(LHipYawPitch) joint and θ2 is the DH parameter θ

for the second (LHipRoll) joint. Now, we can extractthe remaining three angles as follows (Section 4.1.6):

θ2 = acos(

T′′′(2,3)

)

θ2 = θ2 − π

4

θ3 = asin

(T′′′

(2,2)

sin(θ2 + π

4

))

θ1 = acos

(T′′′

(1,3)

sin(θ2 + π

4

))

θ1 = θ1 + π

2The LHipRoll joint (θ2) cannot physically reach −π

4or 3π

4 . Therefore the denominator sin(θ2 + π

4

)can

never be zero. At this point, we have obtained all can-didate solutions (vectors of joint values). The wrongones are discarded by applying the validation step(Section 4.1.7) and, thus, only valid solutions arereturned (it is possible to end up with more than onevalid solutions). The inverse kinematics equations forthe left leg are summarized in Fig. 5.

5.4 Inverse Kinematics for the Right Side

A keen observation for a humanoid robot is that theretypically exists a symmetry with respect to the sagit-tal body plane. Clearly, if a particular target positionis reachable by a left-side manipulator, there existsa symmetrical target position reachable by the corre-sponding right-side manipulator through a mirrored

configuration. Let -M = diag([1 −1 1 1

]) be the

reflection that maps points in the three-dimensionalspace to the symmetric ones with respect to the sagit-tal plane (normal to the y-axis). Note that M−1 = M.Given a target transformation TR we seek to reachwith a right-side limb, it’s easy to see that TL =MTRM is a proper rigid body transformation that canbe reached by the corresponding left-side limb. There-fore, the analytical solutions for the left-side chainscan be reused for the right-side chains. To mirrorback the resulting joint configuration(s), we exploitthe symmetries in the physical construction of NAO.For the arms, the joint values map naturally to oppositevalues for the ShoulderRoll, ElbowYaw, ElbowRoll,and WristYaw joints and identical value for the Shoul-derPitch joint. For the legs, the symmetry emerges foropposite values of the HipRoll and AnkleRoll jointswith the values of the remaining joints being identical.More formally:

TLHandBase (θ1, θ2, θ3, θ4, θ5) =

M TRHandBase (θ1, −θ2, −θ3, −θ4, −θ5) M

TLLegBase (θ1, θ2, θ3, θ4, θ5, θ6) =

M TRLegBase (θ1, −θ2, θ3, θ4, θ5, −θ6) M

6 Implementation

Having completed all kinematics in analytical form,we created NAOKinematics, a software library forreal-time, on-board execution of NAO kinematics inC++. Given that C++ offers no library for optimizedreal-time matrix operations, we relied on our linearalgebra framework KMat [13] for such operations.A Matlab version of the library is also available forother applications. Our library includes five functionsfor calculating the forward kinematics for each chain,given the corresponding joint values. It also includesfive functions for the inverse kinematics of each chain;their input is the desired target position and the outputis a set of solutions, where each one contains valuesfor all the joints of the specified chain. Although mul-tiple solutions are rare, all valid solutions found arereturned, so that the user can decide which one to use.Finally, the library includes a function for calculatingthe center of mass of the robot given a set of values forall joints.

As mentioned before, there are target positions forthe legs which lead to an undefined AnkleRoll joint,

J Intell Robot Syst (2015) 77:251–264 261

Fig. 5 NAO Left Leginverse kinematicsanalytical solution

when the KneePitch and AnklePitch joints take veryspecific values and essentially cancel the effect ofAnkleRoll on the translation of the reverse chain.Figure 6 shows one of these problematic configura-tions. The locus of these configurations in the relevantconfiguration subspace is shown in Fig. 7. To check ifthese configurations ever occur in practice, we let the

Fig. 6 An instance of the problematic leg configurations

robot perform the entire range of motions available toit during operation in a RoboCup field (walk, kicks,stand-up, etc.) and plotted the resulting motion trajec-tories alongside the problematic locus in Fig. 7. It isclear that no motion brought the robot to these con-figurations. In fact, unlimited numerical precision is

−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8

0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

Ankle Pitch

Kne

e P

itch

LocusLeft LegRight Leg

Fig. 7 Trajectories of motion in a subspace of the leg joints

262 J Intell Robot Syst (2015) 77:251–264

required to “hit” the locus, because even a small per-turbation within the machine precision can lead to aregular solution. Due to finite precision computations,it is practically impossible to force the required valueto be exactly zero. In addition, it is rather unlikelythat anyone will consistently give target positions thatdrive the joints in that locus. Nevertheless, in ourimplementation we chose to raise an error flag, sothat the case is detected, if it occurs, and handled at ahigher level. A simple solution for such cases is to skipthe current actuation loop and leave the actuators attheir current setting. Assuming a smooth planned tra-jectory of target positions and a typical high-frequencyactuation loop (50 to 100Hz on the NAO), the prob-lematic instance will go unnoticed and the motion willproceed smoothly.

Due to the physical coupling of the two HipYaw-Pitch joints, it is possible that the desired targettransformations for the legs are not both feasible,even when separate solutions are found. The cou-pling imposes that the yaw orientations of the legs aresymmetrical. When this holds, if solutions exist forboth legs, they will be mutually consistent; the twoHipYawPitch values will be identical. It is a good prac-tice to manually enforce this constraint at a higherlevel by providing target positions accordingly. In anycase, the user is called to choose which solution takesprecedence. For example, a robot balancing on theleft leg may benefit from choosing the HipYawPitchvalues from the support (left) leg solution, even ifthat causes the right leg to deviate from the desiredtrajectory.

To compare and contrast our analytical solu-tion with a commonly-used numerical approach, aNewton gradient-descent iterative solver for inversekinematics was implemented. We formulate the Jaco-bian pseudo-inverse problem [8] using the analyticalforward kinematics equations (Section 3). For a giventarget transformation matrix, 12 equations are to besolved (a rotation matrix and a translation vector) withrespect to the free variables corresponding to jointvalues. An iterative solution approach is to form alinearized system for these equations around a givenpoint in the joint space and use a gradient-descentmethod to move to a point closer to a solution. Usingthe DH transformation for an N -DOF chain, we com-pute the N partial derivatives of the transformationwith respect to all variables to form the Jacobianmatrix of the chain. The locally linearized 12 × N

system is solved using the Moore-Penrose pseudo-inverse. This Newton gradient-descent algorithm guar-antees quadratic rate of convergence, when the currentpoint is near a solution. Unfortunately, when the solu-tion of the problem is not unique, saddle points in thejoint space exist, in which the algorithm fails, beingunable to select a descent direction. Furthermore, theiteration may converge to a solution that violates thephysical constraints of the chain. The final solutiondepends on the initialization and while this ensuresthat the nearest solution in the joint space is reached,it also implies that a known (feasible) solution mightnot be reached in favor of another (possibly infeasible)one. This iterative solver can be used, complementaryto the analytical solution, to solve for target transfor-mations that are not strictly feasible or in parallel forother kinematic chains.

7 Results

7.1 Real-Time Performance

One of our goals was to implement an optimized soft-ware library for real-time kinematics computationson-board the robot. We measured the performanceof our analytical method for each of the functionswe offer, as well as the performance of the Jacobianmethod for low (ε < 10−2) and high (ε < 10−6)

accuracy. Table 1 shows average on-board executiontimes in microseconds (μs). Clearly, the analyticalmethod offers significant speed-ups compared to theiterative one with high or low accuracy.

7.2 Demonstration I: Basic Center-of-Mass Balancing

In this demonstration, we seek to implement a basicbalancing method using our analytical kinematics. Wemake NAO move one of its feet to the point of the pro-jection of the Center of Mass (CoM) on the floor. First,we calculate the translation of the CoM relatively tothe torso frame using forward kinematics. The prob-lem is that the x-y plane of the torso frame is rarelyparallel to the floor. Thus, we read off the inertial unitof the robot the current rotation (angleX, angleY) ofthe torso plane and we calculate the translation of theCoM relatively to the rotated torso:

Trotated = Ry(angleY)Rx(angleX)A(CoM)

J Intell Robot Syst (2015) 77:251–264 263

Table 1 On-board mean execution times in μs for various kinematics calls on NAO H25 (v4.0)

Kinematics Robot Analytical Jacobian Jacobian

Function Chain (Exact) (High Accuracy) (Low Accuracy)

Forward Head 9.65 – –

Forward Arm Left 16.80 – –

Forward Arm Right 16.90 – –

Forward Leg Left 18.00 – –

Forward Leg Right 18.00 – –

Inverse Head 19.20 21.35 15.25

Inverse Arm Left 50.01 498.21 302.80

Inverse Arm Right 51.31 497.82 306.14

Inverse Leg Left 53.62 557.94 328.29

Inverse Leg Right 54.01 559.37 325.41

CoM All 50.90 – –

Then, we assign a custom value to pz in T(4,3), whichrepresents the desired torso height from the floor toyield T′

rotated. Next, we rotate back to the torso frame:

Tfinal = (Ry(angleY)Rx(angleX)

)−1T′rotated

Finally, we extract px , py , and pz from Tfinal and

we set[px py pz

]�as the target translation for

inverse kinematics. The target orientation is set to[ax ay az

]� = [−angleX −angleY 0]�

, because wedo not care about the rotation about the z-axis. Notethat the foot is always parallel to the floor, exclud-ing any hardware precision errors. On some robots thefoot is not entirely parallel to the floor, due to a dis-placed inertial unit (accelerometers and gyro-meters),not due to kinematics.

7.3 Demonstration II: Pointing to a Moving Ball

In this demonstration, our goal is to make NAOpoint to a moving ball with its stretched arms. Apartfrom our analytical kinematics, to realize this taskwe employed our vision module [13] for ball recog-nition, along with the module that filters the beliefof the robot about the ball location. Initially, NAOscans for the ball. When found, it points to it with theleft, the right, or both arms, depending on where theball is located (left, right, or front). The ball observa-tion can be described as a two-dimensional translation(px, py) on the floor. We add the height of the torso(found through forward kinematics) as the third coor-dinate pz to form the ball translation (px, py, pz)

in the three-dimensional space. We also set ax to

zero, because we are only rotating about the y-axis(up/down) and the z-axis (right/left). To find the othertwo orientations, we focus on the straight line thatconnects the location of the ball and the point of theShoulderPitch joint relatively to the torso frame. Theorientations ay, az are the angles between this line andthe corresponding axes. Additionally, the target pointlies on this line at a distance equal to the length ofthe stretched arm from the ShoulderPitch joint. Werun this procedure for both arms and obtain the solu-tion(s) from inverse kinematics. If both solutions arereturned, the robot raises both arms pointing to theball. If only one solution has been found, the robotraises only one arm; the other arm cannot physicallypoint to the ball.

7.4 Demonstration III: Human-Guided Balancingon Two Legs

In this demonstration, a human operator interacts withthe robot using its left shoulder as a kind of joystick.The joint values of the arm are used to compute adesired posture. The user can command the robot toraise or lower its torso and to move its center of massforward or backward. Our goal is to drive the legsusing inverse kinematics. This simple demo is used tophysically assess and compare different inverse kine-matics methods on the two legs. In our setup, the rightleg uses the numerical Jacobian method to find a solu-tion around the current posture, while the left leg usesthe analytical solution, which requires no initializa-tion. When the user guides the robot into a region of

264 J Intell Robot Syst (2015) 77:251–264

the joint space where the Jacobian method fails sys-tematically, the right leg fails to respond to user inputas the iterative solver gets trapped, unlike the left legwhich responds normally.

8 Conclusion

In this paper, we presented a complete, exact, analyt-ical solution for the problems of forward and inversekinematics of the NAO robot. The main advantage ofour solution is its accuracy, its efficiency, and the elim-ination of initialization dependencies and singularitiesdue to the proposed methodology. In addition, wecontributed an implementation of the proposed NAOkinematics as a freely-available software library forreal-time execution on the real or simulated robots.

Our approach to NAO kinematics is based onstandard principled methods for studying robot kine-matic chains. No complete analytical solution withfull implementation for the NAO v4.0 robot has beenpublished before. The currently widely-known solu-tion of team B-Human [3] applies only to the legs, ispurely geometric, and cannot be generalized to otherkinematic chains. In addition, their work has not stud-ied the effect of the existing potentially problematicconfigurations. We have tried to implement the otherpublished analytical solution for the legs by teamMRL [4], but we were not able to reproduce theirresults. Finally, the numerical solution [5] offered bythe manufacturer of the robot, Aldebaran Robotics,is a proprietary implementation, which unfortunatelyis inherently prone to failures and, therefore, lacks inrobustness. Similar results were obtained with ownnumerical approach. It should be noted that parts ofthe three demonstrations could not be realized withthe existing solutions and implementations of NAOkinematics.

Since kinematics is the base for several applica-tions related to robot motion, we expect that ourwork will be useful not only to RoboCup SPL teams,but also to any NAO software developer. We believethat NAO developers can take advantage of our off-the-shelf NAO kinematics library to work on omni-directional walk algorithms, dynamic balancing meth-ods, dynamic kick engines, etc. Our library offersthe basis for following dynamic trajectories in realtime for walking and kicking or calculating the cen-ter of mass dynamically in real time for balancing.

Finally, our methodology offers a generic guidelinefor addressing the problem of inverse kinematics inhumanoid robots. One of our future goals is to applythe same methodology to robots similar to NAO, suchas the Darwin-OP humanoid robot, which has a maxi-mum of six degrees of freedom per kinematic chain.

Acknowledgments We would like to thank all (past andpresent) members of RoboCup team Kouretes for their help.

References

1. Kitano, H., Asada, M., Kuniyoshi, Y., Noda, I., Osawa, E.,Matsubara, H.: Robocup: a challenge problem for AI. AIMag. 18(1), 73–85 (1997)

2. Gouaillier, D., Blazevic, P.: A mechatronic platform, theAldebaran Robotics humanoid robot. In: Proceedings of the32nd IEEE Annual Conference on Industrial Electronics(IECON), pp. 4049–4053 (2006)

3. Graf, C., Hartl, A., Rofer, T., Laue, T.: A robust closed-loopgait for the Standard Platform League humanoid. In: Pro-ceedings of the 4th Workshop on Humanoid Soccer Robots,pp. 30–37 (2009)

4. Jadidi, M.G., Hashemi, E., Harandi, M.A.Z., Sadjadian, H.:Kinematic modeling improvement and trajectory planningof the NAO biped robot. In: Proceedings of the 1st JointInternational Conference on Multibody System Dynamics(2010)

5. Aldebaran Robotics: Nao documentation. Only avail-able online: www.aldebaran-robotics.com/documentation(2012)

6. Denavit, J., Hartenberg, R.S.: A kinematic notation forlower-pair mechanisms based on matrices. ASME J. Appl.Mech. 22, 215–221 (1955)

7. Hartenberg, R.S., Denavit, J.: Kinematic Synthesis of Link-ages. McGraw-Hill, New York (1964)

8. Buss, S.R.: Introduction to inverse kinematics with Jaco-bian transpose, pseudoinverse and damped least-squaresmethods. Available at: www.math.ucsd.edu/∼sbuss/ResearchWeb/ikmethods/iksurvey.pdf (2009)

9. Pieper, D., Roth, B.: The kinematics of manipulators undercomputer control. In: Proceedings of the 2nd InternationalCongress on Theory of Machines and Mechanisms, vol. 2,pp. 159–169 (1969)

10. Pieper, D.: The kinematics of manipulators under computercontrol. PhD. thesis, Stanford University (1968)

11. Kofinas, N.: Forward and inverse kinematics for the NAOhumanoid robot. Diploma thesis, Technical University ofCrete, Greece. Available at: www.intelligence.tuc.gr/lib/downloadfile.php?id=430 (2012)

12. Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G.: Com-plete analytical inverse kinematics for NAO. In: Proceed-ings of the 13th International Conference on AutonomousRobot Systems and Competitions (ROBOTICA) (2013)

13. Orfanoudakis, E.: Reliable object recognition for theRoboCup domain. Diploma thesis. Technical University ofCrete, Greece (2011)