Numerical Methods for Robotics -...

Numerical Methods for Robotics

Nicolas MansardLAAS

31077 Toulouse Cedex 4, FranceEmail: [email protected]

Contents

1 Outline of the class 9

I Inverse geometry 11

2 Special Euclidean Group 152.1 Euclidean space E3 . . . . . . . . . . . . . . . . . . . . . . . . . . 152.2 Rotation SO(3) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.2.1 Matrix representation . . . . . . . . . . . . . . . . . . . . 162.2.2 Angular velocities so(3) . . . . . . . . . . . . . . . . . . . 162.2.3 Rodrigues’s formula . . . . . . . . . . . . . . . . . . . . . 172.2.4 Canonical exponential coordinates . . . . . . . . . . . . . 18

2.3 General overview: standard Lie Group representation . . . . . . . 182.3.1 Differential manifold . . . . . . . . . . . . . . . . . . . . . 182.3.2 Tangent space . . . . . . . . . . . . . . . . . . . . . . . . 192.3.3 Lie group and algebra . . . . . . . . . . . . . . . . . . . . 19

2.4 The exponential map is not everything . . . . . . . . . . . . . . . 202.4.1 Euler angles . . . . . . . . . . . . . . . . . . . . . . . . . . 202.4.2 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . 202.4.3 Summary of rotation representations . . . . . . . . . . . . 21

2.5 Rigid displacement SE(3) . . . . . . . . . . . . . . . . . . . . . . 222.5.1 Homogeneous matrix representation . . . . . . . . . . . . 222.5.2 Pose-u-theta coordinates . . . . . . . . . . . . . . . . . . . 222.5.3 Exponential coordinates . . . . . . . . . . . . . . . . . . . 232.5.4 Movement of coordinates system in SE(3) . . . . . . . . . 24

2.6 A first taste of rigid velocities . . . . . . . . . . . . . . . . . . . . 24

3 Direct geometry 253.1 Kinematic tree . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.2 Bodies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3 Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.3.1 Revolute joint . . . . . . . . . . . . . . . . . . . . . . . . . 263.3.2 Prismatic joint . . . . . . . . . . . . . . . . . . . . . . . . 273.3.3 Free-flyer joint . . . . . . . . . . . . . . . . . . . . . . . . 27

3

4 CONTENTS

3.3.4 Other joints . . . . . . . . . . . . . . . . . . . . . . . . . . 273.4 Configuration space . . . . . . . . . . . . . . . . . . . . . . . . . 273.5 Direct geometry function . . . . . . . . . . . . . . . . . . . . . . 273.6 Denavit-Hartenberg parametrization . . . . . . . . . . . . . . . . 283.7 Workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4 Analytical inverse geometry 314.1 Overview of the possible methods . . . . . . . . . . . . . . . . . . 31

4.1.1 Substitution: geometrical view . . . . . . . . . . . . . . . 314.1.2 Substitution: algebraic view . . . . . . . . . . . . . . . . . 324.1.3 Alternative methods . . . . . . . . . . . . . . . . . . . . . 32

4.2 Geometric substitution . . . . . . . . . . . . . . . . . . . . . . . . 324.2.1 Simple planar 2R robot . . . . . . . . . . . . . . . . . . . 324.2.2 And the wrist? . . . . . . . . . . . . . . . . . . . . . . . . 33

5 Iterative optimization 355.1 Optimality condition . . . . . . . . . . . . . . . . . . . . . . . . . 365.2 Overview of the algorithms . . . . . . . . . . . . . . . . . . . . . 375.3 Descent direction . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.3.1 Gradient descent method . . . . . . . . . . . . . . . . . . 385.3.2 Newton method . . . . . . . . . . . . . . . . . . . . . . . 395.3.3 Quasi-Newton method . . . . . . . . . . . . . . . . . . . . 405.3.4 Gauss-Newton method . . . . . . . . . . . . . . . . . . . . 41

5.4 Modified direction and trust region . . . . . . . . . . . . . . . . . 425.4.1 Regularization . . . . . . . . . . . . . . . . . . . . . . . . 425.4.2 Trust region . . . . . . . . . . . . . . . . . . . . . . . . . . 425.4.3 Levenberg-Marquardt algorithm . . . . . . . . . . . . . . 43

5.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

6 Numerical inverse geometry 45

II Inverse kinematics 47

7 Direct kinematics 517.1 The kinematic twist . . . . . . . . . . . . . . . . . . . . . . . . . 51

7.1.1 Velocity vector field . . . . . . . . . . . . . . . . . . . . . 517.1.2 Instantaneous twist . . . . . . . . . . . . . . . . . . . . . . 527.1.3 Twist actions . . . . . . . . . . . . . . . . . . . . . . . . . 527.1.4 Velocity vector field . . . . . . . . . . . . . . . . . . . . . 537.1.5 Twist sum . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

7.2 From the kinematic tree to the kinematic direct map . . . . . . . 547.2.1 Joint Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 547.2.2 Examples of joint . . . . . . . . . . . . . . . . . . . . . . . 547.2.3 Robot direct kinematics . . . . . . . . . . . . . . . . . . . 557.2.4 Kinematics map . . . . . . . . . . . . . . . . . . . . . . . 55

CONTENTS 5

7.2.5 Robot Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 557.2.6 Elements of algorithm . . . . . . . . . . . . . . . . . . . . 55

8 The pseudo inverse 578.1 Moore-Penrose . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

8.1.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 578.1.2 Case of invertible matrices . . . . . . . . . . . . . . . . . . 588.1.3 Proof of unicity . . . . . . . . . . . . . . . . . . . . . . . . 58

8.2 Singular-value decomposition . . . . . . . . . . . . . . . . . . . . 588.2.1 Definition from the Eigen decomposition . . . . . . . . . . 588.2.2 Constructive proof of existence of the pseudo inverse . . . 59

8.3 Some properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . 608.3.1 Range, kernel and projectors . . . . . . . . . . . . . . . . 608.3.2 Rank, full rankness . . . . . . . . . . . . . . . . . . . . . . 608.3.3 Construction from the square . . . . . . . . . . . . . . . . 60

8.4 Other generalized inverses . . . . . . . . . . . . . . . . . . . . . . 618.5 Unconstrained quadratic programing . . . . . . . . . . . . . . . . 61

8.5.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 628.5.2 Least-square inverse . . . . . . . . . . . . . . . . . . . . . 628.5.3 Elements of calcul . . . . . . . . . . . . . . . . . . . . . . 638.5.4 Regularization . . . . . . . . . . . . . . . . . . . . . . . . 65

9 Resolution of the inverse-kinematics problem 679.1 Inverting the Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 679.2 Task function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

9.2.1 Formal definition . . . . . . . . . . . . . . . . . . . . . . . 679.2.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

10 Redundancy 6910.1 Projection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

10.1.1 Projected gradient . . . . . . . . . . . . . . . . . . . . . . 6910.1.2 Second task . . . . . . . . . . . . . . . . . . . . . . . . . . 6910.1.3 Stack of tasks . . . . . . . . . . . . . . . . . . . . . . . . . 6910.1.4 Regularization . . . . . . . . . . . . . . . . . . . . . . . . 69

10.2 Weighted inverse . . . . . . . . . . . . . . . . . . . . . . . . . . . 6910.2.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 6910.2.2 Resolution . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

11 Active-set search 71

III Inverse dynamics 73

12 Overview by Andrea Del Prete 7512.1 Motivations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7512.2 Task function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

6 CONTENTS

12.3 Forward model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

12.3.1 Robot dynamics overview . . . . . . . . . . . . . . . . . . 76

12.3.2 Deriviation . . . . . . . . . . . . . . . . . . . . . . . . . . 76

12.4 Inverse dynamics control . . . . . . . . . . . . . . . . . . . . . . . 78

12.4.1 First control law: computed torque . . . . . . . . . . . . . 78

12.4.2 Quadratic program . . . . . . . . . . . . . . . . . . . . . . 78

12.5 Mobile robot in contact . . . . . . . . . . . . . . . . . . . . . . . 79

12.5.1 Case of the underactuation . . . . . . . . . . . . . . . . . 79

12.5.2 Contact forces . . . . . . . . . . . . . . . . . . . . . . . . 79

12.6 Operational space inverse dynamics (OSID) . . . . . . . . . . . . 80

12.6.1 Task-space acceleration . . . . . . . . . . . . . . . . . . . 80

12.6.2 OSID problem . . . . . . . . . . . . . . . . . . . . . . . . 80

12.6.3 Constrained quadratic problem . . . . . . . . . . . . . . . 80

12.6.4 OSID with contact . . . . . . . . . . . . . . . . . . . . . . 81

12.6.5 Elements of computations . . . . . . . . . . . . . . . . . . 82

13 Geometry of forces and accelerations 83

14 The dynamic equation of a multi-body system 85

14.1 Recursive Newton-Euler algorithm . . . . . . . . . . . . . . . . . 85

15 Operational-space inverse dynamics 87

16 Synthesis: an overview of constrained optimization 89

IV Optimal control 91

17 Problem formulation and properties 95

17.1 Definition of the problem . . . . . . . . . . . . . . . . . . . . . . 95

17.1.1 Robot dynamics . . . . . . . . . . . . . . . . . . . . . . . 95

17.1.2 Cost functionnal . . . . . . . . . . . . . . . . . . . . . . . 96

17.1.3 Optimal control formulation . . . . . . . . . . . . . . . . . 97

17.2 Trajectory optimization: Boundary-value problem . . . . . . . . 98

17.2.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . 98

17.2.2 Shooting resolution . . . . . . . . . . . . . . . . . . . . . . 98

17.2.3 Numerical integration methods . . . . . . . . . . . . . . . 98

17.2.4 Multiple-shooting resolution . . . . . . . . . . . . . . . . . 98

17.3 Infinite-dimension optimization: calculus of variation . . . . . . 98

17.4 Optimality conditions . . . . . . . . . . . . . . . . . . . . . . . . 98

17.4.1 Bellman principle . . . . . . . . . . . . . . . . . . . . . . . 98

17.4.2 Equation of Hamilton-Jacobi-Bellman . . . . . . . . . . . 99

17.4.3 Pontryagin Maximum Principle . . . . . . . . . . . . . . . 100

17.4.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

17.5 Indirect methods . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

CONTENTS 7

18 Direct resolution 10118.1 Discretization and non-linear finite-dimension optimization . . . 10118.2 Explicit versus implict formulation . . . . . . . . . . . . . . . . . 10118.3 Direct multiple shooting . . . . . . . . . . . . . . . . . . . . . . . 10118.4 Linear-quadratic optimization . . . . . . . . . . . . . . . . . . . . 101

18.4.1 Gauss-Newton hypothesis . . . . . . . . . . . . . . . . . . 101

19 The example of the robot walk 103

8 CONTENTS

Chapter 1

Outline of the class

The objective of the class is to provide numerical methods to express, computeand reason about a motion to be executed by a physical platform. In particular,we will deal with representation of the motion, numerical resolution of algebraicequations and automatic numerical methods to approximate the solution toequality problems or minimization problems.

We will start with the static problem of finding one configuration answeringto a set of constraints, typically finding a configuration such that the end effectorof the robot is at a given position and orientation (chap 1: inverse geometry).This problem does not comprehend any concept of trajectory: only the finalconfiguration is considered. In Chap. 2, we will consider the problem of findingthe configuration velocity that brings the system closer to a given goal. Thisproblem, named inverse kinematics, leads by integration to the resolution of theinverse geometry problem, and will be shown to be of a much simpler class ofdifficulty. Coming from geometry then kinematics, we will consider the systemdynamics in the third chapter. In particular, we will present the inverse dynam-ics problem as an increment of the inverse kinematics problem. Finally, the lastchapter we really consider the robot trajectory from an initial position to thefinal goal as a single object, and exhibit the methods to approximate the systemoptimality on the full trajectory. Compared to the initial static optimizationproblem, this last optimal control problem is of a more complex class, but canbe approximate to a static problem to the cost of a increase of dimensionality.

Each part of the class will be an oportunity to visit a general class of numer-ical resolution methods. In Chapter 1, we will introduce the Newton iterativeoptimization method. In Chapter 2, the pseudo-inverse will be the key tool.Chapter 3 we be the occasion to consider the optimization under constraints.Finally, in Chapter 4, we will quickly recall the optimal-control framework andconsider more in detail the linear-quadratic regulator (LQR).

9

10 CHAPTER 1. OUTLINE OF THE CLASS

Part I

Inverse geometry

11

13

We mainly consider in the part open kinematic chain, that is to say a tree ofrigid bodies attached two by two by joints. Recall that the configuration spaceof the robot is a representation (desirably minimal) that uniquely defined theposition of all the bodies of the robot. Before discussing the robot geometry, it isimportant to introduce the concept to manipulate a body in space. The correctstructure to do so is the special Euclidean group SE(3), which is described inSection 2.

The direct geometry function1 of the robot is the function that map oneconfiguration of the robot to the corresponding placement (position and orien-tation) of the robot in the space. For open-chain robots, this function is veryeasy to compute. Performing an action with the robot often comes to findingone configuration where the end-effector is at the right place with the right ori-entation. Finding such a configuration is called the inverse geometry problem2.

In the general case, the inverse geometry problem is written as an alge-braic problem, and solving it reduces to finding the roots (possibly several ora manifold of them) of a polynomials of several variables (one per degrees offreedom). In some particular cases, algebraic solutions exists to automaticallyor semi-automatically compute exact solutions or nearly exact solution to thisproblems. The most studied particular case has been the non-redundant ma-nipulator with six degrees of freedom: in that case, we have the same numberof constraints and variables. These methods are described in Section 4.

Alternatively, numerical methods can be used to solve the problem in thegeneral case, using descent methods. A basic introduction to the numericalmethods is given in Section 5, and their application to the inverse geometryproblem is given in Section 6.

1The map that gives the position of the end effector with respect to the robot configurationis often called direct kinematics. The kinematics being the branch of mechanics that studiesthe motion (κινηµα, motion in Greek) of sets of points, i.e. velocity, acceleration, we ratheruse direct geometry for the function h.

2This problem is often called inverse kinematics in the litterature.

Chapter 2

Special Euclidean Group

2.1 Euclidean space E3

We consider the three-dimensional Euclidean space E3. The Euclidean structureis totaly defined by selecting a representation E3, by selecting a coordinateframe, i.e. an origin O and three independant direction e1, e2, e3. In that case,any point p ∈ E3 can be identified with a vector p ∈ R3 which represents it:

p ∼= p =

xyz

∈ R3

where the symbol ∼= denotes the representation. In E3, a vector v is determinedby a pair of point a and b. Its coordinates are defined in R3 by

v ∼= v = b− a

where a,b are the representation of a and b in the chosen coordinate system.

The canonical basis on R3 implicitely defines a dot product < v|w >,vTw and a norm ||v|| = ||v||2 =

√< v|v > = v2

1 + v22 + v2

3 with v1, v2, v3 thecomponents of v (v = (v1, v2, v3)).

Coming from the coordinate system, the cross product operator between twovectors v, w is also defined by:

v × w =

v2w3 − v3w2

v3w1 − v1w3

v1w2 − v2w1

The cross product is an internal composition law of R3.

The transformations of E3 that preserves the dot and cross product are calledthe special Euclidean transformations (or rigid-body motion). The “special” at-tribute corresponds to the preservation of the cross product, i.e. the orientation.

15

16 CHAPTER 2. SPECIAL EUCLIDEAN GROUP

The preservation of the dot product directly induces the preservation of the dis-tances (both are equivalent using that < u|v >= 1

4 (||u+ v||2 − ||u− v||2)). Theset is denoted SE(3):

SE(3) ,{g : E3 → E3, so that ∀x, y ∈ E3, ||g(x)− g(y)|| = ||x− y||

}The set SE(3) equiped with the composition of application is a group (i.e.

the composition keeps the elements in the set, the 0 motion is the neural elementsand any transformation is revertible).

In the remaining, we will define some possible representations of SE(3), i.e.some vector spaces that surjectively maps SE(3).

2.2 Rotation SO(3)

2.2.1 Matrix representation

We start with the subset of SE(3) that keeps the origin fixed. This set is denotedSO(3) for special orthogonal group.

SO(3) ,{g : E3 → E3, so that ∀x, y ∈ E3, ||g(x)−g(y)|| = ||x−y|| and g(0) = 0

}It can be shown that any elements r of SO(3) is a linear application in R3: SO(3)is a subgroup of the group of linear applications on R3 denoted by GL(3). Anyelement of SO(3) can then be represented by its action on the canonical basis,i.e. its matrix in R3×3.

From the definition of SE(3), we can easily show that the transformation ofthe canonical basis is still orthonormal, i.e. that

∀i = 1..3, ||r(ei)|| = 1

∀i, j = 1..3, i 6= j,< r(ei)|r(ej) >= 0

This defines six constraints, that can be summarized by

RRT = I (2.1)

where R = [r(e1)r(e2)r(e3)] is the matrix representation of r. In addition, sincethe orientation (cross product) is preserved, we also have the positivity of thedeterminant, i.e. :

det(R) = +1

2.2.2 Angular velocities so(3)

Consider now a curve r(t) in SO(3) represented by R(t) in R3×3. By derivationof (2.1), we have that:

R(t)R(t)T = −(R(t)R(t)T )T

2.2. ROTATION SO(3) 17

Say otherwise, R(t)R(t)T is antisymmetric. The group of antisymmetric matri-ces is denoted so(3):

so(3) ,{ 0 −w3 w2

w3 0 −w1

−w2 w1 0

, (w1, w2, w3) ∈ R3}

The group so(3) is evidently isomorph to R3. We denote w ∈ R3 → w ∈ so(3)the isomorphism from R3 to so(3) and . its reciprocal. The vector w(t) can beidentified to the angular velocity corresponding to r(t).

Consider now the curve obtained by integration of a constant velocity w ∈so(3):

∀t > 0, R(t) = wR(t), R(0) ∈ SO(3)

The differential equation is uniquely integrable in R3 (theorem of Cauchy). Theexpression of the integrale is obtained using the exponential of w defined by itspower serie:

exp(tw) = etw =

+∞∑n=0

1

n!(tw)n

Using this definition, the integrale is:

R(t) = R(0)etw

It is easy to prove that the exponential satisfies the differential equation, andits uniqueness is ensured by the theorem of Cauchy.

From the differential construction, we obtain that etw is a rotation matrix.

This property can also be proved directly, by verifying that etwTetw = I using

the power serie, and by showing that det(etw) = 1 by continuity from det(e0) =1.

2.2.3 Rodrigues’s formula

The infinite power serie of the exponential map easily reduces to a simple formulausing the simple structure of so(3). Indeed, we have the two following properties:

w2 = wwT − I3 and w3 = −w

The development immediatly reduces to:

etw = I + sin(t)w +1− cos(t)

tw2

using the serie of sin(t) =∑ (−1)n

(2n+1)! t2n+1 and cos(t) =

∑ (−1)2

(2n)! t2n. This ex-

pression is named the Rodrigues’s formula.The exponential map is an application from so(3) to SO(3):

exp : w ∈ so(3)→ ew

The cases studied above is only decoupled expression, w being the direction andt the magnitude of the rotation. From the Rodrigues’s formulat, we directly seethat the exponential is not injective, since the norm can be chosen modulo 2π.


2.2.4 Canonical exponential coordinates

The exponential map from so(3) generates rotation matrices. It can be shownthat the map generates all the group SO(3), that is to say that it is surjective(onto). This is done by building the reciprocal, called the logarithm map. Beinggiven a rotation matrix R with rij its components, the logarithm is defined by:

log : R ∈ SO(3)→ w ∈ so(3)

||w|| = cos−1(trace(R)− 1

2),

1

||w||w =

1

2 sin(||w||)

r32 − r23

r13 − r31

r21 − r12

The surjectivity guarantees that any rotation matrix R can be generated by

taking the exponential of a vector of R3. The group SO(3) can therefore berepresented using R3. This is called a parametrization of the group. We callthis parametrization the canonical exponential coordinates.

Compared to the representation using rotation matrix, of dimension 9 butwith six constraints on the effect on the canonical basis, this representation isof dimension 3 without any constraint. It is therefore minimal.

Intuitively, the coordinates w = θu with θ = ||w|| and u = 1θw corresponds to

the integration of an unitary rotation velocity during a time θ, or said differentlyto a rotation of an angle θ around the direction u. It corresponds to the olderresults that any rotation of a solid in space can be brought back to a purerotation around a particular axis. The coordinates are also named “angle vector”or “u theta” for this reason.

2.3 General overview: standard Lie Group rep-resentation

The definition of the canonical coordinates seems pretty artificial, since we hadto introduce a complex map coming from an arbitrary differential equation. Infact, the process to obtain the parametrization is coming from the very topolog-ical structure of the group. The same process can be used for obtaining usefullparametrization on similar topologies. We make here a brief overview of thetopological tools that where used, in the general case.

2.3.1 Differential manifold

A differential manifoldM is a set that is locally diffeomorphic to a vector spaceRn, that is to say, in any point x ∈ M, there exists a local neighborhood U ofx with a diffeorphism ψ from U to a neighborhood of 0n in Rn. A collection ofU , φ covering M is called an atlas of M. Each φ is called a local coordinatessystem. We consider here only cases where all the atlas are equivalent, i.e. thediffemorphism are kept from one local coordinates system to the other.

Intuitivelly, a differentiable manifold is a set that locally behaves like Rn.

2.3. GENERAL OVERVIEW: STANDARD LIE GROUP REPRESENTATION19

2.3.2 Tangent space

On each point p of a differential manifold M we write C∞(p) the set of allsmooth function from any neighborhood of p into R. The tangent space Tp(M)is the subset of linear form over C∞(p) satisfying the “derivation” Leibniz rule,i.e. :

Tp(M) ,{

Xp : f ∈ C∞(p)→ R, (2.2)

∀f, g ∈ C∞(p), α ∈ R, Xp(αf + g) = αXp(f) +Xp(g) (2.3)

and Xp(fg) = Xp(f)g + fXp(g)}

(2.4)

Intuitively, the tangent space defines all the possible directions at which a tan-gent to a curve of M in p can pass.

The tangent space is a linear space. A basis can be built from the canonicalbasis (x1, ..., xn) of Rn using any local coordinates φ at p. The basis is denoted( ∂∂x1

, ..., ∂∂xn

).A vector field X is simply the association of a vector of the tangent space

to any point of the manifold:

X : p ∈M→ Xp ∈ TpM

2.3.3 Lie group and algebra

A Lie group G is a differential manifold with a smooth group operation andinverse. The neutral element of the group is denoted by 1G .In the case of spatial rotation, the Lie group is SO(3).

Using the group law, invariant vector fields can be produced by morphing avector of the tangent space at 1G by all the left composition Lg : h ∈ G → hg.Details can be found in [Murray 93]. Therefore, the tangent space at 1G isisomorphic to the set of invariant vector fields. An invariant vector field isdenoted by Xw with w ∈ 1G the generator.

This space T1G is called the standard Lie algebra associated with the Liegroup and denoted by g. It inherits its vector-space structure from the tangentspace, to which is added the algebraic structure coming from the Lie bracketoperation, inherited from the vector field set. This operation being anectodicfor this study, it is not described forward.In the case of spatial rotation, the Lie algebra is so(3).

From the standard Lie algebra, it is possible to define the exponential mapfrom g to a neighborhood of 1G . For any w ∈ T1G, let gw : t ∈ R → gw(t) ∈ Gdenotes the integral curve of the invariant vector field Xw generated by w andpassing through 1G at t=0. The exponential map of w is defined from g afterunitary integration by exp(w) = gw(1), or for any t:

exp(tw) = gw(t)

In the case of spatial rotation, the exponential map definitions fit.


The exponential map is a diffeomorphism from a neighborhood of 0g to aneighborhood of 1G . This defines a local coordinate system around 1G . Usingthe group law of mathcalG, this local coordinate system can be morphed to aproper atlas.

Moreover, if G is compact, the exponential map is surjective. In that case, theexponential map defines a global coordinate system, inherited from the vectorspace structure of g. It is called the canonical exponential coordinates.

2.4 The exponential map is not everything

Despite the topological origin, the exponential coordinates do not keep the topol-ogy of the initial space. In particular, the exponential map is not injective ingeneral, which means that several coordinates correspond to one same point.Typically for SO(3), the exponential coordinates are defined modulo 2π anddoes not keep the symmetry of the group with a singularity in the neighborg-hood of the null rotation I3 (the direction being degenerated when the angle isbecoming null).

2.4.1 Euler angles

Other representation of SO(3) exists. The first to be known are the Eulerangle, which corresponds to a sequence of three elementary rotations of angleai, i = 1..3, around three axes. Two types can be distinguished: the first whenthe rotations are performed around fixed axes. They can be written using theexponential map by:

r(a1, a2, a3) = ea1w1ea2w2ea3w3

where wi, i = 1..3 are three rotation axis. The second type correspond to therotations that are performed around axes that rotates with the transformation.In that case:

r(a1, a2, a3) = ea1w1+a2w2+a3w3

Note by the way that ew1+w2 6= ew1ew2 except in particular cases.The most used rotation are the roll-pitch-yaw. It corresponds to the rotation

of the first type, with w3 = (1, 0, 0) (roll, around the X axis), w2 = (0, 1, 0)(pitch, around the Y axis) and w1 = (0, 0, 1) (yaw, around the Z axis). Theseangles are much used in the aerospace industry.

2.4.2 Quaternions

The quaternion are a way to get rid of the singularity of the exponential co-ordinates in 0 and of their non surjectivity modulo 2π. Geometrically, thenormalized quaternions correspond to the sphere S3 which is a submanifold ofR3, while keeping the same topology than S2 for R2. They are defined from thecomplex numers C exactly like the complex numbers are defined from R using

2.4. THE EXPONENTIAL MAP IS NOT EVERYTHING 21

C = R+Ri, with i2 = −1. We define the imaginery number j such that j2 = −1and ij = −ji. By convenience, we denote by k = ij, that has the propertiesthat k2 = −1, jk = i and ki = j. The quaternion space H is then defined by:

H = C + Cj = R + Ri+ Rj + Rk

The quaternions are equiped with the multiplicative law of R. It is relativelyeasy to show that the inverse elements in H is:

q−1 ,q

||q||2

with the conjugate of the quaternion q = q0 + q1i + q2j + q3k being q , q0 −q1i−q2j−q3k and the norm being ||q||2 = qq, that is to say the Euclidean normof R4.

The quaternion space has many intersting property. The interest for theEuclidean motion representation is that it can embed the rotation group SO(3)in its unitary sphere S3:

S3 , {q ∈ H, ||q|| = 1}

The sphere S3 is trivially a subgroup of H. It is directly associated with so(3)while preserving the group structure of SO(3) using the simple map:

(θ, u) = (||w||, 1

||w||) ∼= r ∈ SO(3)→ q(r) = cos(

θ

2) + sin(

θ

2)(u1i+ u2j + u3k)

with u = (u1, u2, u3) the coordinates of u. It is possible thus teddious to verifythat the group structures of SO(3) and H fit.

The reciprocal in the proper subspace of so(3) is:

θ = 2cos−1(q0), u =

{0, θ = 0,

1sin(θ/2)q1:3 θ 6= 0

with q1:3 = (q1, q2, q3). The conditional equality depending of θ = 0 correspondsto the singularity of so(3) around the zero rotation. Moreover, we can noticethat both q and −q produce the same rotation. SO(3) in fact correspondstopologically of the half sphere or projection plane RP 3.

2.4.3 Summary of rotation representations

The quaternion representation is of dimension 4 with one constraint (||q|| = 1).It is therefore not minimal. However, it more adequatly captures the topologyof SO(3). In addition, it is equiped with a composition law corresponding to thecomposition of rotation, while being cheaper to compute numerically. Finally,the action of the linear map q ∼= r ∈ SO(3) on any v ∈ R3 can be computeddirectly from the quaternion using: r(v) = qvq where v = v1i + v2j + v3k. Inmany situations, the quaternions are the most adapted representation of SO(3).The exponential coordinates have to be chosen when minimality is crucial, whileR3×3 might be more interesting when many vector multiplications are computed(typically when rotating a cloud of thousand of points).


2.5 Rigid displacement SE(3)

We now come back to the initial SE(3) group, the set of all application in E3

that preserves the distance and the orientation. SE(3) can be shown to be thecomposition of a pure rotation r ∈ SO(3) and a pure translation t ∈ R3:

SE(3) = R3 × SO(3) = {(r, t), r ∈ SO(3), t ∈ R3}

2.5.1 Homogeneous matrix representation

Denoting by m ∼= (r, t) a rigid motion of SE(3), its application on a point p ∈ E3

is:m(p) = r(p) + t

The application m is therefore affine in R3. In can be transformed for convenientto a linear map to the cost of embeded it in the larger space GL(4) ∼= R4×4 by:

Mp =

[R t

01×3 1

] [p1

]where M ∈ R4×4 is the homogeneous representation of m ∈ SE(3), R ∈ R3×3

is the matrix representation of the rotation part of m, t ∈ R3 is the translationpart of m and p ∈ R3 is the vector representation of the point m is applied to.

The matrix product fits to the composition operation of SE(3), providingdirectly the group structure of SE(3):

• if m1,m2 ∈ SE(3), then m1 ◦m2 ∈ SE(3). Moreover, the law is associa-tive.

• the identity of SE(3) corresponds to I4.

• the inverge m−1 of m is represented by:

M−1 =

[RT −RT p0 1

]

2.5.2 Pose-u-theta coordinates

Like for SO(3), the matrix representation of SE(3) is convenient but not min-imal: 16 components are necessary but are subject to 7 constraints, three forthe R part and 4 for the last row (these 4 last components are indeed trivial).Moreover, it is more convenient to express the coordinates as a vector space Rn(n being intuitively equal to 6), where the neutral element is 0n and where thegroup topology is closer to R than GL(4).

Using the canonical exponential coordinates of SO(3), we can directly pro-pose the coordinates system R3 × so(3):

m = (r, p) ∼=[pw

]

2.5. RIGID DISPLACEMENT SE(3) 23

where r ∼= w is the exponential representation of the rotation part of m ∈ SE(3).Like so(3), this coordinates system has no group structure that fits the topologyof SE(3).

2.5.3 Exponential coordinates

This intuitive coordinates system is close to be the exponential coordinates ofSE(3) but is not exactly it. This means that their derivative does not fit withthe tangent space to SE(3), i.e. to the velocity of rigid bodies. To obtain theexponential coordinates of SE(3), the procedure is similar to the one performedin SO(3).

Let m(t) be a curve in SE(3). By analogy, consider the following matrix:

M(t)M(t)−1 =

[R(t)R(t)T p(t)− R(t)R(t)T p(t)

0 1

](2.5)

We denote by w(t) the angular velocity w(t) = R(t)R(t)T . The last columnsis then denoted by v(t) = p(t) + p(t) × w(t). The pair ν(t) = (v(t), w(t)) ∈se(3) = R3 × so(3) can be identified to the tangent vector to the curve m(t).It corresponds to the kinematic screw (“torseur cinematique” in French) of therigid body moving following m(t), expressed at the origin of the coordinatessystem of E3 in which m(t) is expressed. See next subsection for details.

By integration of a constant kinematic screw ν = (v, w) on [0, 1], the expo-nential map is obtained.

eν =

[ew 1

||w||

(wwT v + (I − ew)w × v

)0 1

], if ||w|| > 0[

I3 v0 1

], otherwise

Intuitively, the kinematic screw can be understood has representing a “screw”motion, i.e. a pure rotation of angle θ around a fixed axis in E3 followed by apure translation of length d in the direction of this same axis. Such a screw mo-tion is defined by six parameters (5 for the axis e.g. 3 for a point x on the axisand 2 for a direction (normalized vector u) plus 1 for the pitch i.e. the rationh = d/θ between translation and rotation, d = ∞ being a pure translation).Indeed, if we set:

u ,1

||w||w (2.6)

x , u× v (2.7)

h , wT v (2.8)

we obtain an equivalence (isomorphism) between the kinematic screw and thescrew motion. This isomorphism prooves by the way the theorem of Chaslesstating that any motion on a rigid body can be expressed as the motion of a


screw, for a uniquely defined screw (that may typically not be attached to therigid body but to an imaginary point). See [Murray 94] for a detailed discution.

The exponential map is sujective into SE(3). The inclusion of its imagein SE(3) is straightforward from its matrix structure. The covering is ensuredby the compactness of SE(3) but can also be constructively demonstrated bybuilding the logarithm map:

log : m = (r, p)→ (w, v)

with w = log(r), v = p when ||w|| = 0 and v = ||w||(wwT + (I − ew)w

)−1

p

otherwise.

2.5.4 Movement of coordinates system in SE(3)

Any rigid movement m ∈ SE(3) displaces a coordinates system of E3 (i.e. apoint O ∈ E3 and three orthonormal vectors of R3) into another coordinatessystem. Consider two coordinates systems of E3 denoted by their frames Faand Fb. The coordinates of a point p ∈ E3 are denoted by ap in Fa and by bpin Fb. We denote by amb

∼= aMb ∈ R4×4 the rigid motion displacing the frameFa into the frame Fb. In that case, we have:

ap = aMbbp

The notation follows the tensor notations.Consider now a fixed inertial frame Fo and a body frame Fb attached to a

moving rigid object, such that the two frames match at t = 0. The position ofany point p attached to the rigid body through the time can be described byonly giving the trajectory omb(t) in SE(3):

op(t) = oMb(t)pb

where pb = pb(0) are the coordinates of the fixed position of the point p in thebody frame.

We say the omb is the placement (i.e. position and orientation) of the bodyb in the coordinates system o.

2.6 A first taste of rigid velocities

Chapter 3

Direct geometry

In the previous section, we have seen how rigid displacements can be repre-sented. We now consider a more complex class of movements: the articulatedrigid motion, i.e. the considered system is composed of a finite set of rigid ele-ments, each of them moving subject to some given constraints imposed by thejoints linking them. We consider here only the case of open kinematic chain(properly describe below described) and will discuss only quickly the case ofkinematic loops later. Like for SE(3), we will discuss here the possible solutionto numerically represent the motion of such systems and to compute the motion(position, velocities, etc) of each points of the system.

The most difficult part of the study has been done with SE(3). This sectionis thus notably shorter.

3.1 Kinematic tree

We consider the graph structure describing the links between each rigid body:each node of the graph represents one rigid body and the edges of the graphrepresents the joints connecting two bodies together. A body can be connectedto several other bodies through several joints. It is not connected to itself andonly connected once to any other. Open kinematic chains are those for whichthere is no loops in the graph, i.e. it is possible to selec a tree structure for thekinematic graph. Simple kinematic chain are reduced to a trivial tree (only onebranch, each node has only one son and one father at maximum).

Two cases can be considered, that are mathematically the same but leadsto two different interpretations of the root: the robot can be attached to theground. In that case, the root of the tree, named the base link, is fixed. Orthe robot can be free to move. The root of the tree is then arbitrarily selected.We say that the robot has a free flyer. Both case are similar in the sense thata free-flyer robot can be seen as attached to a fixed “ground” body by a “free-flyer” joint; or alternatively a fixed robot is a free-flyer chain with the base bodyhaving infinite inertia.

25

26 CHAPTER 3. DIRECT GEOMETRY

In the following, we take the first convention: the base link is fixed, and afree-flyer robot is obtained by adding an artificial “free-flyer” link.

3.2 Bodies

Each body, represented by a node of the graph, is a rigid set of points. The bodyis associated with a body coordinates system, into which any point is describedby a constant vector of R3.

3.3 Joints

The joints are the moving pieces of the articulated rigid body. Each joint isattached two joints, one being the father i and the second the son i + 1. Ingeneral, the configuration qi of a joint is an element of a differentable manifoldQi that uniquely defines the placement of the son body in the coordinates systemof the father body:

Ki : qi ∈ Qi → amb(q) ∈ SE(3)

For convenience, we generally rather consider a class of joint, caracterized bytheir Ki : Qi → SE(3) function. The joint linking body i to body i+ 1 is thendefined by its placement in the system i along with its kinematic function Ki.

In all the relevant cases, Ki is a smooth function andQi a compact Lie group.It is therefore possible to associate a global vector representation to Qi and aJacobian matrix to the tangent application to Ki. The standard position of thejoint is given by the neutral element of the group 1Q, expecting K(1Q) = 1SE(3).

The relative placement of body i + 1 in the coordinate system of body i isthen given by:

qi ∈ Qi → imi+1(qi) = im0i+1 Ki(qi)

with im0i+1 = imi+1(1Q the placement of the joint in the coordinates system of

body i.

3.3.1 Revolute joint

The most classical joint is the revolute joint: one degree of rotation around afixed axis, typically the Z axis. The configuration space of the joint is then S1

the unitary circle. Most of the time, the joint angle is bounded by an upperand a lower joint limit. The configuration space is then an interval of R. Thekinematic function K is simply:

Ki : q ∈ S1 →

cos q − sin q 0 0sin q cos q 0 0

0 0 1 00 0 0 1

3.4. CONFIGURATION SPACE 27

3.3.2 Prismatic joint

Prismatic joint can translate (typically in an interval) along a given fixed axis,say Z. The configuration space is direcly R. The kinematic function K istrivially Ki(q) =

(I, (0, 0, q)

)∈ SO(3)× R3.

3.3.3 Free-flyer joint

Free-flyer joints are configured by SE(3) with their kinematic function beingthe identity on SE(3).

3.3.4 Other joints

Other joints can be found, even if very rarely used in robotics. Sperical jointsallow free and symetrical rotations. Their configuration space is SO(3), withtheir kinematics being r → (r, 03). Very few efficient mechanisms exist to actu-ate such a joint. A similar effect is rather used by a sequence three revolute jointswith concurent rotation axes. However, spherical joints are very often used formodeling purpose, for example for the model of the human body (typically, thehip joint), or for passive mechanisms.

The planar joint is a free 2D motion: two translations, one rotation. Its con-figuration space is SE(2). The cylindric and helicoidal joints are a compositionof on translation and one rotation with shared axis, the first one with free , thesecond with coupled rotation and translation motions.

3.4 Configuration space

The configuration space of the robot is typically the result of the Cartesianproduct of the configuration space of all the joints. Its reprensation is thecartesian product of the representation of the joint configuration spaces. Theorder of the terms in the Cartesian product only matters for the representation ofthe configuration space. It is purely arbitrary. In the case of a simple kinematicchain, the only relevant order is the one given by the chain.

Q = ×QiFor kinematic trees, the selected order should be compatible by the implicitepartial order defined by the tree. The order in which the branches are exploredis typically arbitrary and has to be documented.

When only minimal vector representation of the Qi are used, a minimalvector representation of Q is obtained. The dimension of the vector q is equalto the dimension of the manifold Q.

3.5 Direct geometry function

We called the end effector of the robot one of the body of arbitrary importance(typically the one carrying the tool). For all this part, we can consider only


simple chain without significant loss of generality. In that case, the end effectoris the last body of the chain.

The robot direct geometry function (as said upper, often named direct, orforward, kinematic function in the litterature) is the function that maps theconfiguration space to the placement of the end effector in SE(3):

K : q = (q0, ..., qn−1) ∈ Q →n−1∏i=0

im0i+1Ki(qi)

= 0m01K0(q0)1m0

2...n−2m0

n−1Kn−1(qn−1)

The direct geometry function K can be represented as a function from thevectorial representation of Q to R4×4. In the case of all the joints that weconsidered upper, the components of the homogeneous matrix are polynoms ofthe components of q and of sinus and cosinus of the components of q, with thecoefficients of the polynoms depending on the placement of the joints in theirreference body. The degrees of the polynoms are equal to the dimension of q.

The notion of end effector is arbitrary. In particular, the end effector can bea sub part of the last body. In that case, a last constant rigid move n−1mn isadded to the geometry function:

K(q) = 0m01K0(q0)1m0

2...n−2m0

n−1Kn−1(qn−1)n−1mn

In the case of kinematic trees, several direct geometry functions are definedfor each end effector attached to the end of each leaf of the tree.

3.6 Denavit-Hartenberg parametrization

The Denavit-Hartenberg method is a minimal parametrization of a revolute-prismatic kinematics chain. It is used to minimaly describe the relative jointplacements im0

i+1. This parametrization was a de facto standard 10 years agoand is still often used (even if, now, the SE(3) displacement is generally encodeddirectly using a standard representation).

Indeed, the static displacement im0i+1 requires at least six parameters to

be stored. However, the choice of the rotation around the axes of the joint iand of the the joint i + 1 are free (translation in case of prismatic joints), inthe sense that any static angles around these axes can be compensated to thecost of a translation of the joint configuration interval. The Denavit-Hartenbergparametrization therefore only needs 4 parameters to encode im0

i+1.The detail of how the static displacement can be computed from the Denavit-

Hartenberg parameters can be found in [Murray93].

3.7 Workspace

The workspaceW of the robot is the image space of the direct geometry function,i.e. :

W ={m ∈ SE(3), so that ∃q ∈ Q,K(q) = m

}

3.7. WORKSPACE 29

In general,W is a compact non-convex subset of SE(3). Its border are mostof the time very difficult to compute explicitely.

Chapter 4

Analytical inverse geometry

The direct geometry maps the configuration space to the placement of the endeffector. The map is in general not injective (several configuration correspondsto the same placement) and definitely not surjective: the robot workspace is notthe entire world. However, programming a robot often comes back to placingits end effector to a given reference. The inverse geometry problem is definedby: given a reference placement in SE(3), find one or all the configurations thatmatches this reference. The inverse maps K+ the workspace to some non-emptypossibly non-trivial subset of the configuration space:

K+ : m ∈ W → K+(m) ⊂ Q

The typicaly case of study is the non-redundant case: consider a robot withsix joints. The problem is then properly balanced, with 6 constraints and 6variables.

In general, we do not know how to compute the inverse map explicitely. Thiswas the subject of very extensive works of research in the 90’s, that producesome efficient solutions in the case of non-redundant robot manipulator 6 × 6problem. These approaches where then extended to similar case, like in the caseof one degree of redundancy (7 joints, 6 placement constraints).

4.1 Overview of the possible methods

The following description of the state of the art is taken from Juan Cortes PhDthesis [Cortes03].

4.1.1 Substitution: geometrical view

In most industrial applications, mechanisms are normally designed with partic-ular geometries which allow a closed-form analytical solution to the loop closureequations. For instance, non-redundant serial manipulators often have the lastthree revolute axes intersecting in a same point, which greatly simplifies thesolution of the inverse kinematics problem [Angeles 03].

31

32 CHAPTER 4. ANALYTICAL INVERSE GEOMETRY

4.1.2 Substitution: algebraic view

Two main approaches have been classically used: continuation and elimination(see [Nielsen 97] for a complete survey of these techniques). Polynomial contin-uation methods [Wampler 90] are purely numerical procedures able to find allpossible solutions of the set of non-linear equations (in contrast to other numer-ical methods, such as Newton- Raphson, which converge to a single solution).These methods are based on homotopy techniques for gradually transforminga “start” system whose solutions are known to the system whose solutions aresought. These methods are robust and applicable to any set of equations. How-ever, since they are iterative procedures, they are too slow in practice. Elim-ination approaches use one of the next algebraic methods: the Grobner Basismethod [Buchberger 82], which is an iterative variable elimination technique, orthe resultant method [Gelfand 94], capable of eliminating all but one variablein a single step. In both cases, the elimination process normally leads to anunivariate polynomial, relatively easy to solve [Pan 99]. The applicability ofthe Grobner Basis method is mainly limited by its algorithmic complexity. Re-sultant methods can provide computationally fast techniques, but they requiregeometric intuition to nd (if possible) the formula for the resultant.

4.1.3 Alternative methods

Lately, interval methods for solving systems of non-linear equations have beenproposed as an alternative to continuation and elimination methods. They arebased on interval arithmetic [Moore 79, Hansen 92] and manipulate upper andlower bounds of variables. Two main classes of interval-based methods have beenapplied in Robotics: those based on the interval version of the Newton method[Rao 98, Castellet 98], and those based on subdivision [Sherbrooke 93, Merlet01, Porta 02]. They are completely numerical and robust techniques. Althoughimplemented techniques are still slow, recent improvements are signifficantlyincreasing their efficacity [Porta 03].

4.2 Geometric substitution

We only treat here the geometric substitution case, with some simple exampleto give a broad insight of the approach. The geometric methods are not general,in the sense that they cannot be applied automatically but rather require the in-tuition of a robotic engenieer to chose the proper substitution. These methodswere generalized in the case of 6-DOF revolute robots: the subsequent alge-braic substitution are general (they work automatically for any revolute 6-DOFrobots) but have not been generalized to other kinematic chains.

4.2.1 Simple planar 2R robot

A 2R robot is a fixed kinematic chain composed of two revolute joints withparallel rotation axis. The configuration space is S1 × S1. It therefore evoles in

4.2. GEOMETRIC SUBSTITUTION 33

the plane orthogonal to the rotation axes. Two axes does not enable to controlat the same time the two positions and the orientation. We therefore onlyconsider the projection of the direct geometry function in the position space R2

(removing the orientation). The projected direct geometry map is:

K : (q1, q2) ∈ S1 × S1 →(l1cos(q1) + l2cos(q1 + q2), l1sin(q1) + l2sin(q1 + q2)

)where l1,l2 are the length of the two bodies of the robot.

Now, considering a point p ∈ R2, the inverse geometry returns the configu-ration reaching this point. We denote ρ = ||p|| the distance of p to the robotbasis and α = 1

ρeT1 p the angle of the direction from the robot basis to p.

By anthropomorphism, the first joint is called the “shoulder” and the secondthe “elbow”. The configuration of the elbow is directly given by the distancefrom the robot to the point:

q2 =

The shoulder angle is given by the angle to the point:

4.2.2 And the wrist?

The same decomposition can be applied if two shoulder joints drives the robotorientation in the tridimensionnal space.

If a set of joints are attached to the end effector to enable the control ofthe orientation, a third subproblem should be solved, to map the end effectordesired orientation to the wrist angles. In that case, we first solve the wristorientation, then the elbow lengthening and finally the reach orientation withthe shoulder.

A more formal and very complete description of the geometrical substitutioncan be found in the book of [Murray94].

34 CHAPTER 4. ANALYTICAL INVERSE GEOMETRY

Chapter 5

Iterative optimization

In the previous section, we have defined the inverse geometry problem andshown that there is no solution (yet) to solve it algebrically in the general case.Therefore, the only remaining solution is to approximate it numerically.

The inverse geometry problem comes back to find the root of a non-linearlocally convex real-value function. Since K is not real-value and is not null atthe resolution of the problem, we have to define such a function to encapsulateour problem, for example:

f : q ∈ Q → dist(K(q),m∗)

where m∗ is the reference position of the end effector in SE(3) and dist is a dis-tance (to be properly chosen) in SE(3). Since m∗ might be out of the workspace,the function might never vanish. If such an instance of the problem might beimplemented, it might be more interesting to search for the global optimumof the function rather than for its root (that will match, should the functionvanishes). The inverse geometry problem is then rewritten as an optimizationproblem

minq∈Q

f(q)

In this section, we will introduce the basics of the methods to solve this kindof problem. To simplify the presentation, we will focuse on the optimization inmathbbRn, i.e. in a given global coordinates system rather than directly in themanifold. Care has to be taken when coming back to more general manifolds(like SE(3)) to properly take into account the topology. We will focuse hereon the optimization algorithms rather than on the analysis of the optimizationproblem itself (optimality conditions, caracterization, etc). A complete intro-duction to the optimization analysis can be found in [Hiriart-Urruty 11].

35

36 CHAPTER 5. ITERATIVE OPTIMIZATION

5.1 Optimality condition

We consider here only twice-differentiable C2 functions from a vector space Rnto R:

f : Rn → R

Definition 1 (Global minimizer) The global minizer of f is a vector x∗ ∈Rn if :

∀x ∈ Rn, f(x∗) < f(x)

The minimum of f is f(x∗).

Such a vector x∗ may not exist. Typically, the function might not be lowerbounded, of the lower bound (inf on the function) may not be reached orthe minimum can be reached in several vector, possibly defining an implicitsubmanifold on Rn.

Without any further hypothesis on the geometry of f , we do not have anycondition to characterize x∗. We are reduce to study the local minization con-ditions.

Definition 2 (Local (strict) minimizer) A local strict minizer of f is a vec-tor x∗ ∈ Rn such that there exist a neighborghood of N of x∗ where:

∀x ∈ N , f(x∗) < f(x)

The value f(x∗) is named the local minimum of f .

Take care that the neighborghood can very well contain another local min-imizer, i.e. that there is a sequence of local minimizers of f converging to x∗

(typically for functions that oscillates with diverging frequency when approach-ing to x∗.

The local minimization can intuitively be characterized by the derivatives off . For additional conditions, it might be possible to guarantee that there existonly one local minimizer, i.e. that a local minimizer is a global minimizer: if fis strictly convex, any local minimizer is the global minimizer. Similarly, if wereduce the search to an open set of Rn where f is convex, then a local minimizeron this set is a global minizer on this set. This is why the optimization methodspresented below are often named “convex optimization”.

A local minimizer is characterized by the following conditions:

Theorem 1 (First order necessary condition) If x∗ is a local minimizerof f then the gradient vanishes at x∗:

∂f

∂x(x∗) = 0

.

The reciprocal is of course wrong: in general, a point where the gradientvanishes is a static point of f (it might typically be a saddle point). The Hessianmatrix can be used to distinguish between minimizers and static points.

5.2. OVERVIEW OF THE ALGORITHMS 37

Theorem 2 (Second order necessary condition) If x∗ is a local minimizerof f then ∂f

∂x (x∗) = 0 and the Hessian matrix is positive semidefinite:

∂f

∂x(x∗) = 0,

∂2f

∂x2(x∗) ≥ 0

Here the Hessian is only guaranteed to be semidefinite. This condition alsoholds for local non-strict minimizers, i.e. if x∗ only satisfy f(x∗) ≤ f(x) ona neighborghood. If the Hessian is definite positive it then characterized anecessary condition.

Theorem 3 (Second order necessary condition) The vector x∗ is a localminimizer of f if ∂f

∂x (x∗) = 0 and the Hessian matrix is positive definite:

∂f

∂x(x∗) = 0,

∂2f

∂x2(x∗) > 0

These three conditions can be easily demonstrated using the Taylor devel-opment of f , recalled in the following theorem.

Theorem 4 (Taylor development) Consider f ∈ C2. Then the develop-ments of f in x are:

• (first order development) for any p ∈ Rn, there exists t ∈ R such that:

f(x+ p) = f(x) +∂f

∂x(x+ tp)p

• (second order development) for any p ∈ Rn, there exists t ∈ R such that:

f(x+ p) = f(x) +∂f

∂x(x)p+

1

2pT∂2f

∂x2(x+ tp)p

5.2 Overview of the algorithms

The optimality conditions only use the information provided by the derivativeof f . Similarly, the optimization algorithm will use this same information tosearch for the solution.

All the algorithms start with an initial vector x0, called the initial guess, andwill iteratively improve this guess until it finally converges to a local optimumx∗. At each iteration k, all the algorithm roughly use the same schema: theyfirst decide a direction of descent, i.e. a direction pk of (tangent vector to) Rnwhere the next guess will be search; then the next vector xk+1 is searched inthis direction by choosing a step length α ∈ R+ by solving the one-dimensionnalproblem on the variable α:

minα>0

f(xk + αpk) (5.1)

The direction pk is chosen from a local model of f around xk, typically buildusing the derivative of f at xk, or using the derivatives at the previous xi, i ≤ k.


The step length αk might be fixed (arbitrarily chosen from an apriori canonicalunit of the system) ; or computed by dichotomy (line search algorithm) ; orchosen from the local model of the function (trust region algorithm).

The iterative descente methods build a sequence of points(xk

)k∈N

with

∀k, f(xk) ≤ f(xk+1). Under some decrease conditions, the sequence convergesto a local minimizer x∗. The convergence is assymptotic, which means that in afinite amount of time, only an approximation of x∗ is finally obtained in general.From the decrease conditions, a characterization of how quickly the algorithmconverges toward the local minimizer can also be obtained.

5.3 Descent direction

5.3.1 Gradient descent method

The first method is to choose the descent direction given by the gradient of fin xk:

pk = − 1

||∇fk||∇fk

with ∇fk , ∂f∂x (xk) the gradient of f .

The gradient is indeed the speepest descent direction. Indeed, for a directionp unitary and a step length α, the rate of change in the direction p is given bythe derivative of α→ f(xk + αp). Using the Taylor second order development:

f(xk + αp) = f(xk) + αpT∇fk +1

2α2pT

∂2f

∂x2(xk + tp)p

for some t ∈ R. The derivative of this function for α = 0 should be minimizefor maximizing the descent, which directly gives p∗ = −∇fk.

The gradient is a proper direction descent, in the sense that for α sufficientlysmall, it decreases the value of f :

∃α > 0, f(xk + α∇fk) < f(xk)

This is prove using similar arguments than in the previous statement.The gradient method converges linearly. This is stated by the following

theorem.

Theorem 5 (Rate of convergence of the gradient descent) Consider a se-quence (xk)k∈N generated by a gradient descent on f with exact line search ( i.e.the minimum of (5.1) is exactly reached for any f) converging to a point x∗

where the Hessian of f is positive definite. Then, their exist 0 < ρ < 1 andN ∈ N such that:

∀k > N, f(xk+1) < ρf(xk)

The rate ρ can be computed from the eigenvalues of the Hessian:

ρ =(λn − λ1

λn + λ1

)2

5.3. DESCENT DIRECTION 39

The rate of convergence is linear, which is slow in general, and can typicallybe too slow for any application even for non singular cases. As explained inthe book [LeMarechal06], the gradient descent should be considered only fortheoretical study and should be forbiden for any applicative reason.

The study of the gradient descent on simple examples, like searching for theminimum of a quadric, reveal that the optimal step length is always at a pointwhere the descent direction pk is orthogonal to the gradient computed at thenew point xk+1:

α∗ is such that pTk∇f(xk + α∗kpk) = 0

The gradient descent therefore produces a zigzag trajectory to the optimumwith π

2 rotations at each point xk of the sequence. A typical example used toexhibit the defect of the gradient descent is an optimization landscape where theoptimum is at the end of non-straight deep canyon, for example the “banana”Rosenbrock function:

b : (x, y) ∈ R2 → b(x, y) = (1− x)2 + 100(x2 − y)2

The idea behind the Newton descent is therefore to use additional second-order knowledge about the geometry of f that are not the speepest at the currentpoint but move more directly toward x∗.

5.3.2 Newton method

The Newton1 method approximate the function f at xk by the quadric definedby the second order Taylor approximation (i.e. the inexact development witht = 0), given by:

mk : p→ f(xk) +∇fTk p+ pT∂2f

∂x2(xk)p

The two functions f and x→ mk(x−xk) have the same value of their derivativeof order 0 to 2 at xk.

This function has one global minimum, obtained when the derivative of mk

vanishes and that gives the direction descent of the Newton method:

pk = −∂2f

∂x2(xk)−1∇fk (5.2)

The Newton method has a “natural” step length α = 1. It provides a

quadratic convergence to the local minimum, under the condition that ∂2f∂x2 is

positive definite at any point of the descent sequence. This is formalized by thefollowing theorem.

1Newton was the first one to exhibit its eponymous method, for searching the root of apolynomial, but only in an unpublished booknote. He therefore shared the leadership withRaphson, who published in a similar context an equivalent formulation. Both names Newtonor Newton-Raphson are used indenstigly used.


Theorem 6 (Rate of convergence of the Newton method) Consider a func-tion f and a neighborghood N of a local minimizer x∗ of f where f is Lipschitzcontinuous. Then for any x0 sufficiently close to x∗, (xk)k∈N converges quadrat-ically to x: there exist 0 < ρ < 1 and N ∈ N sufficiently so that:

∀k > N, ||f(xk+1)|| < ρ||f(xk)||2

The complete proof is given in the book [Nocedal 06]. The convergence rate ρdepends on the Hessian norm and Lipschitz coefficient: ρ = L

|| ∂2f

∂x2 (x∗)||.

The Lipschitz continuity around a point satisfying the second-order opti-mality condition guarantee the positive-definitness of the Hessian during all thedescent. On the opposite, is the Hessian becomes non-positive (for example, forx0 too far from x∗), the descent direction (5.2) may not be a descent directionany more. In that case, the algorithm diverges, generally violently.

5.3.3 Quasi-Newton method

Very often in practice, the Hessian is to expensive too compute. Typically, thederivative are very often computed by finite difference:

∂f

∂xi(x0) ≈ f(x0 + εxi)− f(x0)

ε

with xi the iTH element of the cannonical basis and ε is a sufficiently small realnumber. The algorithmic cost of computing the gradient of a function from Rnto R is linear in n, and is n2 for the Hessian, which might be unacceptable.Moreover, if it is often easy to get a good approximation of the gradient byfinite difference, the Hessian approximation is often of much less quality, and thedifference to its real value should be considered when studying the convergencerate.

The quasi-Newton methods are therefore the most-often used class of method.They choose the descent direction as:

pk = B−1k ∇fk (5.3)

where Bk is a n× n positive definite matrix than converges to the Hessian:

limk→+∞

Bk =∂2f

∂x2(x∗) (5.4)

Since we are not using the real Hessian, the rate of convergence is not as goodas for the pure Newton descent. However, we keep a supra-linear convergencerate (much faster than the gradient).

Theorem 7 Rate of convergence of quasi-Newton methods Consider a sequence(xk)k∈N built using (5.3) such that (5.5) holds. If (xk) converges to a localminimizer x∗ of f with positive definite Hesssian, then the rate of convergenceis superlinear, i.e. for any 0 < ρ < 1, there exist N ∈ N such that:

∀k > N, f(xk+1) < ρf(xk)

5.3. DESCENT DIRECTION 41

While a linear rate converges to some linear assymptote, the superlinear becomesfaster than any assymptote after sufficiently many iterations.

This last theorem is stronger and weaker than the one characterizing theNewton method. Stronger because it does not impose the restrictive conditionof the Hessian positivity or continuity. Weaker because it is not able to ensurethe convergence to any minimizer. The condition (5.5) can even be reduced to:

limk→+∞

||(Bk −

∂2f

∂x2(x∗)

)pk|| = 0 (5.5)

for ||pk|| = 1, that is to say, the Hessian has to be correctly approximated onlyin the descent direction.

Two classes of quasi-Newton methods can be distinguished. The first onesestimate the Hessian without any second order computation, by collecting theinformation of the geometry of f while building the descent sequence. Thesecond ones use an approximation of the Hessian, typically that is faster tocompute, and that converges to the real Hessian close to the local minimizer.

Among the first ones, the most often used is the BFGS method (from thenames of the inventors) that maintain during all the algorithm an approxima-tion of the inverse Hessian (sparing at the same time the computation of thederivative and its inverse) by collecting the rate of variation of the gradient.

The next section will focuse on a typical example of the second class.

5.3.4 Gauss-Newton method

The Gauss-Newton method can be used to search for the minimizer of a least-square function, that is to say a function f that is written as the square Eu-clidean norm of a vector function:

∀x ∈ Rn, f(x) =1

2r(x)T r(x)

where the function r : Rn → Rm is a vector-value function, called the residualfunction, that we want to minimize in the least-square sense.

Then, the gradient of f is:

∂f

∂x(x) =

∂r

∂x

T

(x)r(x)

and its Hessian is:

∂2f

∂x2(x) =

∂r

∂x

T

(x)∂r

∂x(x) +

m∑i=1

∂2ri∂x2

(x)ri(x)

where the real-valued function ri is the iTH component of r.The so-called Gauss approximation is to neglect the second term of the

Hessian. This approximation holds when the Hessian of the ri are small andwhen the residuals are small. Neglecting the second term has the advandage


that both the gradient and the approximated Hessian of f can be computedusing only first order derivatives of r. The approximated Hessian is then simply

Bk = ∂r∂x

T(xk) ∂r∂x (xk).

The quasi-Hessian is invertible as soon as there is enough independant resid-uals to ensure that rank(∂f∂x ) = n (typically one m >> n). In that case, thedescent direction of the quasi-Newton method is:

pk =( ∂r∂x

T

(xk)∂r

∂x(xk)

)−1 ∂r

∂x

T

(xk)r(xk)

In this last definition, we can recognize the pseudo inverse of the Jacobian ofthe residuals:

pk =∂r

∂x(xk)

+

r(xk)

5.4 Modified direction and trust region

Newton methods may diverge when the Hessian or its approximation are non-positive. It may converge to a non-strict local minimizer or even a static pointif the Hessian is positive non-definite.

5.4.1 Regularization

To avoid the divergence, a quasi-Newton can be used with a modified Hessianthat is guaranteed to be positive definite. The typical modification is to addthe identity with a weighting parameter:

Bk =∂2f

∂x2(xk) + µIn (5.6)

where µ is the Tikhonov regularization parameter that should be chosen largeenough to ensure that Bk is positive definite. When µ is so large that the Hessianterm is neglectible in practice, the quasi-Newton step B−1

k ∇fk is equal to thegradient step with step length 1/µ. On the opposite, when µ is small enoughso that the identity matrix is neglectible, the quasi-Newton step is equal to theNewton step. The parameter µ is then used to continuously switch between agradient descent and a Newton descent.

Another interpretation of µ in the specific case of least-square functions isto penalize large step ||pk||. The use of Bk will make a trade-off between a largestep caused by a small singular value of the Hessian and a small step enforcedby the penalization term. We will discuss this interpretation with more detailin the next chapter.

5.4.2 Trust region

Finally, we only discuss in this section the line-search methods. The trust-regionmethod also rely on a model mk of f around xk but then decide at the same

5.5. CONCLUSION 43

time the direction and the length by solving a subproblem based on the modelmk:

min||pk||≤∆k

mk(pk)

It can be shown (see [Nocedal 06]) that the minimizer of this problem when mk

is a quadric approximating f in xk has a form equal to (5.6) for some µ thathave to be selected. Using this view on the problem, only ∆k has to be chosen,µ being automatically chosen by solving the trust-region subproblem.

5.4.3 Levenberg-Marquardt algorithm

When the regularizer µ is needed, the complete algorith needs to determinetwo real coefficient at each iteration: α and µ. The parameters are oftenadapted during the descent following some Heuristic. The most well knownis the Levenberg-Marquardt algorithm.

In practice, the two parameters α and µ are modified depending on the ratiobetween the expected improvement of the current step and the improvementreally obtained:

ρk(α, µ) ,f(xk)− f(xk + αkpk)

f(xk)−mk(αkpk)

First αk is decided, typically by dichotomy from the unit step length αk = 1down to a minimal step length αk = 10−6. The dichotmomy stops when adecrease of f is obtained. If no decrease can be obtained, the direction isrejected, and the process start again with a bigger regularizer µ. Now, for anacceptable αk, if ρk is close to 1, the step is ideal and the optimization decreaseas fast as possible. Inversely, if ρk is close to 0 or even negative, the step isvery poor or even diverge. In that case, a smaller trust region (i.e. a biggerregularizer µ) should be chosen.

5.5 Conclusion

The methods that have been quickly presented in this section are convergingvery quickly to a local optimizer as soon as they are in a proper region of theinput space, where the function is locally convex. This bassin of attraction ofthe local minimizer is very difficult to analytically or even numerically describe.However, outside of this bassin, the convergence is not guaranteed and is inany case very slow. Typically, outside the attraction bassin, the trust-regionNewton methods loose their quadratic convergence to become as slow as thegradient. Moreover, the convergence is only on a local minimizer, that is notguaranteed to be global and not even to be interesting. Typically, when solvingthe inverse-geometry problem, the obtained configuration can typically not meetthe desired end-effector placement, or even be very far from it.

All the challenge of numerical methods is to provide the proper intial guess.This is the only solution to ensure a quick and interesting convergence to aproper minimizer.


These numerical methods are working for optimizing a function from Rninto R. Being given a norm on the image space of any vector function, theycan be adapted to search for the minimum of the distance to a given elementof the image space. We now have all the tools to numerically approximate aconfiguration of the robot that matches a given desired end-effector placement.

Chapter 6

Numerical inverse geometry

The Gauss-Newton descent defined in the previous section gives a direct solutionto search for the local minimum residulas of a vector to vector function r : Rm →Rn. However, the inverse-geometry problem is rather written as a minimum-residuals of a function from a Lie group M of dimension m to a Lie group Nof dimension n, typically from the configuration space to SE(3):

r : q ∈ Q → r(q) = m(q)−1m∗ ∈ SE(3)

where m(q) is the placement of the end effector and m∗ is the reference place-ment that should be reached. Moreover, a proper norm on N should be chosento define the least square.

Locally, the map r can be identified to a map from a neighborghood of 0in Rm to a neighborghood of 0 in Rn. Intuitively, the Gauss Newton methodshould then be adapted, since it uses only local information on the residualmaps.

A first naive solution is to identify the residual map r to the vector map fromthe local coordinates of M to the local coordinates of N . When Lie groups areconsidered, this solution is valid since the local chart can be translated to aglobal chart. However, the global chart does not have nice global properties.In particular, it does not preserve the topology of the manifolds and thereforemight badly interact with the iterative descent, adding an additional artificiallayer of non linearity upon the initial problem, which slow down the algorithm.Rather, the local chart should be adapted around the candidate solution duringthe descent. Two aspects have to be considered: the representation of thecandidate parameters in the input space M and the norm in the output spaceN .

Let denote by xk ∈M a candidate minimum of the residual function r. Wesearch a proper representation of the next candidate xk+1. This can simply bedone using the local exponentional representation centered on xk. That is tosay that xk+1 is represented by the increment dk ∈ Rm ∼= Txk

M by:

xk+1∼= xke

dk

45

46 CHAPTER 6. NUMERICAL INVERSE GEOMETRY

Similarly the norm in the output space can be chosen as the Euclidean normof the canonical representation of the residuals.

f(xk+1) =∥∥∥log(r(xk+1)

)∥∥∥2

where f : M → R is the real-value function to be optimized by the Gauss-Newton algorithm.

When the input space M is the special Euclidean groupo SO(3) or a carte-sian product of simple Lie Group with SO(3), the quaternion representation(identified as the vector space R4) is often used in practice. A typical exampleof such problems is the problem known as “structure from motion” or simulta-neous localization and mapping”, where a moving sensor (typically a camera)moves inside an unknowned environment while building a map of it. The opti-mization problem has then to be then rewritten as a constrained optimizationproblem:

minq∈R4

f(q)

subject to qT q = 1

where f is typically the maximum likelyhood of the camera position. Theconstrained-optimization problem is out of the scope of this part and is notfurther detailed. In practice, the constrained might be neglected, each newcandidate qk+1 being normalized after each Newton iteration. This solutionlacks of rigor but not of efficiency.

Part II

Inverse kinematics

47

49

The robot kinematics refers to the motion of the robot bodies with respectto the motion of the robot joints1. Most of the time, the word “kinematics”is limited to the robot velocity, but the same rules apply to the higher-ordermotion derivatives, in particular to the robot acceleration. The last case is oftenrefered as “second-order kinematics”.

As for the geometry, the direct kinematics designates the function that mapsthe velocity of the joint (tangent to the configuration space) to the velocity ofthe robot bodies, typically the linear and angular velocities of the robot endeffector (tangent to SE(3)). This is a proper closed-form function that is easyto calculate. The inverse kinematics is then the problem to find the reciprocalto this direct map, when it is defined, i.e. , being given a reference velocity toexecuted by the robot end effector, what is the configuration velocities that ac-complish this reference. Since we are at the level of the derivative, this problemis linear. It is therefore easier to study and to numerically solve than the inversegeometry problem.

In particular, the resolution of the inverse geometry problem using Gauss-Newton descent can be rewritten as a sequence of inverse-kinematics resolution.

1As mentionned earlier, “kinematics” is often extended to designate also the robot geome-try; in such a case, the reference to the velocity is made explicit by using the term “differentialkinematics”. In these notes, the word kinematics is exclusively used to designate the deriva-tives of the motion, i.e. velocity and its derivatives.

Chapter 7

Direct kinematics

7.1 The kinematic twist

7.1.1 Velocity vector field

Consider a moving rigid object B described by the position Bp ∈ R3 of allits points in a coordinate frame attached to the object. The motion of B isdescribed with respect to a static coordinate frame A by a trajectory in SE(3):

AmB : t→ AmB(t)

For any point p of B, the position of p in the coordinate frame A is given by:

Ap(t) = AmB(t)(Bp) = AMB(t)Bp

with M the homogeneous matrix representation of m. Each point p then definesa trajectory in E(3), that can be derivated. The time derivative is the velocityof the point p expressed in the coordinate system A and denoted by v:

Avp(t) = Ap(t)

The velocities at every points p for a given (fixed) time t defines a vector field.Using the homogeneous representation of AmB , the coordinates of v in A areeasily expressed:

Avp(t) = ˙AMB(t)Bp = ˙AMB(t)AM−1B (t)Ap = AνAB(t)Ap

where AνAB = (AvB ,AwB) is the twist relative to the frames A and B expressed

in the coordinate system A, and defined in (2.5) and . is the operator passingfrom the vector representation to the matrix representation of the Lie algebra.In particular, the v part of the twist corresponds to the velocity of the pointof B that passes by the origin of A at time t (thus making logical to use thenotation v both for the velocity vector field and for the first part of the twist).The twist AνAB corresponds to the classical definition of the twist in mechanics(“torseur cinematique” in French) expressed at the origin A. The first part v iscalled the linear velocity, w being the angular velocity.

51

52 CHAPTER 7. DIRECT KINEMATICS

7.1.2 Instantaneous twist

When defining the twist, we have chosen to look at the product mm−1. Thisis a convention. We can consider the commutation of the product instead.This corresponds to the velocity of a pont p but expressed in the basis of thecoordinate system B and denoted by Bv:

Bv(t) = AmB(t)−1Avp(t) = AmB(t)−1 ˙AmB(t)Bp

Similarly, we define BνAB the twist expressed in the body coordinate systemby:

B νAB = Am−1B

˙AmB

The velocity of any point p in the body system is given by:

Bv(t) = B νAB(t)Bp

The twist in B is defined outside of any world reference and is therefore namedthe instantaneous twist.

7.1.3 Twist actions

From the above two definitions, we have the following simple relations betweenthe two twist representations:

B νAB = AM−1B

AνABAMB

This bilinear form in AmB can be reduced using the bilinear property of thecross product hidden inside ν:

AνAB =

[ARB

ApBARB

0 ARB

]BνAB

with AMB = (ARB ,ApB) denoting respectively the rotation and translation

part of AMB .

proof 1 First recall that we have:

M−1 =

[RT −RT p0 1

]Then:

Aν =

[RT −RT p0 1

] [Bw Bv0 1

] [R p0 1

](7.1)

=

[RT Bw R RT Bv +RT Bw p

0 0

](7.2)

=

[(R Bw) RT Bv −RT p Bw

0 0

](7.3)

using the fact that Rw = (R Bw)R (bilinearity of the cross product). The last equality isfinaly reduced to the vector form to give the expected result.

7.1. THE KINEMATIC TWIST 53

The twist transformation matrix is denoted by AXB . It transforms the twistexpressed at the origin of the system A to the twist expressed at the origin ofthe system B. If the two frames of A and B are aligned, the transformationcorresponds to a change of application point of the twist.

Geometrically, the application X corresponds to the action of the groupSE(3) on the Lie Algebra se(3). See [Murray94] for a detailed discussion on Lieactions.

7.1.4 Velocity vector field

In particular, the twist can be expressed at any point p of the rigid body Busing the basis of the system A. In that case, we recover the velocity vectorfield expressed in the frame A:

Avp = AvAB + Ap× AwAB

We denote by Lp(t) the vector field that at any point p fixed in the world (that isto say defined by a constant Ap associates Avp(t) the velocity of the particule ofthe body passing by p at time t. This corresponds to the Eulerian specificationof the flow (which is rigid in this case) of particules attached to the body B.Intuitively, in corresponds to the point of view of a observer fixed with respectto a “inertial” world frame A and looking at the flow passing at a specific point.

On the opposite, the instantaneous twist defines another vector field, definedby:

Bvp = BvAB + Bp× BwAB

We denote by Ep(t) the vector field that at any point p fixed in the body (i.e.defined by a constant Bp) associates Bvp(t). This corresponds to the Lagrangianspecification of the flow field, and intuitively matches the point of view of anobserver attached to the body and looking to the velocity of one point of thebody as it moves through time.

7.1.5 Twist sum

The twists are defined in se(3) which is a vector space. Two twists expressedin the same coordinate system can then be summed. Consider the case of tworigid bodies B and C moving in relative to a frame A, where the motion of Bis described with respect to A by AmB and the motion of C is defined withrespect to B by BmC . Then the twist describing the velocity of C relative to Ais given by:

AνAC = AνAB + AνBC (7.4)

AνAC = AXB

(BνAB + BXC

CνBC)

(7.5)


7.2 From the kinematic tree to the kinematicdirect map

7.2.1 Joint Jacobian

Recall that the joint geometry map Ki is a function from the joint configurationLie group Qi to SE(3). The map can be derivated using the local vector spacestructure of the input and output manifolds. We denote by Ji the derivative ofKi at the configuration q:

Ji(q) : vq ∈ TqQ → ν = Ji(q)vq ∈ se(3)

where vq is the velocity in the configuration space (formally, a vector of thetangent space to Q at q) and ν is the kinematics twist generated by the joint(formally, a vector of the tangent to SE(3) at Ki(q)). The map Ji(q) is thetangent map to Ki at q1. It is linear in vq. It is often abusively referred as theJacobian of the joint, while the Jacobian should stance for the derivative of themap linking a representation of Qi to a representation of SE(3), and we willfollow the same abusive name in the remaining of the document.

From the joint Jacobian, the joint direct kinematics map is defined by:

(q, vq) ∈ Tq(Qi)×Qi → ν = Ji(q)vq ∈ se(3)

The twist ν is the rigid velocity of the body attached to the output of the jointwith respect to the local coordinate system of the joint, typically attached tothe previous body in the kinematic tree. It can be expressed in any coordinatesystem A. We make the expression explicit as Aν and AJi. Most of the time, itis expressed in the local coordinate system chosen to express Ki. By abuse, wedenote this default expression by Ji = iJi.

7.2.2 Examples of joint

The revolute joint around the arbitrary axis Z has the following simple constantJacobian:

Ji(q) = JRz= (0, 0, 0, 0, 0, 1)

Similarly the prismatic joint of axis Z has the following constant Jacobian:

Ji(q) = JTz= (0, 0, 1, 0, 0, 0)

In both case, the configuration velocity can be considered as the configurationtime derivative vq = q. Finally, the free-flyer joint (that is imposing no con-straint on the joint) has its Jacobian equal to the identity:

Jff = I6

1The tangent map to f :M→ N at m ∈ M is the only map Tmf : TmM→ Tf(m)N sothat for any smooth function g from a neighborghood of f(m) to R and any tangent vector∂∂x∈ TmM , we have (Tmf

∂∂x

)g = ∂∂x

(g ◦ f).

7.2. FROMTHEKINEMATIC TREE TO THEKINEMATIC DIRECTMAP55

7.2.3 Robot direct kinematics

7.2.4 Kinematics map

Recall that the direct geometry of the joint i is expressed in a coordinate systemlocal to the joint and expressed with respect to the output of the previous jointby a constant rigid displacement im0

i+1. The direct geometry map of the robotis then:

K : q → 0m01K0(q0)1m0

2...n−2m0

n−1Kn−1(qn−1)n−1mn

Consider now a curve q(t) in the robot configuration space Q. The curve of therobot end effector 0mn(t) = K(q(t) ∈ SE(3) can be derivated easily using thejoint Jacobians. The robot end effector is then:

(q, vq)→n−1∑i=0

0XiJi(qi)vqi

7.2.5 Robot Jacobian

By identification, the robot Jacobian at q is the following tangent map:

J(q) =

n−1∑i=0

0XiJi(qi)

7.2.6 Elements of algorithm

Algorithmically, the direct kinematics and the Jacobian are computed by abackward loop on the kinematic tree, from the leaf representing the consideredend effector. At each iteration i (from n− 1 downto 0) of the loop, we maintainthe twist expressed in the local frame of joint i and the corresponding Jacobian:

iνi:n = iXi+1i+1νi+1:n + iJi(qi)vqi

iJi:n = iXi+1i+1Ji+1:n + iJi(qi)

where iνi:n is the relative twist of the current body i and the end-effector,expressed in the system i and iJi:n the n− i+ 1 columns corresponding to theexplored n− i+ 1 nodes of the kinematics tree. The action matrix X is simplycomputed from the joint geometry function imi−1 = Ki(qi). The same backwardloop would typically (but not necessarily) compute the robot geometry K(q).

Chapter 8

The pseudo inverse

From the two previous chapters, we can interpret the Gauss-Newton descentas a succession of pseudoinverse of the robot Jacobian J(qk)+, where qk is thecurrent candidate configuration. Up to now, the pseudoinverse was not properlydefined and was only limited to the non-general case where JTJ is invertible,which is clearly not true in general (this square matrix being only positive andsometime non-definite). In this chapter, we properly define the pseudoinverseand link it to the linear least-square problem.

8.1 Moore-Penrose

The pseudoinverse is formally defined by the four rules of Penrose, from whichthe existence, unicity and a long list of properties of this operator can be es-tablished. In this book, we will mostly see the pseudoinverse through the scopeof the singular-value decomposition, that eases a lot (but sometime limits) itsunderstanding.

8.1.1 Definition

The pseudoinverse X of a matrix A ∈ Rn×m is defined by the four followingrules:

AXA = A (8.1)

XAX = X (8.2)

AX is symmetrical (8.3)

XA is symmetrical (8.4)

The matrix A is possibly non invertible or even non square. We will showin the following that X following these four rules always exists and is unique.It is denoted by A+ (sometimes by A† or A#).

57

58 CHAPTER 8. THE PSEUDO INVERSE

8.1.2 Case of invertible matrices

In the case where A is invertible, it is trivial to check that A−1 respects thefour rules and therefore A+ = A−1. Indeed, the pseudoinverse can be seen as ageneralization of the inverse to any matrix.

We denote by N(A) the kernel (or null-space) of A, N(A)⊥ its supplementaryspace (N(A)⊕N(A)⊥) and R(A) the range space of A:

∀x ∈ N(A), Ax = 0 (8.5)

∀x ∈ N(A)⊥, Ax = 0⇒ x = 0 (8.6)

∀y ∈ R(A), ∃x ∈ N(A)⊥, Ax = y (8.7)

Then, the linear map A limited to N(A)⊥ → R(A) is invertible, and its inversematches with the pseudoinverse limited to R(A)→ N(A)⊥ (the proof is left asan exercice).

8.1.3 Proof of unicity

The existence of A+ is not trivial from the Moore-Penrose definition. We leftthe proof for the next section. The unicity is however direct. Suppose that twomatrix X and Y respect the four rules. Then:

X = XAX = XXTAT = XXTATY TAT = XAXAY = XAY AY

In the last equality, the role of X and Y are symmetrical. The same can bedone while inverting the two matrices:

Y = Y AY = ATY TY = ATXTATY TY = XAY AY = X

8.2 Singular-value decomposition

The previous section formally defines the pseudoinverse. We will now give aconstructive property of the pseudoinverse using the singular value decompo-sition. The full understanding of the pseudoinverse cannot be limited to thisconstructive definition, but the properties of the SVD help a lot to establishedmany results linked to the pseudoinverse.

8.2.1 Definition from the Eigen decomposition

First, recall that a symmetric matrix S ∈ Rn×n is diagonalizable in R with byan orthogonal matrix:

S = UΛUT

with U orthogonal, i.e. UUT = I and Λ the Eigen diagonal matrix.Consider now a matrix A ∈ Rn×m. We denote by U, λ the Eigen decomposi-

tion of AAT . The Eigen values are denoted by Λ = diag(λ1...λn). We supposein all the following that the Eigen values λi are sorted. The square matrix AAT

8.2. SINGULAR-VALUE DECOMPOSITION 59

is positive (∀x ∈ Rn, xT (AAT )x = (ATx)TATx ≤ 0) and therefore all Eigenvalues are positive or null. We denote by r the rank of the matrix. From thepositive condition, we have ∀i = 1..n, λi = 0 ⇐⇒ i > r.

We define the singular values of A to be the square-root of the Eigen values:

σi ,√λi

We define the right singular vectors by:

∀i ≤ r, vi =1

σiATui

where ui is the Eigen vector corresponding to λi. It is easy to check that thefamily (vi)i=1..r is orthonormal:

vTi vj =1

σiσjuiAA

Tuj =σ2j

σiσjδij = δij

where δij is 1 when i = j, 0 otherwise.The family (vi)i=1..r is augmented of any basis of n−r vectors of the supple-

mentary space to give a orthogonal matrix V . Similarly, we define Σ ∈ Rn×mthe matrix whose diagonal coefficients are the σi and that is null otherwhere.

The triplet U,Σ, V is called the singular value decomposition of A. It iswritten:

A = UΣV T

The bases U and V are respectivelly called the left and right singular basis. Thesingular value is often rewritten to make the zero of the diagonal of Σ explicit.Denoting by U1 (resp. U0) the Eigen vectors of AAT of non null (resp. null)Eigen value, and V1 (res. V0) the corresponding right singular vectors, and byΣ1 the square diagonal matrix of non null singular values, we can write thesparse decomposition:

A =[U1 U0

] [Σ1 00 0

] [V1 V0

]T= U1Σ1V

T1

With the convention that the singular values are taken positive and sorted,the singular decomposition is unique when the singular values are all different.Otherwise, any choice of the basis of the space span by at least two equal singularvalue respects the definition.

8.2.2 Constructive proof of existence of the pseudo inverse

Using the SVD, the pseudoinverse of A can be easily build:

A+ = V1Σ−11 UT1

It is immediate to check that this matrix complies with the four conditionsof Moore-Penrose. Since the pseudoinverse is unique, this gives a constructivedefinition of the pseudoinverse.


8.3 Some properties

8.3.1 Range, kernel and projectors

The SVD reveals the intrinsic structure of the decomposed matrix. In particular,the bases of the null and range spaces are obtained. We have:

N(A) = V0

R(A) = U1

N(A)⊥ = R(AT ) = V1

R(A)⊥ = N(AT ) = U0

Moreover, the pseudoinverse provides a simple expression of the projectorsin these spaces:

PN(A) = V0V+0 = I −A+A

PR(A) = U1U+1 = AA+

8.3.2 Rank, full rankness

The rank of A can be defined indifferentially as the number of independant rowor column vectors. From the SVD, it is clear that row or column definitions areequivalent and corresponds in fact to the number of non null singular values.

The matrix is full-row rank if its rows form a free family, i.e. if its rank isequal to its number of rows: r = n.

The matrix is full-column rank if its columns form a free family, i.e. if itsrank is equal to its number of columns: r = m.

Abusively, it is said to be full rank when it is full-row rank or full-columnrank.

8.3.3 Construction from the square

The pseudo inverse of A respects the following properties:

A+ = AT (AAT )+ = (ATA)+AT (8.8)

To proove, simply check that both AT (AAT )+ and (ATA)+AT respects theMoore-Penrose conditions. The equality then follows since the pseudoinverse isunique.

When A is full-row rank, AAT is invertible and therefore its inverse is equalto its pseudoinverse. We have then that:

A+ = AT (AAT )−1 [if and only if A is full-row rank ]

This property gives another constructive property of the pseudoinverse. How-ever, this is not a unconditional definition. The property is very often mistakenlyused, while it only holds when A is full-row rank.

8.4. OTHER GENERALIZED INVERSES 61

Similarly, when A is full-column rank, we have:

A+ = (ATA)−1AT [if and only if A is full-column rank ]

In general, only the (non-constructive) property (8.8) holds.

8.4 Other generalized inverses

It is possible to define other generalized inverses of a matrix A that respectsonly some of the four Moore-Penrose conditions. The properties and charac-terizations of the matrices respecting any combination of the four rules aredescribed in detail in the reference book [BenIsrael03]. In particular, the socalled-weighted inverse only respects the two first properties. We denote byL ∈ Rn×n and R ∈ Rm×m two symmetric definite positive matrices and by

√L

and√R their (non-uniquely defined) square-root such that L =

√LT√

L and

R =√RT√

R.The right-weighted inverse of A, denoted by A#R is defined by:

A#R =√R(A√R)+ = RAT (ARAT )+

The right-weighted inverse verifies the three first rules of Moore-Penrose. How-ever, in genteral, we do not have the last one but rather:

A#RAR = (A#RAR)T

Following the notation of [BenIsrael03], it is said a {1, 2, 3} generalized inverse.The left-weighted inverse of A, denoted by AL# is defined by:

AL# = (√LA)+

√L = (ATLA)+ATL

The left inverse respects the rules 1,2 and 4. But, instead of the third rule, wehave:

LAAL# = (LAAL#)T

The L−left R−right weighted inverse of A is defined by:

AL#R =√R(√LA√R)+√R

It only respect the two first rules and instead of the two last, we have:

AL#RAR = (AL#RAR)T

LAAL#R = (LAAL#R)T

8.5 Unconstrained quadratic programing

One of the major interest of the pseudoinverse is that it provides the solutionto the linear least square problem.


8.5.1 Definition

Quadratic formulation

First of all, a quadratic problem of the variable x ∈ Rm is defined by:

minx∈Rm

1

2xTHx− gTx

where H ∈ Rm×m is a symmetric matrix and g ∈ Rm a vector (the notationstances for H the Hessian, and g the gradient, with respect to the Newton stepresolution). The problem does not have any solution if H is non positive (insuch a case, any negative Eigen vector can leads to an arbitrarily large negativevalue). We therefore considere H to be positive. When H is non definite(positive), there is an infinite number of solution along the zero Eigen vectors.In such a case, we are often interested by the least-norm solution ||x||.

Linear least square

A particular quadratic problem arrises when H = ATA and g = AT b. In thatcase, the problem is rewritten:

minx∈Rm

||Ax− b||

with A ∈ Rn×m any rectangular matrix and b ∈ Rn any vector. When Ais not full-column rank (then H = ATA is positive non-definite), there existsa infinite number of solutions, called the least-square solutions. Among thosesolutions, we are interested by the solution of minumum norm, or minimum-norm least-square solution.

8.5.2 Least-square inverse

The minimum-norm least square solution x∗ is uniquely exists and is given bythe pseudoinverse:

x∗ = A+b

proof 2 The vector b can be decomposed as b = R(A)b and b⊥ = N(AT )b. Inthat case, the norm can be developped in:

||Ax− b||2 = ||Ax− b− b⊥||2 = ||Ax− b||2 + ||b⊥||2

this b = b + b⊥ and bT b⊥ = 0. The right term does not depends on x and theminimization problem can be reformulated by the root problem:

findx∈Rm

Ax = b

with b = b ∈ R(A). One solution to this problem is evidently given by thepseudoinverse. We then just have to show that the pseudoinverse is the unique

8.5. UNCONSTRAINED QUADRATIC PROGRAMING 63

minimum-norm solution x∗ = A+b. Suppose that there exist another solution xsuch that Ax = b. Then A(x−x∗) = 0, i.e. x−x∗ ∈ N(A). Since x∗ ∈ N(A)⊥,we have:

||x||2 = ||x− x∗||2 + ||x∗||2

which is minimum when x = x∗.

The minimum-norm least-square can be seen has a problem in two levels:

1. Minimize the sum of square of the residual ||Ax− b||.

2. Among the set of solution of the first objective, minimize the parameternorm.

8.5.3 Elements of calcul

From the decomposition

The pseudoinverse gives the closed-form of the least square solution. Its expres-sion can be reduced using the SVD:

x∗ = V1Σ−1UT1 b

The coefficients of the pseudoinverse should not be computed explicitely inpractice to avoid unecessary extra computation cost. Indeed, the optimum is

compute from the SVD by x∗ = V1

(Σ−1

(UT1 b

)).

In this SVD-based writting, the only divisions are those necessary to invertthe diagonal (σ1..σr). The computation cost is then mainly in the matrix mul-tiplications (O(n2)) and the decomposition itself. The SVD is computed by acomplex 2-stage algorithm that converges toward the exact decomposition butonly approximate it in a finite number of steps. Its complexity is in O(n3), witha large coefficient in place of the O (typically 20). Rather than the SVD, someother decomposition can be used.

Complete orthogonal decomposition

A complete orthogonal decomposition (COD) is any decomposition such that:

A = ULV T =[U1 U0

] [L1 00 0

] [V1 V0

]Twhere U =

[U1 U0

]and V =

[V1 V0

]are orthonormal matrices and L1 is

invertible (this last property justifying the “complete” adjective). With thisdefinition, the SVD is a COD. Most of the time, COD is used when L is lowertriangular.

If the matrix A is full-column rank, then V = In and the COD correspondsto the well QR decomposition. If the matrix A is full-row rank, then U = Inand the COD corresponds to QR of AT (also called LQ of A).

The COD can be computed in two stages, the first one to compute U , thesecond to compute V (or conversly). The first stage is then similar to a rank-reaveling QR decomposition.


QR decomposition

The reference book for the algorithmics of matrix decomposition is [Golub96].The book is a must for implementing any matrix algorithm, in particular QR,COD, SVD or Cholesky. We present quickly here the Givens method to computethe QR.

The idea of the Givens method is to “dig” the upper part of A to introducezeros, by composing a sequence of elementary eponym rotations. A Givensrotation Ri,j,α on Rm is a planar rotation of angle α on the two axis i and j ofthe canonical basis:

Ri,j,α =

1 0 . . . . . . 0

0. . .

...... 1

cos(α) 0 . . . 0 − sin(α)0 1 0...

. . ....

0 1 0sin(α) 0 . . . cos(α)

1 0...

. . ....

0 . . . . . . 0 1

Then, for α = tan−1(

[A]i,l[A]j,l

, the rotation nullifies one elemement of the col-

umn l of A:

[Ri,j,αA]i,l = 0, [Ri,j,αA]j,l =√

[A]2i,l + [A]2j,l

The Givens rotations are applied successively to “dig” the last columns of Auntil the diagonal elements, then the second to last column, etc until the secondcolumn, where only one element is nullified and that conclude the decomposition:

× × × ×× × × ×× × × ×× × × ×× × × ×

→

× × × 0× × × ×× × × ×× × × ×× × × ×

→

× × × 0× × × 0× × × ×× × × ×× × × ×

→

× × × 0× × × 0× × × 0× × × ×× × × ×

→

× × 0 0× × 0 0× × × 0× × × ×× × × ×

→

× 0 0 0× × 0 0× × × 0× × × ×× × × ×

8.5. UNCONSTRAINED QUADRATIC PROGRAMING 65

After the QR

After the left basis U is computed as a sequence of Givens rotation, the samemethod can be expressed to obtain the right basis V . At the end of the twophases of the algorithm, the coefficients of L1 are explicitely expressed, but notthose of U nor V . It is not necessary to explicitely compute them to obtain theleast-square solution. The algorithm is the following:

1. β := UT b : apply the sequence of left Givens to b

2. b := β1:r: only keep the upper part of β, which correspond to the projec-tion in the range of A.

3. y := L1 \ b: compute the substitution of L on b.

4. x∗ := V

[y0

]: express the obtain solution in the canonical basis.

Using the same Givens rotations, the matrix A can be “digged” up to a bidi-agonal form. From this first stage, the SVD can be approximated by iterativelynullifying the bigest non-diagonal elements. The second part of the algorithmtypically converges in a O(n) number of iteration to an approximation in therange of the machine precision. The bidiagonal requires more iterations to beobtained than simply the triangular L1. Furthermore, the second stage increasesagain the cost of the SVD algorithm.

Algorithmic cost

The COD requires O(n3) real multiplications (with typically 9 in factor to thecube). It is the predominant cost. Then, the basis multiplications requiresO(n2) iterations (typically factor O = 4 of each). The triangle substitutionrequires O(r2) (with O = 2).

The cost importantly increases if the explicit expression of the bases U or Vare required (O(n3) each) or if the pseudoinverse is explicitely needed. In thatcase, it corresponds to n substitution and basis multiplication, for an addedO(n3) (with typically O = 6).

A detailed discussion of the algorithmic cost is proposed in [Golub96]. Ingeneral, the carefull engenieer would take care to which elements are reallyneeded in his computations before asking its value.


The Tikhonov regularized least-square problem is:

minx∈Rm

||Ax− b||2 + η2||x||2

where η ∈ R+ is the regularization or damping parameter. When η is null, weobtain the classical least-square problem. On the opposite, the solution x∗ tends


toward 0 when η goes to infinity. The parameter adjust the relative precision ofthe solution to the classical least square problem with respect to a penalizationof the large values of x.

The solution to this problem is obtained by writting the pseudoinverse of

Aη =

[AηIn

], or, using the property A+

η = (ATηAη)+ATη , we have:

x∗ = (ATA+ η2In)+AT b

The obtained matrix is called the regularized (or damped) inverse of A anddenoted by:

A†η , (ATA+ η2In)+AT

Using the SVD of A, it is easy to show that the regularized inverse is equalto:

A†η = V1Σ†ηUT1

where the coefficients σ†ηi of Σ†η are given by:

σ†ηi ,σi

σ2i + η2

The σ†ηi tends toward the σ+i , 1

σiwhen η tends toward 0, which proves that

the regularized inverse tends toward the pseudoinverse when the regularizationis arbitrarily small.

One interesting aspect of the regularized least-square problem is that itsminimum is always unique.

The regularized problem is often prefered to the classical one when smallsingular values can be expected. In particular, this problem corresponds to theone iteratively solved in the Levenberg-Marquardt algorithm.

Chapter 9

Resolution of theinverse-kinematics problem

9.1 Inverting the Jacobian

9.2 Task function

9.2.1 Formal definition

9.2.2 Examples

Center of mass, relative placement, visual servoing

67

68CHAPTER 9. RESOLUTIONOF THE INVERSE-KINEMATICS PROBLEM

Chapter 10

Redundancy

10.1 Projection

10.1.1 Projected gradient

10.1.2 Second task

10.1.3 Stack of tasks


10.2 Weighted inverse

10.2.1 Definition

10.2.2 Resolution

69

70 CHAPTER 10. REDUNDANCY

Chapter 11

Active-set search

71

72 CHAPTER 11. ACTIVE-SET SEARCH

Part III

Inverse dynamics

73

Chapter 12

Overview by Andrea DelPrete

12.1 Motivations

What is better in inverse dynamics:

• dynamics is a better model: the robot is better modeled as a force sourcethan as a velocity source.

• Rigid contact: no control on the contact force by controling the velocity

• Friendly robotics: the robot is never exerting a large force

12.2 Task function

We consider a function of the robot configuration, namely x:

x = f(q)

The function f could also depends on the robot velocity or other state compo-nents. We keep q for simplicity. We want to impose a certain behavior on thefunction output.

12.3 Forward model

Inverse kinematics: the basic model is.

x = J(q)q

The control is q.

75

76 CHAPTER 12. OVERVIEW BY ANDREA DEL PRETE

Inverse dynamics: we need to find a relation between the robot torquesτ and the objectives. Typically, knowing τ , what is q then what is the taskacceleration x.

τ → q → x

12.3.1 Robot dynamics overview

The dynamic equation is

M(q)q + C(q, q)q + g(q) = τ

where M(q) is called the mass matrix, C(q, q)q are the Coriolis and centrifugaleffects and g(q) is the gravity force. This states the relation between τ and q,the terms in q and q being the state of the robot.

The forward dynamics is: being given q, q and τ , what is the resulting q.This typically what dynamics engines of simulators do:

FWD : q, q, τ → q

The inverse dynamics is: being given q, q, q, what is the necessary τ to achievethis acceleration. This is typically what robot control does:

INV : q, q, q → τ

[Say a word about invertibility?]

12.3.2 Deriviation

Most naive way of deriving: coming from Newton law: F = mx, on each rigidbody of the robot taken separately. The problem is that we will have also tofind the constraint forces, i.e. the constraints that prevent the joint constraintsfrom being violated.

Lagrange approach. First define the Lagragian:

L(q, q) = T (q, q)− U(q)

where T (., .) is the kinetic energy, and U(.) is the potential energy. The La-grangian respects:

d

dt

∂L(q, q)

∂q− ∂L(q, q)

∂q= τ (12.1)

If taking a free body (in SE(3)), this last equation boils down to standardNewton F = mx. This is to give the intuition behind the equation, withoutproof.

12.3. FORWARD MODEL 77

Kinetic energy

Let’s now derive T and U . The kinematic energy Ti of one link i is:

Ti(q, q) =1

2mix

2i +

1

2wTi Iiwi

where mi is the mass of the link, Ii ∈ R3×3 its inertia, xi is linear velocity andwi its angular velocity. We can rewrite it spatially by:

Ti(q, q) =1

2νTi Miνi

with νi is the kinematics twist, and Mi is a 6× 6 matrix deduced from mi andIi.

The total system energy is the sum of the Ti over all the bodies:

T (q, q) =

n∑i=1

1

2νTi Miνi

This comes down to the simple form:

T (q, q) =1

2qTMq

where the generalized inertia matrix is:

M =

n∑i=1

JTi MiJi

using the fact that νi = Jiq. M is positive definite, which is important in thefollowing.

Potential energy

The potential energy of one body is:

Ui(q) = mighi(q)

The total potential energy of the system is then:

U(q) =

n∑i=1

mighi(q)

Energy derivation

∂T (q, q)

∂q= M(q)q

d

dt

∂T (q, q)

∂q= M(q)q + M(q)q


∂T

∂q=

1/2

q

T∂M

∂q(q)q

where dpartialMq is a tensor (3D matrix), no detail given yet about the ten-sor/vector product.

The Lagrangian derivatives (12.1) can finally be rewritten:

M(q)q + (M(q)− 1

2q2 ∂M

∂q)q +

∂U

∂q= τ

We find the correspondance with: C(q, q) = M(q)− 12 q

2 ∂M∂q and g(q) = ∂U

∂q . In

the following, we denote by h(q, q) = C(q, q)q + g(q) the dynamic affine term.

12.4 Inverse dynamics control

12.4.1 First control law: computed torque

We denote by τ∗ the control law (the ∗ makes explicit the fact that this is animposed quantity). The computed torque is simply:

τ∗ = Mq∗ + h

The desired acceleration is set to:

q∗ = qd −Kp(q − qd)−Kv(q − qd)

The dynamics of the close-loop system is obtained by putting the τ∗ forminto the dynamics equation, which trivially leads to:

q = qd −Kpe−Kv e

where e = q − qd. We then have the differential second-order linear equation:

e+Kv e+Kpe = 0

which brings the error to 0 as soon as Kp and Kv are positive.

12.4.2 Quadratic program

The computed torque is trivially obtained from the following QP:

τ∗ = minτ∈Rn

||q − q∗||

so that Mq + h = τ

For a “simple” objectives such as tracking q∗||, the QP is a trivial formula-tion, but much more flexible for more complex cases like we will see below.

12.5. MOBILE ROBOT IN CONTACT 79

12.5 Mobile robot in contact

12.5.1 Case of the underactuation

We consider the case of a free floating robot. The configuration of the robotwill then comprehend 6 DOF that represent the position of the robot base. Therobot configuration is then explicitly distinguished:

q =

[xbqa

]where qa are the actuated joints (they have motors, you can directly controlthem) and xb is a representation of the robot basis, typically using a represen-tation of SE(3). The robot dynamic equation is then:

Mq + h =

[0τ

]= ST τ

where S =

[06

In

]is the selection matrix (so that Sq = qa).

12.5.2 Contact forces

If the system is in contact with the environment, then the contact forces areacting on the system and should be considered in the Lagrangian. The contactforces are exerting an equivalent torque at the joint level, given by:

τc = JTc fc

where fc are the Cartesian contact forces (i.e. the force exerted on the contactbody, in whatever representation you line) and Jc is the Jacobian of the con-tact points where the forces are exerted. Let us prove that. The principle ofD’Alembert (virtual work) states that the power of the contact force (Cartesiancontact force times the displacement) is equal to the total power (joint torquetimes the configuration displacement:

fT δx = τT δq

where δx is the Cartesian displacement at the contact point and δq is the con-figuration displacement. Since δx = Jδq we have for any δq:

δqT τ = δqTJT f

By identification, this prooves that τ = JT f . The dynamic equation is then:

Mq + h = ST τ + JTc fc

The contact force fc enters as a new variable that should be found when search-ing for the motor torques.

Example of forces:


• point contact: f is dim 3

• planar contact: you can exert 3 forces and three torques, f is dimension 6.

• several contacts: just stack the forces for each bodies in contact.

[There is some work to do here about duality?]

12.6 Operational space inverse dynamics (OSID)

12.6.1 Task-space acceleration

The task kinematics is:x = Jq

Derivating this relation with respect to time:

x = Jq + J q

Classical case: moving x from a starting point x0 to a reference target x∞.If assuming that a reference trajectory xd(t) is given from x0 to x∞ (with itstime derivatives xd, xd). The imposed Cartesian acceleration should be:

x∗ = xd −Kp(x− xd)−Kv(x− xd)

Like for the proportional-derivative on q, the x acceleration is composed of twopart: a feedforward xd and a feedback Kp+Kv. The feedforward is not sufficientin practice, as modeling and actuation errors prevent a perfect application ofthe feedforward. The feedback makes the control robust.

12.6.2 OSID problem

The problem of finding τ that meets at best the imposed task acceleration isthen written:

minτ∈Rn

||Jq + J q − x∗||2

so thatMq + h = ST τ

This is a constrained quadratic problem.

12.6.3 Constrained quadratic problem

Let us consider the generic linearly-constrained linear least-square problem(LCLLSP):

miny∈Rn

||Ay − b||2 (12.2)

so thatCy = d

We assume that the constraint is feasible, that is to say d is in the range of C(which is typically the case in the simpler case when C is full row rank). This

12.6. OPERATIONAL SPACE INVERSE DYNAMICS (OSID) 81

is a reasonible assumption, as an unfeasible constraint means that the LCLLSPas no solution.

The space of all y that satisfy the constraint can be described using the nullspace of C; it is the set of y of the form:

y = C+d+ ZT z

where Z is a basis of the null space of C, that can be obtained from the SVDas Z = V0 using:

C =[U1 U0

] [Σ 00 0

] [V1 V0

]TIt is straight forward to check that any y of this form respect the constraint(Cy = CC+d+ CZz = d+ 0 since CC+d = d by assumption of d being in therange of C and CZ = 0 by definition of Z). The LCLLSP is then reduced to aunconstrained LLSP:

minz∈Rm−r

||AZz − (b−AC+d)||2

whose solution is directly obtained using the pseudoinverse of AZ, leading finallyto the least-norm solution of (12.2):

y∗ = C+d+ Z(AZ)+(b−AC+d)

[Proof of norm minimality of y∗ ?][Explanation of the cascade... introducing the null space of AZ]

12.6.4 OSID with contact

The previous section directly gives the optimum of the OSID problem. Letus now consider the resolution of the OSID in case of contact. We now havethree variables: τ, q and fc. In addition to the dynamics constraint, we alsohave the constraint that the robot movements should correspond to the contactmodel. Typically, a rigid contact model imposes no motion of the contact point(therefore decoupling the force from the motion). This is written xc(q) = x0

c

where x0c is a constant position. The constraint is written in terms of q by

derivating it twice with respect to time:

Jcq + Jcq = 0

The OSID with contact finally can be written as the following LCLLSP:

minτ∈Rn

||Jq + J q − x∗||2 (12.3)

so thatMq + h = ST τ + JTc fc

Jcq + Jcq = 0


This is written under the form (12.2) by setting:

y = (q, f, τ)

A =[J 0 0

]b = x∗ − J q

C =

[M −JTc −STJc 0 0

]d = (−h,−Jcqdot)

Using variations of A, it is also possible to express objectives in terms ofcontact forces (for ex, applying a given reference force when cleaning the board).

12.6.5 Elements of computations

The typical control frequency when imposing torques is 1kHz, which means thatthe control should be solved in 1ms. The typical dimension of the OSID problemwith contact is: 2n+k+6 variables (n the number of joints, k the number ofcontact); n+6+k constraints. For a standard humanoid, n=30, k=20 (i.e. 86variables, 50 constraints), which is too high for a brut force resolution of (12.3)in real time.

A computational reduction is achieved by exploiting the sparsity of the prob-lem. This can be done at no implementation cost by using our knowledge ofthe specific form of the problem, coming from the dynamics. We split all thequantities in the configuration space between the non-actuated floating base(indexed by u) and the actuated DOF (indexed by a):

M =

[Mu

Ma

],

Chapter 13

Geometry of forces andaccelerations

83

84 CHAPTER 13. GEOMETRY OF FORCES AND ACCELERATIONS

Chapter 14

The dynamic equation of amulti-body system

14.1 Recursive Newton-Euler algorithm

85

86CHAPTER 14. THE DYNAMIC EQUATIONOF AMULTI-BODY SYSTEM

Chapter 15

Operational-space inversedynamics

87

88 CHAPTER 15. OPERATIONAL-SPACE INVERSE DYNAMICS

Chapter 16

Synthesis: an overview ofconstrained optimization

89

90CHAPTER 16. SYNTHESIS: AN OVERVIEWOF CONSTRAINEDOPTIMIZATION

Part IV

Optimal control

91

93

In the three first parts, we have only considered instantaneous approxima-tion of the problem. In Part 1, we have only considered the static problem offinding one configuration satisfying a set of constraints. In the second part,we have shown that the pseudo-path obtained by integrating the search for theoptimal configuration can be viewed as a trajectory joining an initial configu-ration to its optimum. However, we did not really select this trajectory and itis very unlikely that the resulting one is optimizing any interesting criteria. InPart 3, we have considered the robot dynamics, which is closely related to thenotion of trajectory. However, once more, we only looked at the instantaneouslinearization of the dynamics and obtained a trajectory by integration of theinstantaneous control.

In this part, we now consider directly the trajectory of the system and workon its properties to obtain an optimal movement form the start point to theend point. The trajectory is a function of time in the space of both control andstates. It is then evolving in a space of infinite dimension. Finding a minimumover a functional, that is to say a function of functions, can be viewed as a gen-eralization of the problem of minimization over a finite-dimension vector space.It is called calculus of variation. In our case, the state and control trajectoriesare linked by the dynamics, which is formulated as a ordinary differential equa-tion of control. The corresponding problem is called optimal control, that isto say the minimization over a functional of the state and control trajectoriesconstrained by the control differential equation. In the “good” cases, it can beviewed as a special case of the calculus of variation. In many cases (even not sodifficult), the optimal-control class of problem goes beyond calculus of variation.

In a first time, we make a brief overview of the optimal-control formulationand theoretical results. On a numerical point of view, the actual optimal-controlproblems are very often too difficult to be solved using this results. In thesecases, we then approximate the problem by considering a finite-dimension basisof function for the state and the control (for example time discretization) andrely on finite-dimension optimization to find the best approximation to theinitial problem. This so-called “direct” approach is the subject of the secondchapter of this part.

Chapter 17

Problem formulation andproperties

In this part, we give an overview of the theoretical results of the optimal-controlfield. The content of this chapter would very well be the topic of a 25-hourclass and 250-pages book. The idea here is to provide a brief introduction andan insight of what would be the alternatives to a direct resolution. We firstformulate the problem, then gives to related formulation to provide an insightof the difficulty of the optimal-control class. The main results are formulatedwithout proofs in the third section. They enable the formulation of the indirectresolution methods, that we briefly describe and discuss.

17.1 Definition of the problem

17.1.1 Robot dynamics

Up to now, we only describe the robot by its current configuration. If we considerfor example the control to be the joint torques, as in the previous part, the robotconfiguration is certainly not enough to describe the current state of the robotand the action of the torque control on it: the joint velocities are missing andshould be considered in the state. In that case, the joint acceleration are notpart of the state since infinitely many acceleration are instantaneously possiblewhen chosing the torque control. More precisely, at a given time t and for aknown configuration and velocity, it is not possible to know the accelerationuntil the torque controls are chosen.

More generally, the state is chosen with respect to the control in order touniquely define the effect of the first on the second. The dynamics is then givenas a very abstract ordinary differential equation linking the state. Denotingby x(t) the state and by u(t) the control at time t, the dynamics equation iswritten:

x(t) = f(x(t), u(t))

95

96 CHAPTER 17. PROBLEM FORMULATION AND PROPERTIES

We only consider here for simplicity autonomous dynamics, i.e. that are notexplicitely function of time. The same results are obtained with time-varyingdynamics, where the time partial derivative of f acts as a drift to be compen-sated. This case is not so frequent in robotics.

The choice of x and u, and the consequent dynamics f are the modelingchoice of the programmer. One can for example consider the simple dynamics:

x = q, x = q, f : x, u→ u

This dynamics in fact only consider the kinematics of the system, and notwhat we called dynamics in the previous part (the robot inertia, forces andacceleration).

The dynamics of the previous part is:

x = (q, q), u = τ, f : x, u→[

qM(q)−1(b(q, q)− τ)

]with M the robot generalized inertia matrix, b the dynamics drift vector and τthe joint torques.

In this last equation, we do not consider the actuator dynamics, since thecontrol is directly at the joint output level. The motor dynamics can be added.For example, a direct-current electrical motor would be control for example incurrent, which does not significantly complexify the dynamics model. Depend-ing on the level of modeling, the motor temperature might be added to thestate and affect the motor behavior. An air cylinder would be controled by theelectrical current opening the servo-valve that makes the air flow enter in thetube, changing the pressure and producing torque. In that case, the state asto be increased to render the air flow state, which increases the degree of thedifferential equation.

17.1.2 Cost functionnal

The cost is defined as a general functionnal over both the control and the statetime-dependant functions. In order to make explicit that a functionnal manipu-lates a function of the time, and not one instance at a fixed time, we emphasizethe notation of x and u, defining the functional by:

g : x, u→ g(x, u)

The emphase is ommited when the function nature of x and u are evident, forexample in denoting x(t). Very often, it is reduced to the following form:

g(x, u) = c(x(tf )|tf ) +

∫ tf

0

c(x(t), u(t)|t)dt

where tf ∈ R+is the terminal time, c(.|tf ) is the terminal cost and c(., .|t) is

the running (or integral) cost. The terminal cost can be free (i.e. a variable of

17.1. DEFINITION OF THE PROBLEM 97

the problem) or fixed, infinite (in such a case, c(.|tf ) is null) or bounded. In thefollowing, we will only consider fixed finite terminal time, for simplicity. Thesame properties can be adapted for the other cases.

This form of the cost functionnal is named the Lagrange-Mayer form. Twoother forms are very often considered. The Lagrange form only consider theintegral term:

g(x, u) =

∫ tf

0

c(x(t), u(t)|t)dt

The Mayer form only consider the terminal term:

g(x, u) = c(x(tf )|tf )

Theoretically, these three forms are strictly equivalent, as any problem in oneof them can be reformulated under the other forms. For example, it is possibleto pass from the the Lagrange-Mayer to the Mayer by considering an additionalvariable x0 subject to the dynamics x0(t) = c(x(t), u(t) and setting the completedynamics to be x = (x, x0) and f = (f, c). The Mayer cost is then g(x, u) =c(x(T )) + x0(T ). This equivalence also emphasize the symmetry between therunning cost and the dynamics functions.

17.1.3 Optimal control formulation

In the following, we will consider the optimal control problem of the Lagrange-Mayer form:

minx,u

c(x(tf )) +

∫ tf

0

c(x(s), u(s))ds (17.1)

s.t. x(t) = f(x(t), u(t) (17.2)


17.2 Trajectory optimization: Boundary-valueproblem

17.2.1 Problem formulation

17.2.2 Shooting resolution

17.2.3 Numerical integration methods

17.2.4 Multiple-shooting resolution

17.3 Infinite-dimension optimization: calculus ofvariation

17.4 Optimality conditions

The optimal-control problem can be seen as an unconstrained minimizationonly over the u trajectory, as x is then uniquely defined by the integration ofthe dynamics. We denotes this functional by:

J(u|x0) = c(x(tf ) +

∫ tf

0

c(x(s), u(s))ds

We abusively use the same notation to denote the partial cost:

J(u|xt, t) = c(x(tf ) +

∫ tf

t

c(x(s), u(s))ds

The functionnal depends on the initial point xt and of the starting time t. Herext is a fixed state (finite dimension) while u : [t, tf ] → U is a time-dependentfunction.

17.4.1 Bellman principle

The Bellman optimality principle simply states that, for any trajectory u miniz-ing J(u|x, 0), a subtrajectory of u starting at t minimizes J(.|x(t), t). To writethis principle explicitely, we define the optimum of J to be the value functionV :

V ∗(x, t) = minu[t,tf ]

J(u|x, t)

We then have of course:V ∗(x, tf ) = c(x)

The Bellman principle is written as a recursive definition of V ∗:

V ∗(x, t) = minu[t,t+∆t]

{∫ t+∆t

t

c(x(s), u(s))ds+ V ∗(x(t+ ∆t), t+ ∆t)

}

17.4. OPTIMALITY CONDITIONS 99

The value function is a complete solution of the optimal-control problemthat directly gives the optimal trajectories starting from any point of the space.It is not possible to express it in general, neither to compute it numerically.

17.4.2 Equation of Hamilton-Jacobi-Bellman

The Bellman optimality principle can be perceived as an optimal dynamics forthe system. In particular, assuming that it is possible to obtain the first orderTaylor development of V ∗:

V ∗(x(t+∆t), t+∆t) = V ∗(x(t), t)+V ∗t (x(t), t)∆t+V ∗x (x(t), t)f(x(t), u(t))∆t+o(∆t2)

where we denote with indexes the partial derivatives:

V ∗t ,∂V ∗

∂t

V ∗x ,∂V ∗

∂x

The development can be propagated through the Bellman equation. For asmall ∆t, the integral becomes trivial. We then have

V ∗(x(t), t) = minu[t,t+∆t]

{c(x(t), u(t))∆t+ V ∗(x(t), t) + V ∗t (x(t), t)∆t

+ V ∗x (x(t), t)f(x(t), u(t))∆t}

Finally, we can take out of the min operator the terms not depending of u andobtained the following partial derivative equation:

Vt(x(t), t) = maxu(t)∈U

{− c(x(t), u(t))− V ∗x (x(t), t)f(x(t), u(t))

}We define the system Hamiltonian H to be:

H(x(t), u(t), p) =< p|f(x(t), u(t)) > −c(x(t), u(t)

for any vector p ∈ X 1. The differential equation is then reformulated with theHamiltonian as:

Vt(x(t), t) = maxu(t)∈U

{H(x(t), u(t),−Vx(x(t)))

}The choice of have a max on −Vx or a min on Vx is arbitrary. We chose this

convention to follow Pontryagin point of view of the trajectory optimality as amaximum.

This differential equation is called Hamilton-Jacobi-Bellman equation. Itprovides a very powerful result, that is a necessary but also a sufficient condition

1We will see late p as the costate, living in the dual of the state space. This notation inp ∈ X is then strictly-speaking abusive.


of optimality (we only here roughly derivated the necessary side). In generalthe HJB equation is very difficult to solve or integrate. When it is possible, itresults in the value function, which gives all the solutions of the system.

The optimal control corresponds to the argmax of the HJB equation:

u∗(t) = arg maxu(t)∈U

{H(x(t), u(t),−Vx(x(t)))

}17.4.3 Pontryagin Maximum Principle

The HJB equation is somehow too powerful. First, it does not hold for someclasses of systems. In particular, we have used the calculus-of-variation pointof view to obtain it, using a limited development of the value function thatdoes not necessarily exists. For more general classes of system, the condition ofoptimality is given by the Pontryagin maximum principle.

Theorem 8 (Weak Maximum principle) Let u∗ : [0, tf ] → U be the globaloptimum of (17.1) subject to the dynamics (17.2), and x∗ : [0, tf ] → X thecorresponding trajectory. Then there exists a costate trajectory p∗ : [0, tf ]→ Xsuch that:

x∗ = Hp(x∗, u∗, p∗)

p∗ = −Hx(x∗, u∗, p∗)

Then, for any t ∈ [0, tf ], the control maximize the Hamiltonian:

u∗(t) = maxu∈U

H(x∗(t), u, p∗(t))

This is a weak formulation of the PMP, as it does not consider any constraintson the control nor state trajectories, nor any terminal conditions. The weakPMP is not too difficult to demonstate. The complete PMP is much moredifficult. The interested reader is referred to Pontryagin book or to [Liberzon03].

17.4.4 Discussion

HJB states that the control can be computed as:


{H(x∗(t), u(t),−Vx(x(t)))

}On the other hand, PMP states a similar equation including p∗:


{H(x∗(t), u(t), p∗(t)))

}The two principles are then very similar. However, assuming that we are able

to compute the value function V ∗, the HJB provides also a feedback formulation,that is to say

17.5 Indirect methods

Chapter 18

Direct resolution

18.1 Discretization and non-linear finite-dimensionoptimization

18.2 Explicit versus implict formulation

18.3 Direct multiple shooting

18.4 Linear-quadratic optimization

18.4.1 Gauss-Newton hypothesis

101

102 CHAPTER 18. DIRECT RESOLUTION

Chapter 19

The example of the robotwalk

103

Numerical Methods for Robotics -...

Documents

Transcript of Numerical Methods for Robotics -...