BOOK2

Alexander
Reiter
Optimal Path
and Trajectory
Planning for
Serial Robots
Inverse Kinematics for Redundant
Robots and Fast Solution of
Parametric Problems
Optimal Path and Trajectory Planning for
Serial Robots
Alexander Reiter
Optimal Path and

Trajectory Planning for
Serial Robots
Inverse Kinematics for Redundant
Robots and Fast Solution of
Parametric Problems
Alexander Reiter
Johannes Kepler University Linz
Linz, Austria
Dissertation, Johannes Kepler University Linz, 2019
ISBN 978-3-658-28593-7 ISBN 978-3-658-28594-4 (eBook)

https://doi.org/10.1007/978-3-658-28594-4
Springer Vieweg
© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020
This work is subject to copyright. All rights are reserved by the Publisher, whether the whole
or part of the material is concerned, specifically the rights of translation, reprinting, reuse of
illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way,
and transmission or information storage and retrieval, electronic adaptation, computer software,
or by similar or dissimilar methodology now known or hereafter developed.
The use of general descriptive names, registered names, trademarks, service marks, etc. in this
publication does not imply, even in the absence of a specific statement, that such names are
exempt from the relevant protective laws and regulations and therefore free for general use.
The publisher, the authors and the editors are safe to assume that the advice and information in
this book are believed to be true and accurate at the date of publication. Neither the publisher
nor the authors or the editors give a warranty, expressed or implied, with respect to the material
contained herein or for any errors or omissions that may have been made. The publisher remains
neutral with regard to jurisdictional claims in published maps and institutional affiliations.
This Springer Vieweg imprint is published by the registered company Springer Fachmedien Wiesbaden
GmbH part of Springer Nature.
The registered company address is: Abraham-Lincoln-Str. 46, 65189 Wiesbaden, Germany
Acknowledgement
This work has been supported by the COMET-K2 “Center for Symbiotic
Mechatronics” of the Linz Center of Mechatronics (LCM) funded by the
Austrian federal government and the federal state of Upper Austria.
Alexander Reiter
Contents
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . XI
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . XIII
List of Symbols and Abbreviations . . . . . . . . . . . . . XV
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XVII
Kurzfassung . . . . . . . . . . . . . . . . . . . . . . . . . . . XIX
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . 1
2 Numerical Basics . . . . . . . . . . . . . . . . . . . . . . 9
2.1 Numerical Optimization . . . . . . . . . . . . . . . . . . . 10
2.1.1 Convexity . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2 Constrained Optimization . . . . . . . . . . . . . . . . . 11
2.1.3 Parametric Sensitivities . . . . . . . . . . . . . . . . . . 15
2.2 B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . . . 32
2.2.1 Evaluation Algorithm . . . . . . . . . . . . . . . . . . . 32
2.2.2 Derivatives w.r.t. the Path Parameter . . . . . . . . . . 35
2.2.3 Derivatives w.r.t. the Control Points . . . . . . . . . . . 37
2.2.4 Constrained Approximation . . . . . . . . . . . . . . . . 37
2.3 Optimal Control Problems and Direct Methods . . . . . . 40
2.3.1 Direct Single Shooting . . . . . . . . . . . . . . . . . . . 42
2.3.2 Direct Multiple Shooting . . . . . . . . . . . . . . . . . . 43
2.3.3 Direct Collocation . . . . . . . . . . . . . . . . . . . . . 45
3 Kinematics of Serial Robots . . . . . . . . . . . . . . . . 49

VIII Contents
3.1 Position and Orientation . . . . . . . . . . . . . . . . . . . 50

3.1.1 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
3.1.2 Orientation . . . . . . . . . . . . . . . . . . . . . . . . . 51
3.2 Translation and Rotation Velocity . . . . . . . . . . . . . . 53
3.3 Joint Space, Work Space, Task Space . . . . . . . . . . . . 54
3.3.1 Joint Space . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3.2 Work Space . . . . . . . . . . . . . . . . . . . . . . . . . 55
3.4 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . 58
3.4.1 Position-Level Forward Kinematics . . . . . . . . . . . . 58
3.4.2 Velocity-Level and Higher-Order Forward Kinematics . . 59
3.5 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . 62
3.5.1 Kinematically Redundant Manipulators . . . . . . . . . . 63
3.5.2 Position-Level Inverse Kinematics . . . . . . . . . . . . . 65
3.5.3 Velocity-Level and Higher-Order Inverse Kinematics . . . 72
4 Path and Trajectory Planning . . . . . . . . . . . . . . 93

4.1 Boundary Value Problem . . . . . . . . . . . . . . . . . . . 95
4.2 Optimal Point-to-Point Trajectory Planning . . . . . . . . 97
4.2.1 Single-Step Approach . . . . . . . . . . . . . . . . . . . . 98
4.2.2 Multi-Step Approach . . . . . . . . . . . . . . . . . . . . 116
4.2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . 135
5 Optimal Path Tracking . . . . . . . . . . . . . . . . . . . 137

5.1 Joint Space Approaches . . . . . . . . . . . . . . . . . . . 138
5.2 Work Space Approaches . . . . . . . . . . . . . . . . . . . 138
5.2.1 Kinematically Non-Redundant Manipulators . . . . . . . 138
5.2.2 Kinematically Redundant Manipulators . . . . . . . . . . 139
6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . 155
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
A B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . . 175

A.1 Arc Length Parameterization . . . . . . . . . . . . . . . . 176
A.2 B-Spline Software Package . . . . . . . . . . . . . . . . . . 178
A.2.1 Open Challenges . . . . . . . . . . . . . . . . . . . . . . 178
Contents IX
B Mechatronic Multibody Systems . . . . . . . . . . . . . . 181

B.1 Solution of the Equations of Motion . . . . . . . . . . . . . 183
B.1.1 Constraints . . . . . . . . . . . . . . . . . . . . . . . . . 183
B.1.2 Solution of System of DAEs . . . . . . . . . . . . . . . . 185
B.1.3 Divide & Conquer: Constraint Forces and Accelerations 185
B.1.4 Solution by Time Integration . . . . . . . . . . . . . . . 186
B.1.5 Conditional Constraints . . . . . . . . . . . . . . . . . . 187
B.1.6 Solution Algorithm . . . . . . . . . . . . . . . . . . . . . 188
B.2 Code Generation Utility . . . . . . . . . . . . . . . . . . . 188
B.2.1 Current Limitations and Open Challenges . . . . . . . . 190
List of Figures
1.1 SCARA4 Lab Prototype. . . . . . . . . . . . . . . . . . . . . 4
1.2 SCARA4 Planar Model. . . . . . . . . . . . . . . . . . . . . 4
1.3 Stäubli TX90L Industrial Manipulator On Linear Axis. . . 4
1.4 Comau Racer R3 Industrial Manipulator. . . . . . . . . . . 5
2.1 A Convex Function. . . . . . . . . . . . . . . . . . . . . . . 11
3.1 Position Vector. . . . . . . . . . . . . . . . . . . . . . . . . . 50

3.2 Reachable Work Space of SCARA4 Manipulator. . . . . . . 56
3.3 Dexterous Work Space of SCARA4 Manipulator. . . . . . . 57
3.4 Forward Kinematics of the SCARA4 Manipulator. . . . . . . 62
3.5 Inverse Kinematics of the SCARA3 Manipulator. . . . . . . 66
3.6 Inverse Kinematics of the SCARA4 Manipulator. . . . . . . 68
3.7 Domains of the Jacobian. . . . . . . . . . . . . . . . . . . . 80
3.8 Algorithmic Singularity in JSD of the SCARA4 Manipulator. 84
4.1 Furuta Pendulum. . . . . . . . . . . . . . . . . . . . . . . 95

4.2 Swing-Up Motion of Furuta Pendulum: Results for Joint
Angles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
4.3 Swing-Up Motion of Furuta Pendulum: Results for Joint
Velocities. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
4.4 Swing-Up Motion of Furuta Pendulum: Results for Motor
Torque. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
4.5 SCARA4 Minimum-Time Trajectory Planning. . . . . . . . 103
4.6 SCARA4 Minimum-Time Motion: Results for Constrained
Accelerations. . . . . . . . . . . . . . . . . . . . . . . . . . . 107
4.7 SCARA4 Minimum-Time Motion: Results for Constrained
Motor Torques. . . . . . . . . . . . . . . . . . . . . . . . . . 109
XII List of Figures
4.8 SCARA4 Minimum-Energy Trajctory Planning. . . . . . . . 110

4.9 SCARA4 Minimum-Energy Motion: Results for Constrained
Motor Torques. . . . . . . . . . . . . . . . . . . . . . . . . . 112
4.10 Comau Racer R3 Trajectory Planning: Experimental Setup. 113
4.11 SCARA4 Path Planning: Optimal Picking Poses. . . . . . . 118
4.12 SCARA4 Path Planning: Admissible Picking Positions. . . . 125
4.13 SCARA4 Path Planning: Admissible Picking Paths. . . . . . 126
4.14 Synchronization Tasks of Two Comau Racer R3. . . . . . . 127
4.15 Synchronization Tasks of Two Comau Racer R3: sync1. . . 133
4.16 Synchronization Tasks of Two Comau Racer R3: sync2. . . 134
5.1 SCARA4 Minimum-Time Path Tracking. . . . . . . . . . . . 143

5.2 SCARA4 Minimum-Time Path Tracking: Path Parameter and
Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
5.3 SCARA4 Minimum-Time Path Tracking: Joint Angles and
Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
5.4 SCARA4 Minimum-Time Path Tracking: Motor Torques. . . 146
5.5 Stäubli TX90L Minimum-Time Path Tracking. . . . . . . . 148
5.6 Stäubli TX90L Minimum-Time Path Tracking: Tracking Error. 149
5.7 Stäubli TX90L Minimum-Time Path Tracking: Motor Torques. 149
5.8 Stäubli TX90L Minimum-Time Path Tracking: Path
Parameter and Derivatives. . . . . . . . . . . . . . . . . . . 152
5.9 Stäubli TX90L Minimum-Time Path Tracking: Joint Angles
and Derivatives. . . . . . . . . . . . . . . . . . . . . . . . . . 153
5.10 Stäubli TX90L Minimum-Time Path Tracking: Motor Torques. 153
5.11 Stäubli TX90L Minimum-Time Path Tracking, Experiment:
Tracking Error. . . . . . . . . . . . . . . . . . . . . . . . . . 153
B.1 Schematics of Autonomous Walking Robot (AWARO). . . . 182

List of Tables
2.1 Parametric QP: Results. . . . . . . . . . . . . . . . . . . . . 28
2.2 Parametric NLP: Results. . . . . . . . . . . . . . . . . . . . 29
2.3 Parametric NLP: Results. . . . . . . . . . . . . . . . . . . . 31
4.1 SCARA4 Minimum-Time Trajctory Planning: Results. . . . 108

4.2 SCARA4 Minimum-Energy Trajctory Planning: Results. . . 111
4.3 Comau Racer R3 Trajectory Planning: Results. . . . . . . . 115
4.4 SCARA4 Path Planning: Computation Time and Cost Increase
in Synchronization Tasks. . . . . . . . . . . . . . . . . . . . 126
4.5 SCARA4 Path Planning: Computation Time and Cost Increase
4.6 Synchronization Tasks of Two Comau Racer R3: Success Rates
4.7 Synchronization Tasks of Two Comau Racer R3, sync1 :
Computation Time and Cost Increase. . . . . . . . . . . . . 132
4.8 Synchronization Tasks of Two Comau Racer R3, sync2 :
Computation Time and Cost Increase. . . . . . . . . . . . . 132
5.1 SCARA4 Minimum-Time Path Tracking: Results. . . . . . . 147

5.2 Stäubli TX90L Minimum-Time Path Tracking: Results. . . 153
List of Symbols and
Abbreviations
Abbreviation Description
DMS direct multiple shooting
DOF degree(s) of freedom
DSS direct single shooting
EE end-effector
EoM equations of motion
GNA generalized nullspace augmentation
iff if and only if
IK inverse kinematics
JSD joint space decomposition
KKT Karush-Kuhn-Tucker
LICQ linear equality constraint qualification
NLP nonlinear programming
OCP optimal control problem
PG projected gradient
QP quadratic programming
RG reduced gradient
w.l.o.g. without loss of generality
correct, successful
incorrect, failed
Abstract
This dissertation covers the topic of optimal path and trajectory planning
for serial robots in general, and rigorously treats the challenging application
of path tracking for kinematically redundant manipulators therein in partic-
ular. Furthermore, methods for fast computation of approximate optimal
solutions to planning problems are presented.
As the theoretical foundation relevant to this thesis, basics of numerical

optimization, B-spline curves and their use for the solution of optimal
control problems using direct methods are discussed. Furthermore, the
kinematics of serial robots is reviewed in depth. In particular, the inverse
kinematics problem regarding kinematically redundant manipulators is
considered. Therein, numerical optimization is applied not to find any
solution to the path tracking problem, but rather to solve it in an optimal
manner. As one contribution of this thesis, a method is presented that is
particularly useful for this task. Therein, differential inverse kinematics
is augmented by a weighted linear combination of the Jacobian nullspace
basis, which allows to directly exploit a system’s internal motion capabilities
in accordance with the path tracking optimization goal. It is capable of
resolving both, the path tracking task and the optimal inverse kinematics
problem, simultaneously. The efficacy of this approach is demonstrated
using numerical examples and experiments.
Optimal control problems require a certain level of complexity of the

mathematical-physical models used therein to accurately reflect the cor-
responding real-world problem. As a result, the numerical optimization
problem used to represent such a task, is typically time-consuming to solve.
Moreover, certain parts of the problem setup may be subject to change and
XVIII Abstract
are therefore not known beforehand. A sudden parameter change in the

time between computing the optimal solution and the task execution using
this solution may result in a suboptimal, or even inappropriate system
behavior. The goal is of course to obtain a solution to the modified prob-
lem. Computing an exact optimal solution is typically not possible due to
large computation times, classical methods for finding approximate optimal
solutions produce – if at all – inadmissible results for larger perturbations.
As another contribution, this thesis presents a method that exhibits fast
convergence to an admissible, nearly optimal solution in the case of larger
parameter perturbations. Therein, a first-order approximation of an opti-
mal solution for a perturbed optimization problem is obtained by means
of parametric sensitivities. The possibility of an active set of constraints
different from that of the nominal solution is addressed which is the main
advantage over existing methods. The potential of this approach is shown
by means of a comparison with established methods as well as numerical
examples and corresponding experiments.
Kurzfassung
Diese Doktorarbeit behandelt optimale Bahn- und Trajektorienplanung für
serielle Roboter im Allgemeinen und Pfadfolgestrategien für kinematisch
redundante Roboter im Speziellen. Außerdem werden Näherungsmethoden
für optimale Lösungen solcher Planungsaufgaben entwickelt, die im Vergleich
zur exakten Lösungen eine bedeutend geringere Rechenzeit benötigen.
In dieser Arbeit werden – soweit für das Verständnis relevant – zunächst

die Grundlagen zur numerischen Optimierung, zu B-Spline-Kurven, sowie
die Vereinigung dieser Gebiete in Optimalsteuerungsproblemen in direkter
Formulierung untersucht. Zusätzlich wird eine genaue Untersuchung der
Kinematik von seriellen Robotern durchgeführt, wie sie in Pfadfolgepro-
blemen auftritt. Besonderes Augenmerk wird auf die Inverskinematik für
kinematisch redundante Roboter gelegt. Dabei wird numerische Optimie-
rung angewandt, um nicht nur irgendeine, sondern die optimale Lösung für
dieses Problem zu finden. Als einer der Beiträge dieser Arbeit wird eine
Methode vorgestellt, die für diese Aufgabestellung besonders gut geeignet
ist, da darin optimales Pfadfolgen und das darin enthaltene Inverskine-
matikproblem simultan optimal gelöst werden können. Dabei handelt es
sich um einen differentiellen Inverskinematikansatz, der mittels gewichteter
Basisvektoren des Nullraums der Jacobi-Matrix erweitert wird. Diese Vor-
gangsweise erlaubt es, die Möglichkeit für interne Bewegungen eines Systems
hinsichtlich des Optimierungsziels des übergeordneten Pfadfolgeproblems
direkt auszunutzen. Die Effektivität dieses Ansatzes wird mittels einiger
Rechenbeispiele und zugehöriger Experimente gezeigt.
Optimalsteuerungprobleme benötigen einen gewissen Detailgrad der dar-

in verwendeten mathematisch-physikalischen Modelle, um die zugehörige
Aufgabenstellung ausreichend genau abbilden zu können. Mit wachsender
Genauigkeit steigt allerdings häufig auch die zur Lösung des Problems
notwendige Rechenzeit an. Zusätzlich kann sich die Aufgabenstellung durch
XX Kurzfassung
sich ändernde Parameter beeinflusst werden, womit die Aufgabe zu einem

parameterabhängigen Optimalsteuerungsproblem wird. Wenn Parameter-
schwankungen im Zeitraum zwischen der Bestimmung der optimalen Lösung
und ihrer Ausführung liegen, so wird die neue Aufgabe im Allgemeinen
durch diese Lösung nur suboptimal oder garnicht gelöst. Das Problem für
die tatsächlich gültigen Parameter zu lösen ist oftmals aufgrund von Re-
chenzeiteinschränkungen nicht möglich. Einen Ausweg stellt eine Methode
dar, die in kurzer Rechenzeit eine zulässige Näherung der optimalen Lösung
des parameterabhängigen Optimalsteuerungsproblems liefert. Klassische
Methoden sind hier hinsichtlich der erlaubten Größe der zulässigen Para-
meterstörung für diese Aufgabenstellung nur eingeschränkt nutzbar. Einen
weiteren Beitrag dieser Arbeit stellt ein Ansatz dar, mit dessen Hilfe mit
vergleichsweise geringem Rechenaufwand eine zulässige Näherung erster
Ordnung der optimalen Lösung bestimmt werden kann. Diese wird mittels
parametrischer Sensitivitäten aus dem Optimierungsproblem für nominale
Parameterwerte berechnet. Der Ansatz zeichnet sich durch die Eigenschaft
aus, dass auch eine Veränderung der Menge der aktiven Nebenbedingungen
des Optimierungsproblems berücksichtigt werden kann, was einen Vorteil
gegenüber bestehenden Methoden darstellt. Die Wirksamkeit dieser Me-
thode wird durch Vergleichsrechnungen mit bewährten Methoden, sowie
durch weitere numerische Beispiele und deren experimentelle Umsetzung
demonstriert.
Chapter 1
Introduction
Due to the rapid progress in developing solutions for industrial applications
of robotic manipulators, today it is not sufficient to simply fulfill a given
task. Now it is required to solve a given problem in an optimal manner. To
this end, not only foundational knowledge about kinematics and dynamics
of multi-body systems and robotics sub-disciplines emerging therefrom are
required, it also necessitates expertise in numerical optimization.
The present thesis provides contributions to sub-fields of kinematics and

also of numerical optimization. In the former field, the inverse kinematics
for a specific class of robotics is investigated. In the latter, numerical
optimization, particular approximate solutions of parametric optimization
problems are discussed. In order to facilitate understanding of these topics,
basics regarding all required fields of knowledge are presented in brevity
and references to state-of-the-art literature are provided.
An important portion of the present thesis is devoted to kinematic redun-

dancy. In short, it is a property of the kinematic structure that robots
of certain types exhibit. It allows a robot to perform additional, internal
motion while executing a prescribed motion of its end-effector. That is, for
a given motion of a robot’s end-effector, the joint motion is ambiguous, thus
enabling additional criteria to be pursued in addition to end-effector path
tracking. The mention of kinematic redundancy in this thesis is twofold.
On the one hand, it is discussed in the kinematics chapter in Section 3.5
where its basic properties and consequences are reviewed. In particular,

A. Reiter, Optimal Path and Trajectory Planning for Serial Robots,
https://doi.org/10.1007/978-3-658-28594-4_1
2 1 Introduction
inverse kinematics methods for such kinematic structures are examined.

On the other hand, kinematic redundancy is exploited in course of optimal
path tracking in Chapter 5 to increase the task execution performance.
Outline
In Chapter 2, numerical basics relevant for the rest of the work are presented.
Therein, elementary knowledge regarding numerical optimization is repeated
in Section 2.1. In particular, parametric optimization problems are discussed
wherein an iterative scheme to compute approximate solutions to such
problem is proposed as one contribution of this work. Furthermore, B-
spline curves are reviewed in Section 2.2 which are then used for general
optimal control problems in Section 2.3.
Chapter 3 treats kinematic properties of serial robots, i.e. the problem

of forward kinematics as well as the more challenging inverse kinematics.
Therein, kinematically non-redundant and redundant manipulators are
considered and solutions on position-, velocity- and higher derivative level
are presented. A further contribution of this thesis is a particular approach
to compute a solution to the inverse kinematics problem for kinematically
redundant manipulators on derivative level.
Optimal path and trajectory planning, cf. Chapter 4, are robotic applica-
tions of the above. Therein, the evolution of a robot’s joint positions (and
by virtue of the kinematic chain) the end-effector, is computed to fulfill a
specified task, e.g. point-to-point motion. This is accomplished using dif-
ferent strategies, a single-step strategy wherein the geometric and temporal
aspects are treated simultaneously, and a multi-stage approach where these
subtasks are solved subsequently. Furthermore, in order to allow for rapid
computation of the above, the iterative approximation scheme for optimal
solutions using parametric sensitivities is applied.
Chapter 5 discusses optimal path tracking. In contrast to path and tra-

jectory planning, therein, a path is prescribed and needs to be tracked by
the robot’s end-effector in an optimal way. With the previously discussed
inverse kinematics for kinematically redundant manipulators and optimal
control theory, optimal solutions not only regarding the inverse kinematics
1 Introduction 3
resolution, but also for the path tracking problem are obtained. For kine-
matically non-redundant robots, neither the inverse kinematics, nor the
path tracking problem are in the scope of this thesis.
Chapter 6 provides final remarks on this thesis and concludes with a brief
summary of the work presented herein.
In Appendix A, an alternative parametrization of B-spline curves in terms

of the arc length is discussed. A software package that allows for evaluation
and construction of such functions is also proposed therein. Appendix B
presents additional information about the dynamics of multi-body systems,
their representation as a system of coupled differential-algebraic equations
and their numerical solution. Furthermore, a software library that facili-
tates code generation regarding the numerical solution of such problems is
introduced.
Examples
In order to ease understanding, theoretical parts are frequently comple-
mented by numerical examples of typical applications. The robots featured
therein are of various types, planar and spatial, kinematically non-redundant
and redundant, as well as industrial manipulators and lab prototypes. The
following brief overview provides an introduction to the robots mentioned
throughout this work.
Example 1.1: SCARA4 (description)

The SCARA4 manipulator is a kinematically redundant lab prototype
that is shown in Figures 1.1 and 1.2. It comprises of four links, serially
connected by four actuated revolute joints as well as a prismatic joint
with a gripper attachment. In this thesis, only the planar structure
with four revolute joints is considered, cf. the reduced presentation in
Figure 1.2. The vertical prismatic joint with the gripper attachment
are not part of the model.
4 1 Introduction
Figure 1.1: SCARA4 Lab Prototype. Figure 1.2: SCARA4 Planar Model.
Example 1.2: Stäubli TX90L (description)

The Stäubli TX90L is a spatial industrial robot, cf. Figure 1.3 that
consists of six revolute joints in a serial structure and features a spherical
wrist. In this setup it is placed on top of a linear axis, i.e. a prismatic
joint, establishing inherent kinematic redundancy (which can be disabled
by simply locking the prismatic joint).
Figure 1.3: Stäubli TX90L Industrial Manipulator On Linear Axis (Contour

Rendering).
Example 1.3: Comau Racer R3 (description)

The Comau Racer R3 is a kinematically inherently non-redundant,
spatial, industrial robot, cf. Figure 1.4 that consists of six revolute
joints in a serial structure and features a spherical wrist.
1 Introduction 5
Figure 1.4: Comau Racer R3 Industrial Manipulator (Contour Rendering).
In this thesis a number of numerical examples are discussed including their

respective computation times. These computations were implemented in
MATLAB R2015b on Windows running on an Intel Xeon E3-1245V3 CPU.
Aim & Contributions

The two main sets of contributions in this thesis are the following:
• Fast solution approximation using parametric sensitivities. In certain

applications, the computation time available to find a solution to a
complex optimization problem is limited, e.g. for a robotics path
trajectory planning task by a real-time system with a fixed cycle
time. To that end, in many cases, solutions to such problems cannot
be obtained by traditional optimization schemes which gives rise to
computationally less demanding methods that compute approxima-
tions to the solution of an optimization problem. Such an approach
requires an existing solution to a nominal problem as an initial guess
to which the actual situation presents a deviation or perturbation.
A frequent limitation to such approaches is the size of the allowed
deviation. This work proposes an iterative scheme using parametric
sensitivities that allows for large perturbations. Of course, it cannot
only be applied to above robotic tasks, cf. Chapter 4, but is applicable
to more general problems, cf. the (successful) comparison to more
established approaches in the academic examples from Section 2.1.3.
Publications of the author in this field are the conference papers [112,
116].
6 1 Introduction
• Optimal inverse kinematics and path tracking for kinematically re-

dundant manipulators. Optimal path tracking applications typically
include an inverse kinematics scheme that allows to compute joint an-
gles that correspond to a prescribed end-effector configuration. While
for kinematically non-redundant manipulators this does not pose a
problem, for redundant system it is more involved as infinitely many
joint configurations result in the same end-effector pose. This leads
to the question about the configuration that is the best, and how best
is understood in this case. While the user necessarily specifies the
latter when setting up the optimal path tracking problem, the former
can be solved in the numerical optimization therein. However, the
combination of both tasks, optimal inverse kinematics resolution in
optimal path tracking poses a challenging endeavor. As mentioned in
the above overview, a novel inverse kinematics approach is presented
in Section 3.5.3 of this thesis. It is called generalized nullspace aug-
mentation and allows direct parameterization of the internal motion
of the robot under consideration of the path tracking goal. This
method is successfully applied in the optimal path tracking example
in Chapter 5. This field was investigated by the author beginning
with his Master’s thesis [109, 110] and the conference paper [117]
resulting therefrom. In the conference papers [113, 114, 115] and the
journal paper [111] research on this topic was intensified.
Minor contributions that are a side product of the work leading up to this
thesis are:
• B-spline software package. Another contribution is a library to evalu-

ate and construct B-spline curves and surfaces which is documented
in Appendix A. In a similar manner, a method for curve fitting of
measurement data with periodic functions was contributed to [141]
in the context of trajectory generation for a robotic replacement for
therapeutic horseback riding.
• Code generation software package. For applications such as numerical
simulation, model-based control and parameter identification, typi-
cally the differential-algebraic equations governing a system’s behavior
1 Introduction 7
are derived symbolically. The contribution consists of a code genera-

tion utility that allows such models to be conveniently exported from
the Maple computer algebra software into the MATLAB/Simulink
simulation environment. This is discussed in Appendix B. The au-
thor’s publicactions in the field of modeling, model reduction and
control of multi-body dynamics of mechanical systems and system
complexity reduction are [63, 51, 84].
Chapter 2
Numerical Basics
In this chapter, the mathematical and numerical foundation of the subse-
quent chapters will be established.
First, a brief overview of optimization theory regarding gradient-based

optimization approaches is given. It includes definitions of relevant terms,
introduces formalisms which will be applied later and provides information
about the subfield of parametric optimization problems. A more detailed
treatment of these topics as well as applicactions can be found in textbooks,
e.g. [39].
In the following sections, B-spline curves are discussed. These constitute a

versatile, well-researched (most famously [103]), widely used function class
not only in robotics but also in many other fields. Not only basic definitions,
but also algorithms for evaluation and construction of these functions
are provided. Therein, special attention will be paid to construction by
constrained approximation (and interpolation) as this is an important tool
used throughout the present thesis.
Both topics, numerical optimization and B-spline curves, will then be

combined in the subsequent chapters in order to facilitate optimal path and
trajectory planning for robotic applications.
It is not an aim and not in the scope of this thesis to provide comprehensive
treatment of these well-researched subjects, but only to provide sufficient

https://doi.org/10.1007/978-3-658-28594-4_2
10 2 Numerical Basics
details for understanding the contribution of the presented work in the

following chapters.
2.1 Numerical Optimization

In the following brief summary of numerical optimization basics, proofs for
definitions are omitted as long as they are not constructive or otherwise
beneficial to the main subjects of this thesis. Interested readers are referred
to some of the many good works on this subject, e.g. on general numerical
optimization [92], on convex optimization [16], the textbook [39]. The
following definitions are largely adapted from these excellent works.
2.1.1 Convexity
Definition 2.1: Convex Set
A set S ⊆ Rn is called convex if a line connecting the points x, y ∈ S
lies entirely in S, i.e. x + k(y − x) = (1 − k)x + ky ∈ S ∀k ∈ [0, 1].
Definition 2.2: Convex Function

A function f : S → T is called convex if its domain S is a convex set
and f ((1 − k)x + ky) ≤ (1 − k)f (x) + kf (y) ∀k ∈ [0, 1] holds.
Definition 2.3: Concave Function

A function f : S → T is called concave if (−f ) is convex.
Example 2.1: Convex Function

Consider the plot of the function f (x) = x2 defined on the interval
x ∈ S = R as shown in Figure 2.1. As S is obviously a convex set and
f ((1 − k)x1 + kx2 ) ≤ (1 − k)f (x1 ) + kf (x2 )

((1 − k)x1 + kx2 )2 ≤ (1 − k)x21 + kx22
(1 − k)2 x21 + 2(1 − k)kx1 x2 + k 2 x22 ≤ (1 − k)x21 + kx22
0 ≤ k(1 − k) (x1 − x2 )2

≥0 ≥0
2.1 Numerical Optimization 11
with x1 , x2 ∈ S holds ∀k ∈ [0, 1], f is a convex function.

f (x)
1
0.8
0.6
0.4
0.2
0 x
−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1
Figure 2.1: A Convex Function.
2.1.2 Constrained Optimization

With the vector of optimization variables w a nonlinear programming (NLP)
problem is given as
minimize
w
J(w) (2.1.1a)
s.t. gi (w) = 0, i = 1, . . . , Neq (2.1.1b)
gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.1c)
which minimizes the objective function J(w) w.r.t. w while satisfying

the Neq equality constraints gi (w), i = 1, . . . , Neq and the Nineq inequality
constraints gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq that are collected in the
vector g(w) of dimension Ncon = Neq + Nineq . Therein, Neq is the number
of equality constraints and Nineq counts the inequality constraints. For the
following it will be assumed that J(w) and g(w) are twice continuously
differentiable w.r.t. w.
Definition 2.4: Feasible Set
The feasible set Ω is given as the set of points w which satisfy the
constraints (2.1.1b) and (2.1.1c), i.e.
Ω = {w : gi (w) = 0, i = 1, . . . , Neq ; gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq }.

Definition 2.5: Convex Programming Problem

An optimization problem (2.1.1) is convex if the feasible set Ω and the
objective function J : Ω → R are convex.
The feasible set is convex if the equality constraints are linear and the
inequality constraints are concave.
Definition 2.6: Active Constraints, Active Set

A constraint gi is called active at w∗ iff gi (w∗ ) = 0, otherwise inactive.
The vector collecting all active constraints is denoted ga which is of di-
mension N a . The index set A(w∗ ) ⊆ {1, . . . , Ncon } of active (in)equality
constraints is called the active set.
Note that this not only concerns inequality constraints (2.1.1c) but also
all equality constraints (2.1.1b) as they are considered to be always active.
The above definitions rely on the equality operator = to determine the
activity state of a constraint. In practice it is difficult to correctly identify a
constraint’s state due to numerical issues and limited accuracy in computer
implementations. This topic will be later picked up when parametric
sensitivities are discussed as the correct identification of the active set is of
eminent importance.
Definition 2.7: Linear Inequality Constraint Qualification (LICQ)

For a solution w∗ of (2.1.1) and an active set A(w∗ ) the LICQ holds
if the set of

the gradients
of the active constraints w.r.t. w, i.e.
∗ ∗
∂
∂w gi (w ) : i ∈ A(w ) , is linearly independent, i.e.
⎡ ⎤
∂ a ∗ ∂ a
ker⎣ g (w ) , . . . , g a (w∗ ) ⎦ = ∅.
∂w 1 ∂w N
Definition 2.8: Lagrange Function

The Lagrange function of the optimization problem (2.1.1) is given as
L(w, λ) = J(w) + λ g(w) (2.1.2)
with the vector of Lagrange multipliers λ of dimension Ncon (the

same dimension as the vector of constraints g).
Definition 2.9: Global Minimum

A point w∗ that is a (strict) global solution to the NLP problem (2.1.1)
is found if it is a (strict) global minimizer, i.e. J(w∗ ) ≤ (<)J(w)
∀w\{w∗ }.
Unfortunately, in many cases such global information is not available, often

due to non-convexity of J(w) w.r.t. w. However, in such cases, a local
qualification can be obtained.
Definition 2.10: (Strict) Local Solution

A point w∗ is a (strict) local solution to the above NLP problem if it is
a (strict) local minimizer, i.e. J(w∗ ) ≤ (<)J(w) ∀w\{w∗ } in a certain
neighborhood of w∗ .
The following necessary conditions must be satisfied by any solution w∗ to

(2.1.1). However, if a point w∗ satisfies these conditions, it not guaranteed
to be a solution.
Definition 2.11: First-Order Necessary Conditions, Local Minimizer
If w∗ is a local minimizer of J(w), and J(w) is continuously differentiable

w.r.t. w in an open neighborhood of w∗ , then ∂w ∂
J(w∗ ) = 0.
Definition 2.12: First-Order Necessary Conditions (Constrained

Case)
If w∗ is a local minimizer, and J(w) and g(w), and the LICQ holds at

w∗ , then there exist Lagrange multipliers λ∗ = λ∗1 λ∗2 . . . λ∗Ncon
such that

∂
L(w∗ , λ∗ ) = 0 (2.1.3a)
∂w
gi (w∗ ) = 0, i = 1, . . . , Neq (2.1.3b)
∗
gi (w ) ≤ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.3c)
λ∗i ≥ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.3d)
λ∗i gi (w ∗
) = 0, i = 1, . . . , Ncon (2.1.3e)
These requirements are also referred to as the Karush-Kuhn-Tucker

(KKT) conditions. Thus, points w∗ that satisfy the KKT conditions (2.1.3)
are also known as KKT points.
Condition (2.1.3a) states, that the gradient at the minimizer is zero, i.e. no
local first-order change occurs. A local solution satisfies the (in)equality
constraints which is reflected in the conditions (2.1.3b) and (2.1.3c). Con-
dition (2.1.3d) requires positive sign of the Lagrange multipliers λ. The
complementarity condition (2.1.3e) implies that either the multipliers λ∗i ,
or the constraints gi (w∗ ) (or both) are zero. Multipliers that correspond to
inactive inequality constraints are zero, thus condition (2.1.3a) yields

∂ ∂ ∂
L(w∗ , λ∗ ) = J(w∗ ) + λ∗i gi (w∗ ) = 0. (2.1.4)
∂w ∂w i∈A(w∗ ) ∂w
If exactly one of λ∗i and gi (w∗ ) is zero (and not both), (2.1.3e) is called
the strict complementarity condition. Note that this stronger condition
only applies to inequality constraints. If λ∗i > 0 holds for an inequality
constraint, it is called strictly active.
Definition 2.13: Second-Order Necessary Condition (Constrained

Case)
If w∗ is a local solution, the LICQ and the KKT conditions (2.1.3) hold
with the multipliers λ∗ , strict complementarity holds and the columns of
∂2
the matrix A(w∗ ) span ker ∂w ∂
g(w∗ ), then A (w∗ ) ∂w ∗ ∗ ∗
2 L(w , λ )A(w ) ≥
0.
2
∗ ∗
2 L(w , λ ) must be positive semi-
∂
This means that the Hessian matrix ∂w
∂
definite on the nullspace of the constraint Jacobian ∂w g(w). The expression
∗ ∂2 ∗ ∗ ∗
A (w ) ∂w2 L(w , λ )A(w ) is also called the reduced Hessian matrix.
If the sufficient conditions below are satisfied, the point w∗ is guaranteed

to be a solution to (2.1.1).
Definition 2.14: Second-Order Sufficient Conditions (Constrained

Case)
If at a point w∗ the LICQ and the KKT conditions (2.1.3) hold with the
multipliers λ∗ , strict complementarity holds and the columns of the ma-
∂2
trix A(w∗ ) are spanning ker ∂w
∂
g(w∗ ) and A (w∗ ) ∂w ∗ ∗ ∗
2 L(w , λ )A(w ) >
∗
0, then w is a local solution.
Note that these conditions are sufficient but not necessary, i.e. they do not
need to be met by a strict local minimizer.
2.1.3 Parametric Sensitivities

2.1.3.1 Parametric Nonlinear Programming Problems
In most practical cases, NLP problems such as (2.1.1) do not only involve
optimization variables w, but also – implicitly – other problem-specific
parameters p.
Consider, for example, one of the robots shown in the introductory Chapter 1.
In an optimal trajectory planning task, a load applied to its end-effector
(EE) could be regarded as a parameter that is subjected to uncertainties.
How does an optimal motion trajectory depend on the EE load and how
does it change for load variations?
While the solution w∗ of (2.1.1) is found for a specific, nominal parameter

setting pnom , the question remains how the optimal solution w∗ depends on
p and how it deviates for parameter changes. Research on such parametric,
i.e. parameter-dependent, NLP problems has been mainly initiated by [47,
46, 119].
For the following investigations, consider the parametric NLP problem
w∗ (p) = arg minimize

w
J(w, p) (2.1.5a)
s.t. gi (w, p) = 0, i = 1, . . . , Neq (2.1.5b)
gi (w, p) ≤ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.5c)
that is specified analogously to (2.1.1) but now includes explicit dependencies

of the vector of parameters p in the objective function J(w, p) and the vector
g(w, p) that collects the equality constraints (2.1.5b) and the inequality
constraints (2.1.5c). Also, the optimization result w∗ is now denoted with
a dependency of the parameters p in order to emphasize their impact.
By extending (2.1.2), the parametric Lagrange function is
L(w, λ, p) = J(w, p) + λ g(w, p), (2.1.6)
and similar to (2.1.3), the parametric KKT conditions are found to be

∂
L(w∗ , λ∗ , p) = 0 (2.1.7a)
∂w
gi (w∗ , p) = 0, i = 1, . . . , Neq (2.1.7b)
∗
gi (w , p) ≤ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.7c)
λ∗i ≥ 0, i = Neq + 1, . . . , Neq + Nineq (2.1.7d)
λ∗i gi (w∗ , p) = 0, i = 1, . . . , Ncon . (2.1.7e)
2.1.3.2 Derivation of Parametric Sensitivities

By considering the local solution w∗ , the KKT conditions (2.1.7a) to (2.1.7c)
can be rewritten as
⎛ ⎞
∗ ∗
⎜ ∂w L(w , λ , p) ⎟
∂
k(w∗ , λ∗ , p) = ⎝
a ∗
⎠
g (w , p)
⎛ ⎞
∗ a∗
⎜ ∂w J(w , p) + ∂w g (w, p) λ ⎟
∂ ∂ a
= ⎝
a ∗
⎠ =0 (2.1.8)
g (w , p)
wherein by virtue of strict complementarity only the active inequality

constraints are included that are collected in the vector ga (w∗ , p). Inactive
constraints are not relevant for the subsequent derivations.
By computing the total derivative of (2.1.8) w.r.t. the vector of parameters

p, the second-order expression
⎡ ⎤⎡ d ⎤
d ∂ 2
L(w∗ , λ∗ , p) ∂ a
g (w, p) w∗ ⎥
k(w∗ , λ∗ , p) = ⎢⎣ ∂w2∂ a ∗ ∂w ⎥⎢ dp
⎦⎣ d a ∗ ⎦ + ...
dp ∂w g (w , p) O dp λ

K(w∗ ,λ∗ ,p)
⎡ ⎤
∗ ∗
⎢ dp ∂w L(w , λ , p) ⎥
d ∂
+ ⎣
d a ∗
⎦ =O (2.1.9)
dp g (w , p)
is obtained that includes the KKT matrix K(w∗ , λ∗ , p) which is regular

d
at a solution. From this equation, the parametric sensitivities, i.e. dp w∗ ,
the partial derivatives of the optimal solution w.r.t. the parameters and
d a∗
dp λ , the partial derivatives of the optimal active multipliers w.r.t. the
parameters, are obtained by
⎡ ⎤ ⎡ ⎤
d ∗ ∗ ∗
⎢ dp ∂w L(w , λ , p) ⎥
d ∂
⎢ dp w ⎥ −1 ∗ ∗
⎣ d a∗⎦ = −K (w , λ , p)⎣ ⎦. (2.1.10)
dp λ
d a ∗
dp g (w , p)
2.1.3.3 First-Order Approximation of Optimal Solutions to

Perturbed Parametric NLPs
The above result gives rise to a first-order Taylor series approximation of
the optimal solution (w∗ , λ∗ ) for perturbed parameters ppert , i.e.
d ∗
w∗ (ppert ) ≈ ŵ∗ (ppert ) = w∗ (pnom ) + w (ppert − pnom ) (2.1.11)
dp
d a∗
λa∗ (ppert ) ≈ λ̂a∗ (ppert ) = λa∗ (pnom ) + λ (ppert − pnom ) . (2.1.12)
dp
Δp
Henceforth, the abbreviations (·)∗nom := (·)∗ (pnom ) and (·)∗pert := (·)∗ (ppert )
will be used to denote the optimal solution in the nominal and perturbed
case, respectively. A similar notation will be used for the approximate
quantities.

∗
Note that the solution estimate ŵpert , λ̂∗pert obtained by means of the first-
order approximations (2.1.11) and (2.1.12) is not necessarily admissible w.r.t.
the (in)equality constraints (2.1.5b) and (2.1.5c) imposed onto the original
NLP problem. Therefore the question arises how large the admissible
parameter perturbations Δp may become. The limitations of (2.1.11)
and (2.1.12) are given by the change of the active set of constraints due
to Δp. Note, that the vector of the estimate of optimal multipliers in the
perturbed case λ̂∗pert is found by calculating the active multiplier estimate
λa ∗ as in (2.1.12) and then inserting zeros for all inactive constraints.
Admissible Parameter Range Without Active Set Change

Therein, three variants may occur wherein the impact of perturbations of
single parameters on single constraints and their corresponding multipliers
is considered:
1. The trivial case in which a constraint gi , i = 1, . . . , Ncon maintains its

nominal activity state.
∗
2. An inactive constraint gi , i ∈
/ A(wnom ) may become active, i.e. enter
the active set which corresponds to the constraint residue becoming
zero. Therefore, a first-order approximation is employed, i.e. for only

a perturbation of the j-th element of the parameter vector p

∗
gi wpert , ppert ≈ gi (ŵ∗ (ppert ), ppert ) ≈ ĝi (ppert )
∗ d ∗
ĝi (ppert ) = gi (wnom , pnom ) + gi (wnom , pnom )(pj,pert − pj,nom )
dpj
(2.1.13)
∗
∗ ∂ ∗ dwnom
holds with dpdj gi (wnom , pnom ) = ∂w gi (wnom , pnom ) dpj +
∂ ∗
∂pj gi (wnom , pnom ). Note the sensitivity of the optimization variables
therein. By rearranging this equation an approximation p̂j,pert of the
perturbed j-th parameter is found such that the approximation of
the i-th constraint becomes active, i.e. ĝi (ppert ) = 0:
∗
gi (wnom , pnom )
p̂j,pert = pj,nom − d ∗
. (2.1.14)
dpj gi (wnom , pnom )
If the denominator in the second term vanishes, the parameter ap-

proximation becomes infinite. This means that the constraint gi does
not depend on pj and thus arbitrary perturbations of this parameter
are admissible.
3. An active constraint gi , i ∈ A(w∗ ) may become inactive, i.e. leave the
active set which corresponds to a multiplier becoming close to zero.
Assuming only a perturbation of the j-th element of the parameter
vector p, the first-order approximation (2.1.12) of the i-th multiplier
yields
dλ∗i (pnom )
λ̂∗i (ppert ) = λ∗i (pnom ) + (pj,pert − pj,nom ). (2.1.15)
dpj
Now the perturbed parameter pj,pert is computed that yields a corre-

sponding multiplier λ̂∗i (ppert ) = 0 that is considered inactive:
λ∗i (pnom )
p̂j,pert (λ∗i (pnom )) = pj,nom − dλ∗i (pnom )
. (2.1.16)
dpj
Again, if the denominator in the second term vanishes, the parameter

approximation becomes infinite as the active multiplier λi does not
exhibit a sensitivity w.r.t. pj and thus arbitrary perturbations of this
parameter are admissible.
In [22] the authors present a first-order approximation of the parameter

range that does not yield a change in the active set of constraints. In the
referenced work this is called the sensitivity domain which is somewhat
misleading as it refers not to the admissible domain of the sensitivities but
to the parameter range. In the present work, the term admissible range
of parameters will be used. A a hyperrectangular approximation of the
admissible parameter range is given by

pj,pert ∈Pj,pert = max({p̂j,pert (λ∗i (pnom )) < pj,nom } ∪ {−∞}),
i

min({p̂j,pert (λ∗i (pnom )) > pj,nom } ∪ {∞}) , i = 1, . . . , Ncon .
i
(2.1.17)
As only the impact of single parameters on the active set are considered
with the other parameters remaining at their respective nominal values, it is
not necessarily admissible to superpose ranges of perturbations of multiple
parameters in order to find the true admissible range.
However, in [22] the authors further propose a multi-step first-order so-

lution approximation that exploits knowledge of the admissible range of
parameters:
d ∗ d ∗
ŵ∗ (ppert ) = w∗ (pnom ) + w (pnom )Δp1 + ŵ (ppert,adm − p)Δp2
dp dp
(2.1.18)
d ∗ d ∗
λ̂∗ (ppert ) = λ∗ (pnom ) + λ (pnom )Δp1 + λ̂ (ppert,adm − p)Δp2 .
dp dp
(2.1.19)
Therein, the perturbation Δp = (ppert − p) = Δp1 + Δp2 is divided in two

parts, one up to the admissible range and the rest. Regarding the former part
Δp1 = (ppert,adm − p), i.e. the perturbation within the admissible range, the
approximations (2.1.11) and (2.1.12) are applied to compute the solution ap-
proximation at the bounds of the admissible parameter range. For the latter
part Δp2 = (ppert − ppert,adm ), i.e. the perturbation exceeding the admissi-
d
ble range, additionally sensitivities dp ŵ∗ (ppert,adm − p), dp
d ∗
λ̂ (ppert,adm − p)
are computed at the boundary point which are then used for a similar
approximation. Note that these sensitivities are directional sensitivities,
i.e. they are tangential to the hyperrectangle of constraint approximations.
They are obtained by modifying the KKT system (2.1.9) in order to reflect
the active set at the bounds of the admissible parameter range. Therefore
knowledge of the correspondence of the admissible range limits and the
responsible constraints is required which can be obtained as a byproduct
by (2.1.14) and (2.1.16).
Identification of Active Set

So far, identifying the active set of constraints, i.e. Definition 2.6, relies on
exact satisfaction of equalities. However, in practice, such comparisons are
unsuitable due to round-off errors and other accuracy issue resulting from
commonly used number systems in computer implementations. Therefore,
it is more practical to check if a certain quantity is in a range given by
the result of an identification function that corresponds to the considered
optimization problem.
In case of a perturbation the multipliers and the inequality constraint

residues inform about the active set and possible changes thereof. As
in numerical implementations comparisons against zero are impractical.
Instead, a nonzero, but small threshold value, given by an identification
function ρ(w, λ), should be used.
The active set
A(w, λ, p) = {i ∈ {1, . . . , Ncon } : gi (w, p) ≥ −ρ(w, λ, p)} (2.1.20)
and the set of strictly active constraints
A+ (w, λ, p) = {i ∈ A(w, λ, p) : λi ≥ ρ(w, λ, p)} (2.1.21)

are determined using the identification function [43]

⎛

⎞
⎜
⎝ ∂w
∂
L(w, λ, p) ⎟
ρ(w, λ, p) = ⎠ (2.1.22)
min(−g(w, p), λ)
wherein min(·, ·) : Rn × Rn → Rn is applied elementwise, or alternatively

[95]
⎛ ⎞
∂w L(w, p, λ)
∂
⎜ ⎟
⎜ ⎟
⎜ ⎟
⎜
⎜
g1 (w, p) ⎟
⎟
⎜ ⎟
⎜ g2 (w, p) ⎟
⎜ ⎟
⎜ .. ⎟
⎜ . ⎟
⎜ ⎟
⎜ ⎟
ρ(w, λ, p) = ⎜
⎜ gN eq
(w, p) ⎟
⎟ (2.1.23)
⎜ ⎟
⎜
⎜
⎜
min−gNeq +1 (w, p), λNeq +1 ⎟ ⎟
⎟
⎜ ⎟
⎜
⎜ min −gNeq +2 (w, p), λNeq +2 ⎟
⎟
⎜ .. ⎟
⎜ . ⎟
⎜ ⎟
⎝ ⎠
min −gNeq +Nineq (w, p), λNeq +Nineq 1
For the parametric NLP problems considered in this section, the function
from the referenced publications required extension by the parameters p,
i.e. ρ(w, λ, p).
A Special Case: Linear Constraint Perturbations

In preparation of the next section, dealing with restoration of admissibility
of solution approximations, consider the parametric NLP problem (2.1.5)
with additional parameters p
w∗ (p) = arg minimize

w
J(w, p) (2.1.24a)
s.t. g(w, p, p) = g(w, p) − p ≤ 0, i = 1, . . . , Ncon .
(2.1.24b)
Therein, p are considered as linear constraint perturbations themselves, i.e.

pnom = 0. The objective J is perturbation-free and the only perturbations
appear linearly in the (in)equality constraints.
For this special case, the parametric sensitivities (2.1.10) yield

⎡ ⎤ ⎡ ⎤
d ∗ ∗ ∗
⎢ dp ∂w L(w , λ , p, p) ⎥
d ∂
⎢ dp w ⎥ −1 ∗ ∗
⎣ d ∗⎦ = −K (w , λ , p)⎣ ⎦
dp λ
d a
dp g (w, p, p)
⎡ ⎤
O
= −K−1 (w∗ , λ∗ , p)⎣ ⎦ (2.1.25)
−I

∂
as ∂w L(w∗ , λ∗ , p, p) = ∂w∂
J(w∗ , p) + ∂w g (w∗ , p, p) λa ∗ is indepen-
∂ a
dent of p and g is linear in p. Note that also the KKT matrix K is

independent of the linear perturbation p, in fact, it is the same KKT
matrix as in the original parametric NLP problem, cf. (2.1.9).
Restoring Admissibility
The first-order approximations (2.1.11) and (2.1.12) yield a result that is
not necessarily admissible w.r.t. the (in)equality constraints. However, for
small perturbations (that do not change the active set) the stability of the
solution can be exploited by the Newton-type iterative Algorithm 1 in
order to restore admissibility, cf. [21].
Therein, inadmissible constraint residues are regarded as linear perturba-

tions of these constraints which gives rise to the application of the sensitivity
expressions from (2.1.25). Algorithm 1 assumes that a parametric NLP
problem (2.1.5) has already been solved for a set of nominal parameters
pnom and that parametric sensitivities dp d
w∗ and dpd ∗
λ are readily available.
Algorithm 1 Iterative Feedback Method From [21].

1: ŵ∗ (ppert ) ← w∗ (pnom ) + dp w∗ Δp first-order approximation (2.1.11)
d
2: λ̂∗ := λ̂∗ (ppert ) ← λ∗ (pnom ) + dp

d ∗
λ Δp first-order approximation
(2.1.12)
3: p ← ga (ŵ∗ , ppert
) residueof active constraints (2.1.5b) and (2.1.5c)
4: while p > ρ ŵ∗ , λ̂∗ , ppert do threshold (2.1.22)
dw∗
5: ŵ∗ ← ŵ∗ − dp p sensitivities (2.1.25)
∗ ∗ dλ∗
6: λ̂ ← λ̂ − dp p
7: end while
8: return ŵ
Note the difference in sign in the iterative update steps of Algorithm 1

compared to the approximations (2.1.11) and (2.1.12). These are a result
of the Newton-type iteration scheme that reduces the residues in the
active constraints. Convergence of this algorithm is guaranteed in a small
neighborhood of the nominal parameter pnom due to the stability of the
solution as shown in [20, 19].
As mentioned before, this method is limited to a constant set of active

constraints. This issue can be addressed by means of the first-order predictor
approach presented in [39]. Therein, an NLP is approximated by a quadratic
programming (QP) problem
1 ∂ 2 L(wnom , λnom , pnom )

Δw∗ = arg minimize Δw Δw+
Δw 2 ∂w2
∂J(wnom , pnom ) ∂ 2 L(wnom , λnom , pnom )
+ Δw + Δp Δw
∂w ∂w∂p
(2.1.26a)
∂g(wnom , pnom ) ∂g(wnom , pnom )
s.t. g(wnom , pnom ) + Δw + Δp ≤ 0
∂w ∂p
(2.1.26b)
yields the optimal solution increment Δw∗ that gives an approximation of

the optimization variables
ŵ∗ (ppert ) = wnom + Δw∗ (2.1.27)
in case of a parameter perturbation Δp. It can be shown that the KKT

conditions of the quadratic approximation (2.1.26) are the same as in (2.1.7).
The multipliers resulting from the QP problem (2.1.26) are the desired
approximations λ̂∗ (ppert ). The active set of constraints may be different from
A(wnom , λnom , pnom ) and may converge to A(wpert , λpert , ppert ) in certain
cases. However, in general it cannot be expected that the result obtained by
the approximation is admissible w.r.t. the (in)equality constraints imposed
onto the original NLP problem as the above QP does not include the original
constraints but only their first-order approximation at the nominal solution.
Another similar QP-based approach can be found in [71].
In contrast to Algorithm 1, Algorithm 2 allows for an active set change

which is an extension of Algorithm 1. In each iteration, the active set is
updated by observing the current state of the constraints. As detailed above,
a constraint can maintain its active state, i.e. remain active or inactive,
an active constraint may become inactive and an inactive constraint may
become active.
Algorithm 2 Improved Iterative Feedback Method, termed iterative+.
1: ŵ∗ := ŵ∗ (ppert ) ← w∗ (pnom ) + dp w∗ Δp
d
(2.1.11)
2: λ̂∗ := λ̂∗ (ppert ) ← λ∗ (pnom ) + dp d ∗
λ Δp (2.1.12)
∗
3: p ← g (ŵ , ppert
a

) residue
of active constraints (2.1.5b) and (2.1.5c)
∗ ∗
4: while p > ρ ŵ , λ̂ , ppert do threshold (2.1.22)

5: determine active set A ŵ∗ , λ̂∗ , ppert (2.1.20)
6: determine updated sensitivities
∗
7: ŵ∗ ← ŵ∗ − dw
dp p sensitivities (2.1.25)
∗ ∗ dλ∗
8: λ̂ ← λ̂ − dp p
9: end while
10: return ŵ
The differences of iterative+, i.e. Algorithm 2, to Algorithm 1 are found

in lines 5 and 6. In order to accommodate a changed active set, the new
active set must be determined first. This is facilitated by means of (2.1.20)
wherein the constraints are evaluated and compared with the identification
function from (2.1.22). Then the KKT matrix is updated such that the
constraint Jacobians therein reflect the current active set update which may
lead to a change of dimensions. In the final steps of every iteration, the
approximate optimization variables ŵ∗ and multipliers λ̂∗ are computed as
in the original scheme.
In case of convergence of iterative+, the result is admissible w.r.t. the

constraints. However, it is not necessarily optimal, i.e. in general ŵ∗ = w∗
and λ̂∗ = λ̂∗ .
It should be noted, that, by adding constraints to the active set, the LICQ
can be violated in certain cases. Therefore, possible LICQ violations may
be avoided by either pre-selecting constraints such that their gradients
may never be linearly dependent which may not be suitable for practical
applications. Alternatively, the optimization problem can be relaxed by
removing linearly dependent constraints from the iteration scheme, e.g. by
assigning priorities to constraints an applying a Gram-Schmidt process.
2.1.3.4 Comparison using Examples

The property of the methods presented above is analyzed using three
showcase examples presented in [22] and [39]. Therein, rather simple
parametric NLP problems are considered that already allow to capture the
most relevant properties of the methods.
Regarding the numerical solution, the implementation is facilitated by

means of CasADi 3.4.5 [2] together with MATLAB R2015b. As solvers,
qpOASES 3.2 [44] and Ipopt 3.12.3 [133] are employed for QP, and general
NLP problems, respectively. In detail, CasADi code generation which may
improve the execution time was not used for the functions implementing
the formalism required for the above approximation schemes. This may
especially improve the directional sensitivity-based scheme and the improved
iterative method as in the other approaches the most effort is required by
the optimization solvers which are readily available as compiled libraries.
Regarding the computation time for the exact solutions of the perturbed
problems, note that these were obtained using the nominal solution as the
initial guess for the non-quadratic problems.
Example 2.2: Parametric NLP 1, from [22]

The parametric NLP (QP)
w∗ = arg minimize
w
J(w) = (w1 + 1)2 + (w2 − 2)2 (2.1.28a)
⎛ ⎞ ⎛ ⎞
g1 −w1 + p ⎠
s.t. g(w, p) = ⎝ ⎠ = ⎝ ≤0 (2.1.28b)
g2 2w1 + w2

with the optimization variables w = w1 w2 and the parameter p.
Note that, as the objective function is quadratic and the inequality
constraints are linear in w, the NLP is convex which eases its solution.

For a nominal parameter pnom = 1 the solution wnom = 1 2 with the

multipliers λnom = 4 0 was found using qpOASES . The constraint

residues at the nominal solution are g = 0 −2 . Thus only constraint
g1 is found to be active while g2 is inactive, thus ga = g1 .
Next, the parametric sensitivities at the nominal solution are determined.
The Lagrange function is defined as
L(w, λ, p) = J(w) + λ g(w, p)

⎛ ⎞
2 2
−w1 + p ⎠
= (w1 + 1) + (w2 − 2) + λ1 λ2 ⎝ .
2w1 + w2
The derivatives
⎛ ⎞ ⎡ ⎤
∂ 2(w1 + 1)⎠ ∂ −1 0⎦
J(w, p) =⎝ g(w, p) = ⎣
∂w 2(w2 − 2) ∂w 2 1
and the KKT conditions

⎛ ⎞
∗ ∗
⎜ ∂w L(w , λ , p) ⎟
∂
k(w∗ , λ∗ , p) = ⎝
a ∗
⎠ =
g (w , p)
⎛ ⎞
⎛ ⎞
∗

a∗
2(w1 + 1) − λ1 ⎟
⎜
⎜ ∂w J(w , p) + ∂w g (w, p) λ ⎟
∂ ∂ a
⎜
= ⎝
a ∗
⎠ = ⎜
⎝
2(w2 − 2) ⎟ ⎟
⎠
=0
g (w , p)
−w1 + p wnom
λnom
yield the parametric KKT system

⎡ ⎤⎡ d ⎤
2 ∗
d ∂
L(w∗ , λ∗ , p) ∂ a
∂w g (w, p) ⎥ ⎢ dp w ⎥
k(w∗ , λ∗ , p) = ⎢⎣ ∂w2∂ a ⎦⎣ d ∗ ⎦ + . . .
dp ∂w g (w, p) O dp λ
⎡ ⎤ ⎡ ⎤
⎡ ⎤

∗ ∗
⎡
2 0 −1⎥ d ∗ ⎤ ⎢0⎥
⎢ dp ∂w L(w , λ , p) ⎥
d ∂ ⎢ w
⎢
+ ⎣ ⎦ = ⎢ 0 2 0 ⎥⎥⎦⎢⎣ dp ⎥
d ∗⎦ + ⎢
⎢ ⎥
0⎥ = 0
d a
dp g (w, p)
⎣ λ ⎣ ⎦
−1 0 0 dp 1
and thus the parametric sensitivities

⎡ ⎤−1 ⎡ ⎤ ⎡ ⎤
⎡ ⎤
d ∗ ⎢
2 0 −1⎥ 0
⎢ ⎥
1
⎢ ⎥
⎢ dp w ⎥
⎣ d ∗⎦ = −⎢⎢⎣0 2 0 ⎥⎥⎦ ⎢ ⎥
⎢0⎥ = ⎢ ⎥
⎢0⎥.
dp λ
⎣ ⎦ ⎣ ⎦
−1 0 0 1 2
The admissible range of parameter without a change of the active

set of constraints is found to be Ppert = [−1, 2]. As for a perturbed
parameter inside this range easily can be accounted for by application
of Algorithm 1, only a perturbation outside this range is considered for
evaluating the above algorithms.
Table 2.1: Parametric QP from [22]: Results for ppert = 2.5.
w λ g admissible tCPU in ms

exact 2.5 1 11 2 · 12.1

directional λ = 11 8 12.5
QP 11.5

iterative+ λ = 11 9 12.1
From the results in Table 2.1 it can be seen that for ppert = 2.5 the
approach based on the QP approximation (2.1.26) converges to the exact
optimal solution. The first-order approximations (2.1.18) and (2.1.19)
based on directional sensitivities also yields the exact solution for the
optimization variables as well as a deviation of the multipliers which is
also the case for the iterative+ method.
All methods produce results that are admissible w.r.t. the constraints
imposed onto the optimization problem. For this example, the QP
is the best solution approximation method as it yields the optimal
solution (which is also admissible) and requires the least computation
time. However, in this particular case, the original NLP problem is
a QP problem of the same dimension as its QP approximation and
thus requires similar computational effort to obtain an exact solution.
Ultimately, computing the exact solution for this rather small problem
(with or without perturbations) is the best option in this case.
Example 2.3: Parametric NLP 2, (16.10) in [39]

The parametric NLP
1 1
w∗ = arg minimize
w
J(w) = (w1 − 1)2 + (w2 − 2)2 (2.1.29a)
2 2
s.t. g(w, p) = 4w12 + w22 − p2 ≤ 0 (2.1.29b)

with the optimization variables w = w1 w2 and the parameter p.
For

a nominal parameter pnom = 2.2 the optimal solution wnom =

0.6551 1.767 is found using Ipopt. The constraint g is active, the
corresponding multiplier is λnom = 0.06582. The parametric sensitivities
at the nominal solution are
⎛ ⎞
d ∗ ⎝0.5203⎠ d ∗
w = λ = −0.1516.
dp 0.4734 dp
For a perturbed parameter ppert = 2.5 the results of the exact solution
as well as for its approximations are reported in Table 2.2.
Table 2.2: Parametric NLP (16.10) from [39]: Results for ppert = 2.5, tCPU in ms.
w λ J(w) act. set g admiss. tCPU

0.9839
exact 1.6221 × 10−4 2.042 × 10−3 · · 219
1.992

0.9843
directional −30.08 × 10−3 2.052 × 10−3 10.9
2.067

0.8810
QP 0 7.443 × 10−3 11.2
1.973

0.9556
iterative+ −23.92 × 10−3 2.360 × 10−3 14.6
2.046
While the approximation based on directional derivatives is the fastest

to compute, its result is not admissible w.r.t. the constraint g. The
QP-based scheme yields a result that is admissible, it fails to estimate

the correct active set. Each approximate result requires less than 7 %
of the solution to the exact NLP to compute. Although the improved
iterative scheme requires the most computation time, it yields a result
that is not only admissible but also yields an objective function value
close to the exact solution of the perturbed problem and features the
correct active set of constraints. However, it yields a multiplier that
is negative and thus not admissible w.r.t. the KKT condition (2.1.7d)
which is not necessary as it is only a solution approximation.
Example 2.4: Parametric NLP 3, (16.12) in [39]

The convex parametric NLP
1 1 1
w∗ = arg minimize
w
J(w, p) = (w1 − p1 )2 + (w2 − p1 )2 + w1 w2
2 2 2
(2.1.30a)
s.t. g(w, p) = w12 + 5w22 − p2 ≤ 0 (2.1.30b)

with the optimization variables w = w1 w2 and the parameter

p = p1 p2 . For a nominal parameter pnom = 0.25 0.25 the optimal

solution is found at wnom = 0.1667 0.1667 with Ipopt. The only
constraint is inactive, its residue is g = −0.08333 with the corresponding
multiplier λnom = 0. The parametric sensitivities at the nominal solution
are
⎡ ⎤
d ∗ ⎣0.6667 0⎦
w = .
dp 0.6667 0
d ∗
As the only constraint is inactive, sensitivities dp λ do not exist. Fur-
d ∗
thermore, the second column of dp w is zero as p2 is only included in the
inequality constraint g which is inactive at the nominal solution. The
admissible range of parameters is Ppert = (−∞, 0.3125] × [0.1667, ∞).

In this example, the perturbed parameter is set to ppert = 0.25 0
where only the second element changes that forces the inequality con-
straint g to become active. The results of the exact solution as well as

for its approximations are reported in Table 2.3.
Table 2.3: [Param. NLP (16.12) from [39]: Results for p1,pert = 0.25, p2,pert = 0. tCPU
in ms.
w λ J(w, p) act. set g admiss. tCPU

1 × 10−4
exact 1369 62.47 × 10−3 · · 547
0

0.1667
directional 5 × 10−5 61.10 × 10−3 13.9
0.1667

0.2024
QP 53.57 × 10−3 25.3 × 10−3 11.1
0.05952

5 × 10−3
iterative+ 5.215 20.83 × 10−3 30.2
6 × 10−4
While all methods correctly estimate the active set, the improved it-
erative scheme is the only one whose result is admissible w.r.t. the
constraints as the others work only with first-order constraint approxi-
mations but without the constraint functions themselves. Due to this
feature, in this example the improved iterative scheme is superior to
the other methods and also to the exact solution as it requires only 6 %
of its computation time and yields a similar result.
Summarizing, the numerical examples presented above demonstrate that

the improved iterative Algorithm 2 can act as a suitable alternative to
the exact computation of solutions to perturbed parameter NLP problems.
More complex applications will be presented in the context of fast robot
path and trajectory planning required in the case of changing conditions in
Chapter 4.
For the solution of the above NLP examples, the interior-point solver Ipopt
[133] was employed. Its enhancement, sIpopt [106], is also capable of
computing solution approximations for parameter perturbations by means
of parameter sensitivities. However, results obtained with sIpopt were
found to be inadmissible, i.e. violate inequality constraints in the perturbed
case. This behavior was confirmed by the developers [13] as the software
– in constrast to the iterative+ method from Algorithm 2 in the present
thesis – cannot properly handle active set changes.
2.2 B-Spline Curves

B-spline curves are piecewise polynomials that find wide application in the
field of robotics, e.g. in path and trajectory planning. Archival publications
in this field are by de Boor [33] and Cox [32]. Although the fundamentals
of B-spline curves are well-known, they are reproduced here for convenience
as far as they will be used at multiple points throughout this thesis. For a
comprehensive overview of theory and applications of B-spline curves the
interested reader is referred to e.g. [103] from which the notation regarding
this matter was largely adopted in this thesis.
2.2.1 Evaluation Algorithm

This section is devoted to evaluating a B-spline curve q(t) in terms of its
parameter t. Therefore, first its definition is provided in a recursive form
which is useful for evaluation. Then, the formulation is further developed
into a more compact notation that leads to computational benefits at the
price of degraded numerical accuracy.
Recursive Formulation, Summation Representation

A B-spline curve q(t) of degree p is defined as
n

q(t) = Nj,p (t)dj (2.2.1)
j=0
with the basis functions Nj,p (t) recursively defined as
t − tj tj+p+1 − t
Nj,p (t) = Nj,p−1 (t) + Nj+1,p−1 (t) (2.2.2a)
tj+p − tj tj+p+1 − tj+1
⎧
⎨ 1 t ≤ t < t
j j+1
Nj,0 (t) = ⎩ (2.2.2b)
0 otherwise
which are piecewise polynomials. It follows that Nj,p (t) = 0, t ∈ / [tj , tj+p+1 ),
and consequently, only the functions Nj−p,p (t), . . . , Nj,p (t) are non-zero on
the knot interval [tj , tj+p+1 ). This fact can be exploited in the evaluation
of (2.2.1). The curve q(t) is defined for t ∈ [a, b] on which m + 1 knots
2.2 B-Spline Curves 33
tj , j = 0, . . . , m are defined. Regarding the distribution of knots, note the

following terminology:
• clamped = non-periodic = open vs. unclamped = periodic = closed:

The term open refers to knots with a distribution in which the first
and last knots have multiplicity p + 1, i.e.
a = t0 = t1 = · · · = tp ≤ tp+1 ≤ . . .
· · · ≤ tm−p−1 ≤ tm−p = · · · = tm−1 = tm = b.
• uniform vs. non-uniform: Uniform describes a knot distribution in

which intermediate knots are equally spaced, multiplicities greater
than 1 are allowed for boundary knots. The above open distribution
would be uniform if
tm − t0
tj+1 = tj + Δt, j = p, . . . , m − p − 1 with Δt =
m − 2p
Most splines considered in the following are chosen to be open-uniform for

simplicity.
Relevant properties
• The curve q(t) is arbitrarily continuously differentiable on each knot

interval except for the knots (with multiplicity k) at which continuous
differentiability is degraded to C p−k .
• The outmost control points are interpolated , i.e. q(a) = d0 and
q(b) = dn .
• It can be shown, that the curve is locally contained in the range
of the $surrounding control
% points (local convex hull property), i.e.
q(t) ∈ min dj−p , max dj holds for t ∈ [tj , tj+1 ). Consequently, gob-
j $ j %
ally, q(t) ∈ min dj , max dj holds (global convex hull property).
j j
Matrix-Vector Representation
By replacing the sum in (2.2.1) with a vector product, the curve can be
computed as
q(t) = n (t)d. (2.2.3)
wherein the basis function vector is denoted n(t) and the vector of control
points is d with

n (t) = N0,p (t) N1,p (t) · · · Nn−1,p (t) Nn,p (t)
and

d = d0 d1 · · · dn−1 dn .
Having the basis functions (2.2.2) explicitly evaluated, their polynomial

form is found as
Nj,p = nj,0 (t) + nj,1 (t)t + · · · + nj,p (t)tp

⎛ ⎞
⎜
1
⎟
⎜ ⎟
⎜ t ⎟
⎜
⎜ ⎟
= nj,0 (t) nj,1 (t) · · · nj,p (t) ⎜ . ⎟
.
⎜ . ⎟
⎟ (2.2.4)
⎜ ⎟
⎜ n−1 ⎟
⎜t ⎟
⎝ ⎠
n
t

τ (t)
with the vector of parameter powers τ (t) and the coefficients nj,i , i = 0, . . . , p
which are piecewise constant on each knot interval. Now (2.2.3) can be
rewritten as
q(t) = τ (t)N(t)d (2.2.5)

with the p + 1 × n + 1 coefficient matrix (time-dependency omitted)

⎡ ⎤
⎢n0,0 n1,0 · · · nn,0 ⎥
⎢ ⎥
⎢n0,1
⎢ n1,1 · · · nn,1 ⎥⎥
N(t) = ⎢ . .. . . . ... ⎥⎥
⎢ .
⎢ . . ⎥
⎣ ⎦
n0,p n1,p · · · nn,p
that is constant on each knot interval. Note that it is also sparse due to
the limited parameter domain with non-zero values of each basis function
Nj,p (t).
This matrix-vector representation offers the advantage that the coefficient

matrices N(t) can be precomputed as they only depend on the degree
and the knots. In fact, even shifting or scaling the parameter domain has
no influence if the vector τ of parameter powers is scaled appropriately.
The only operations required for evaluation of q(t) are finding the knot
interval that contains the requested parameter (e.g. with binary search),
t ∈ [tk , tk+1 ), computing τ , and performing the sparse vector-matrix-vector
multiplication (2.2.5).
However, this notation exhibits numerical disadvantages arising from the

difference in magnitude of the coefficients stored in N(t). A possible conse-
quence is degraded continuity at knots which deteriorates with increasing
spline degree. For numerical accuracy, [103] suggests to use the recursive
formulation (2.2.2) to evaluate B-spline curves by (2.2.1).
2.2.2 Derivatives w.r.t. the Path Parameter

Summation Representation
Deriving a B-spline curve with degree p (2.2.1) w.r.t. the parameter t
results in another B-spline
d n−1
(1)
q(t) = q (1) (t) = Nj+1,p−1 (t)dj (2.2.6)
dt j=0
with degree p − 1. The n control points of the resulting spline (in contrast
to the n + 1 control points of the original spline) are
(1) p
dj = (dj+1 − dj ), j = 0, . . . , n − 1 (2.2.7)
tj+p+1 − tj+1
which are linear combinations of the control points of the original B-spline
curve. Furthermore, the boundary knots are dropped, i.e. m − 1 knots
(1)
remain. The control points dj can be collected in the vector d(1) =

(1) (1) (1)
d0 d1 . . . dn−1 .
This scheme can of course be repeated for higher derivatives in a similar
manner, e.g. to compute the control points d(2) of the second-derivative
B-spline curve.
Matrix-Vector Representation
Derivation of (2.2.5) w.r.t. t is straightforward as effectively only the vector
of parameter powers needs to be derived as the coefficient matrix is constant
on each interval. With
d
τ (t) = 0 1 · · · (n − 1)tn−2 ntn−1
dt ⎡ ⎤
⎢0 · · · · · · · · · · · · · · · 0⎥ ⎛ 1 ⎞
⎢ ... .. ⎥⎥ ⎜ ⎟
⎢1 .⎥ ⎜ ⎟
⎢ ⎜ t ⎟
⎢ ⎥⎜ ⎟
⎢ .. ⎥⎥ ⎜ 2 ⎟
⎢0
⎢ 2 ... .⎥ ⎜ ⎜ t ⎟
⎟
⎢ .. ⎥⎥ ⎜ .. ⎟
= ⎢⎢ ... ... 3 ... . ⎥⎥ ⎜ ⎜ . ⎟
⎟
⎢
⎢ ..
⎢.
... ... ... .. ⎥⎥ ⎜ ⎜ n−2 ⎟
t
⎟
⎢ .⎥ ⎜ ⎜
⎟
⎟
⎢. . . . n − 1 . . . ... ⎥⎥ ⎜ ⎟
⎢. ⎜tn−1 ⎟
⎢. ⎥⎝ ⎠
⎣ ⎦
0 ··· ··· ··· 0 n 0 tn

T τ (t)
d2
τ (t) = TTτ (t)
dt2
..
.
the derivatives of (2.2.5) yield
d
q(t) = τ (t)T N(t)d (2.2.8a)
dt
d2
q(t) = τ (t)T T N(t)d (2.2.8b)
dt2
..
.
Of course, derivatives vanish for orders k exceeding the degree p, i.e.

dk
dtk q(t) = 0 for k > p.
2.2.3 Derivatives w.r.t. the Control Points

Deriving a B-spline curve q(t) w.r.t. the vector of control points d can be
conveniently expressed by the matrix-vector representation discussed above,
i.e.
d
q(t) = τ (t)N(t). (2.2.9)
dd
Derivatives of a B-spline curve w.r.t. the parameter t, cf. (2.2.8), can be
derived w.r.t. the vector of control points in an analogous manner. For
example, for the first derivative, the results is
d d
q(t) = τ (t)T N(t). (2.2.10)
dd dt
While the properties and applications described in this section are fairly
commonly known in robotics, a software library that includes additional
functionality was developed. A brief overview of features can be found in
Appendix A.
2.2.4 Constrained Approximation

While the above sections detail how to evaluate a B-spline curve and
its derivatives, this section is devoted to constructing a curve. Therein,
constrained B-spline approximation is a valuable tool, especially in optimal
path and trajectory planning. For example, for many practical optimization
problems, an initial guess that is at least admissible w.r.t. the constraints

benefits the computation time required to solve the problem. B-splines can
be used to construct such a guess by means of a constrained approximation
problem. Therein, the control points collected in the vector d act as
optimization variables and are adjusted such that they satisfy certain
requirements.
Pure approximation, i.e. computing a curve that fits best for a given set of
points, can be formulated as a linear programming (LP) problem. Here,
it is formalized as a QP in order to perform quadratic curve fitting. A
given point q a,i should be fitted by the curve that assumes the value q(ti )
at a given parameter ti . Approximation points are collected in the vector
qa should be fitted by the curve that assumes the values qa at the same
respective parameter values. The same holds for the curve derivatives, e.g.
for the first derivative qa(1) ≈ q(1)
a .
If at a given parameter ti the curve should be constrained to a prescribed in-

terval, q c,min ≤ qc ≤ q c,max , pure approximation is turned into a constrained
approximation problem. Constrained points are subsequently collected in
vectors qc for the basic curve, qc(1) for its first derivative and so on.
Smoothing of the curve is achieved by additionally minimizing the highest

relevant derivatives in form of the corresponding derivative B-spline curve
control points.
Possible inputs for an approximation scheme is point data to be approxi-

mated, but also interpolation constraints. Also, limits of the magnitude of
the spline and its derivatives can be efficiently included using the convex
hull property. For a given spline degree and knot distribution, the goal is
to find a suitable vector of control points d. Therefore the corresponding

constrained approximation problem is stated as the QP
1 1
minimize (qa − qa ) (qa − qa ) + q(1) a − qa(1) q(1) a − qa(1) + · · · +
d 2 2
1 (p−1)
(p−1) (p−1)
1
+ qa − qa qa − qa(p−1) + αd(p) d(p) (2.2.11a)
2 2
s.t. qc,min ≤ qc ≤ qc,max (2.2.11b)
(1)
qc,min ≤ qc(1) ≤ q(1)
c,max (2.2.11c)
..
.
(p−1)
qc,min ≤ qc(p−1) ≤ q(p−1)
c,max (2.2.11d)
1q min ≤ d ≤ 1q max (2.2.11e)
(1)
1q min ≤d
(1)
≤ 1q (1)
max (2.2.11f)
..
.
(p)
1q min ≤ d(p) ≤ 1q (p)
max (2.2.11g)
in which the mixed objective (2.2.11a) is to be minimized. Therein, the

pointwise approximation

residue for

all derivatives from 0, i.e. (qa − qa ),
up to p − 1, i.e. qa (p−1)
− qa(p−1)
, as well as the control points d(p) of
the p-th, the highest non-zero derivative is quadratically minimized. The
latter term increases the smoothness of the result which can be adjusted
with the weight α ≥ 0. For α = 0, pure constrained approximation is
performed, which in practice typically yields high magnitudes of the spline
derivatives. In (2.2.11b) to (2.2.11d) pointwise ineqality constraints of all
derivatives are collected. Interpolation is achieved by setting both, the
respective lower and upper bound to the required value which results in
an equality constraint. The inequality constraints (2.2.11e) to (2.2.11g)
represent convex hull constraints of the spline derivatives, i.e. the magnitude
of the respective spline derivatives is limited by the specified bounds. The
vector 1 denotes a vector of ones of appropriate dimension.
The above constrained approximation QP problem (2.2.11) can be solved

with QP solvers such as qpOASES [44]. Typically, it is therefore required
to extract the optimization variables, i.e. in this case the control points d
from the expressions listed in the QP. The implementation of such problems,
the matrix-vector notation (2.2.5) and (2.2.8) can be employed to express

pointwise approximation and constraint data in (2.2.11a) and (2.2.11b) to
(2.2.11d), respectively. For the convex hull part of the objective (2.2.11a)
and the constraints (2.2.11e) to (2.2.11g) the relationship (2.2.6) can be
exploited.
2.3 Optimal Control Problems and Direct

Methods
In optimal control problems, the best control trajectory u(t) is sought that
drives the system towards its goal which can be, e.g., a point-to-point motion
of a robot or a path tracking application. Several types of constraints may
therein be imposed onto the system such as input and state constraints.
In this context, the word best is used synonymously with optimal. Thus not
only a control trajectory u(t) that solves the main challenge is required, it
needs to stand out from all other possible trajectories in a quantitative man-
ner. The latter qualification can be expressed by minimizing an objective
function within an numerical optimization problem, cf. Section 2.1.
As a result of the selection of a control trajectory u(t), a state trajectory

x(t) is found that explicitly describes the time evolution of the system under
consideration. Frequently, both, control and state trajectories are treated
simultaneously, enabling efficient solution algorithms. However, it is also
possible to construct the time evolution of the state from the time evolution
of the control by time integration of the forward dynamics. Conversely, the
control trajectory can be reconstructed by means of inverse dynamics from
the state trajectory (in the case of redundantly actuated systems further
considerations are required). Textbooks on optimal control are e.g. [11] or
[39].
2.3 Optimal Control Problems and Direct Methods 41
An optimal control problem (OCP) is formulated as
&T
minimize L(x(t), u(t))dt + E(x(T )) (2.3.1a)
x(t),u(t)
0
s.t. x0 − x(0) = 0 (2.3.1b)
r(x(T )) ≤ 0 (2.3.1c)
ẋ(t) − f (x(t), u(t)) = 0 (2.3.1d)
h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ] (2.3.1e)
wherein the state x and the control u are functions of time t ∈ [0, T ]
and serve as optimization variables. The terminal time is denoted T . The
objective consists of the integral cost L(x(t), u(t)), referred to as Lagrange
term, and the terminal cost E(x(T )), named the Mayer term. This
particular combination results in a so-called objective of Bolza type. The
OCP is subjected to initial and terminal constraints, (2.3.1b) and (2.3.1c),
respectively, as well as the system dynamics (2.3.1d) and constraints (2.3.1e)
that apply during the path. While (2.3.1b) is a constraint that is linear w.r.t.
the (inital) state, the other constraints are in general nonlinear w.r.t. state
and control. The terminal time T may be fixed as denoted in the above
OCP, or free through reparametrization of t and with T as the terminal
value of an additional state xT = t with its time derivative ẋT = 1.
The central paradigm of direct methods is that before optimization tech-

niques are employed, the original infinite dimensional OCP (2.3.1) is dis-
cretized, yielding a finite dimensional problem. Therefore, such methods
are also known as discretize first, then optimize approaches. Therein, the
time domain is split into N time intervals with N + 1 increasing knots
t0 = 0 < t1 < t2 < · · · < tN = T . Without loss of generality, in this
thesis the considered time interval is [0, T ], other values can be realized by
introducing a time shift. Regarding the state and control trajectories, x(t)
and u(t), respectively, suitable parameterizations are required to complete
the discretization.
Below, the most common direct methods, i.e. single and multiple shooting,
as well as collocation, will be discussed. Therein, the shorthand (·)j := (·)(tj )
is used for expressing points along state and control trajectories.
2.3.1 Direct Single Shooting

The concept of direct single shooting1 (DSS) originates from [60] where it
was introduced in the context of general optimal control. While such OCPs
are mostly defined in time domain, the concept of DSS can be applied to
problem sets in different domains. The infinite dimensional OCP (2.3.1) in
DSS formulation can be written as
&T
minimize L(x(t), u(t))dt + E(x(T )) (2.3.2a)
u(t)
0
s.t. r(x(T )) ≤ 0 (2.3.2b)
&T
x(t) = x(0) + f (x(t), u(t))dt (2.3.2c)
0
h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ]. (2.3.2d)
The considered system is only influenced by the control u(t), therefore the
constraint (2.3.1b) on the initial state is no longer included. The OCP
(2.3.2) is constrained by the system dynamics (2.3.1d), included in (2.3.2c)
as numerical simulation by forward time integration, starting at the initial
state x(0). Therefore the state depends nonlinearly from the input. Thus,
even constraints h linear w.r.t. the state may then depend nonlinearly from
the states and from the input.
The OCP (2.3.2) is discretized by parametrizing the control u(t) using

variables

wu = u 0 u 1 · · · u N .
1 In the textbook [11] the author reports (without reference), that the name shooting stems
from a control example in which a cannon is required be optimally aimed to shoot at a target.
The constraints (2.3.2d) can be typically not enforced over the whole interval
[0, T ] but are checked on Nch discrete checkpoints tk ∈ [0, T ], k = 1, . . . , Nch .
The resulting finitely dimensional OCP thus yields
&T
minimize
w
L(x(t), u(t))dt + E(x(T )) (2.3.3a)
u
0
s.t. r(x(T )) ≤ 0 (2.3.3b)
&T
x(t) = x(0) + f (x(t), u(t))dt (2.3.3c)
0
h(x(tk ), u(tk )) ≤ 0, tk ∈ [0, T ], k = 1, . . . , Nch (2.3.3d)
with the time integration in (2.3.2c) and (2.3.3c) replaced by a numerical

scheme suitable for the system dynamics.
Regarding the practical implementation of such a DSS OCP, for gradient-

based optimization derivatives of the objective and the constraints are
required which are best obtained by automatic differentiation. Control
parameterizations wu may represent piecewise constant, or piecewise poly-
nomial functions, e.g. B-spline curves from Section 2.2. In these cases the
effects of each element of wu will be typically limited to a few time intervals
which is reflected in the sparsity of the resulting derivative matrices. The
above choices typically lead to increased sparsity compared to function
classes for which parameters impact the control function over all time
intervals, e.g. Fourier series.
2.3.2 Direct Multiple Shooting

In contrast to DSS, where the control variables are the only optimization
variables, direct multiple shooting (DMS) additionally incorporates the
states, cf. (2.3.1). This concept was introduced in [15].
Both, the variables representing the states and the controls at certain points
in time are collected in the vector

w = x 0 u 0 x 1 u 1 · · · x N u N .
Note the alternating order of states and controls which is motivated by

improving the sparsity of the resulting derivatives that can be exploited by
optimization solvers for computation time reduction.
The OCP (2.3.1) is thus rewritten in finite dimensional form as
N
&tj
minimize
w
L(x(t), u(t))dt + E(xN ) (2.3.4a)
j=1 tj−1
s.t. x0 − x0 = 0 (2.3.4b)
r(xN ) ≤ 0 (2.3.4c)
&tj
xj − f (x(t), u(t))dt = 0, j = 1, . . . , N (2.3.4d)
tj−1
h(xk , uk ) ≤ 0, k = 1, . . . , Nch . (2.3.4e)
While the objective (2.3.4a) is equivalent to (2.3.1a), it was rewritten so that

it now reflects the state trajectory split at the interval bounds. The equality
constraint (2.3.4d) maintains continuity of the state integration between
intervals that was previously ensured by the integration over the entire
interval. Therein, the integral sign again represents a suitable numerical
time integration scheme. Instead of enforcing the system dynamics as an
equality constraint (2.3.1d), they are incorporated in (2.3.4d).
The additional state variables allow for shorter integration time intervals,
reducing numerical errors occurring for longer times. They also enable
direct access to the system behavior, allowing to handle especially unstable
systems and systems with high sensitivity w.r.t. the initial value well.
While the dimension of the resulting KKT system is significantly increased
compared to DSS, its sparsity is improved as intermediate states have no
explicit dependency on all previous states and controls.
The fact that the integration over an interval does not depend on the
result of any of its predecessors enables simultaneous computation of all
integrations which allows for parallel computing.
While a basic DMS method can be implemented in less than 100 lines of code,
an enhanced and particularly efficient tool to solve OCPs is MUSCOD-II
[75].
2.3.3 Direct Collocation

As for the shooting methods, the time domain is discretized. It is split up
into N intervals at the N + 1 collocation points t0 = 0 < t1 < t2 < · · · <
tN = T . In collocation methods, the state integration process that has
been handled by numerical integration schemes in the shooting methods is
now written in terms of piecewise polynomials that interpolate the state
along time. The time evolution of the state x(t) and the control u(t) are
parametrized using wx and wu , respectively.
The corresponding NLP problem reads
N
&tj
minimize
w ,w
L(x(t), u(t))dt + E(xN ) (2.3.5a)
x u
j=1 tj−1
s.t. x0 − x0,0 = 0 (2.3.5b)

r(xN ) ≤ 0 (2.3.5c)
ẋ(tj,i ) − f (x(tj,i ), u(tj,i )) = 0, j = 0, . . . , N − 1, i = 0, . . . , p
(2.3.5d)
xj,p − x(tj,p ) = 0, j = 0, . . . , N − 1 (2.3.5e)
h(xk , uk ) ≤ 0, k = 1, . . . , Nch (2.3.5f)
wherein elements of the state x(t) are represented by polynomials of degree p,

thus p + 1 coefficients are available on each collocation interval [tj , tj+1 ), j =
0, . . . , N −1. These polynomials are used to interpolate the system dynamics
at M + 1 knots tj,0 , . . . , tj,M on each subinterval, cf. (2.3.5d), as well as the
state optimization variables at the collocation points tj , cf. (2.3.5e). For
example, Legendre polynomials can be used for this interpolation task.
The state values at each knot xj,i are collected as

wx = x 0,0 x 0,1 · · · x 0,M x 1,0 · · · x N −1,M −1 x N −1,M .
The elements of the control u(t) are again parametrized by variables wu ,

e.g. stair function levels for piecewise constant parametrization, control
values at collocation points that are linearly interpolated, or coefficients of
polynomials, e.g. B-spline curves.
The objective (2.3.5a) is again evaluated using a numerical integration

scheme, preferably reusing the states and controls evaluated at the col-
location points. Again, initial and terminal conditions are incorporated
using the constraints (2.3.5b) and (2.3.5c), respectively. In addition, the
(in)equality constraints (2.3.5f) are enforced at Nch check points which may
also coincide with the collocation points.
Direct Collocation with B-Spline Curves

This section is devoted to a specific form of piecewise polynomials that can
be used for state interpolation: B-spline curves, cf. Section 2.2. In this case
the control points dxi , i = 1, . . . , n serve as the optimization variables

wx = d x1 d x2 . . . d xn ,
parametrizing the time evolution of the state x(t). The control trajectory
u(t) is still parametrized in terms of wu .
The NLP problem (2.3.5) accordingly modified to accommodate B-spline

curves (2.2.1) is
N
&tj
minimize
w ,w
L(x(t), u(t))dt + E(xN ) (2.3.6a)
x u
j=1 tj−1
s.t. x0 − x0 = 0 (2.3.6b)
r(xN ) ≤ 0 (2.3.6c)
ẋj − f (xj , uj ) = 0, j = 0, . . . , N − 1 (2.3.6d)
h(xk , uk ) ≤ 0, k = 1, . . . , Nch (2.3.6e)
A particularly notable advantage of B-spline curves is their convex hull

property, i.e. that a curve and its derivatives (which are splines, too)
are contained in the range of their respective control points. It can be
conveniently exploited as linear path constraints to box-constrain the state

variables or their derivatives. Note, that the total number of equality
constraints (2.3.6b) to (2.3.6d) regarding the time evolution of the state
must not exceed the total number of state control points, otherwise the
NLP problem would be overconstrained.
In this thesis, direct collocation with B-spline curves is used for trajectory
planning in Chapter 4.
Numerical Solution
In order to compute a solution to a practical OCP, the considered system
behavior first needs to be formalized in terms of the equations govern-
ing relevant physical processes in a dynamics model. Furthermore, the
optimization goal needs to be expressed in terms of dynamics quantities.
Then, the above OCP formulations are applied to obtain a formulation
ready for numerical solution. Software frameworks used to implement such
formulations are CasADi [2] or YALMIP [79]. The numerical solution is
obtained with an appropriately selected solver program such as Ipopt [133]
for general NLP problems, or qpOASES [44] for QP problems.
Chapter 3
Kinematics of Serial
Robots
Kinematics describes the motion of a system of one or more bodies by
considering their position, velocity, acceleration and possibly higher deriva-
tives, i.e. disregarding forces and torques acting upon them. As this field
is well-researched, the present chapter does not aim to provide a compre-
hensive discussion of existing methods. Interested readers are referred to
general robotics textbooks such as [127] while advanced readers may prefer
publications focusing on kinematics such as [86]. However, the approaches
most relevant for the successive chapters are discussed in great level of
detail.
Depending on the complexity of the considered task and the manipulator

executing the desired motion, kinematically non-redundant and redundant
robots are distinguished. While for non-redundant serial structures solutions
to the forward and inverse kinematics problems are well known and can
thus be considered solved, the inverse kinematics problem for redundant
robots still poses a challenge in research. Consequently, a large portion
of the following section is devoted to that inverse kinematics problem. It
is treated with geometric, iterative and optimization-based methods for
both, kinematically non-redundant, and also redundant structures. After
well-known differential inverse kinematics methodologies are revisited as
an introduction, in particular the joint space decomposition method is

https://doi.org/10.1007/978-3-658-28594-4_3
50 3 Kinematics of Serial Robots
reviewed in detail as it poses a convenient approach to optimization-based

redundancy resolution methods, cf. the optimal path tracking applications
in Chapter 5.
One of the main contributions of this thesis is contained in Section 3.5.3

that generalizes differential inverse kinematics approaches for kinematically
redundant robots. Therein, velocity Jacobian nullspace augmentation, most
notably by means of the projected gradient method, but also joint space
decomposition are covered within a unifying framework.
In this section, at first, basic definitions and concepts are presented as

detailed as required for the subsequent presentation of kinematics rela-
tionships on position, velocitiy and higher derivative level in forward and
inverse form are discussed thereafter.
3.1 Position and Orientation

3.1.1 Position
The position of a point (B) relative to a point of reference (A) is denoted
by the position vector rAB , cf. Figure 3.1.
(B)
(A)
Figure 3.1: Position Vector from Point (A) to Point (B).
For practical use, such position vectors need to be expressed in coordinates

w.r.t. a frame of reference. An elementwise representation of rAB w.r.t. a
frame denoted R using Cartesian coordinates is
⎛ ⎞
x
⎜R AB ⎟
⎜ ⎟
R rAB = ⎜ R yAB ⎟
⎝ ⎠
∈ R3 (3.1.1)
R zAB
3.1 Position and Orientation 51
wherein R xAB , R yAB and R zAB are the distances between A and B measured
in the x, y and z directions of the R frame, respectively. The index on
the left, here R, denotes the frame in whose coordinates the vector is
represented in.
Other forms of position representations commonly used in robotics use

cylindrical or spherical coordinates. However, for the applications discussed
in this thesis description by means of Cartesian coordinates is sufficient.
3.1.2 Orientation
Rotation Matrix
Orientation describes the relative rotation between two coordinate frames
which can be uniquely represented by a rotation matrix RRS . Such a
construct not only characterizes the relative rotation between the two coor-
dinate frames indexed with R and S, it is also a coordinate transformation
between these frames. Therefore, the rotation matrix
' (
RRS = R (S e1 ) R (S e2 ) R (S e3 ) ∈ SO(3) (3.1.2)
comprises three mutually orthogonal column vectors that are the unit vectors
aligned with the principal directions of frame S resolved in coordinates of
the R system. It is an element of the special orthogonal group in three
dimensions SO(3). Note that this representation is redundant, i.e. its nine
parameters are more than necessary to fully describe the rotation.
Example 3.1: Coordinate Transformation using Rotation Matrix

For a position vector from point A to point B in coordinate representa-
tion w.r.t. the frame 2, i.e. 2 rAB , the representation w.r.t. the frame 1
is obtained as
1 rAB = R12 2 rAB . (3.1.3)
In the larger part of this thesis this rotation matrix representation will be
used for expressing general coordinate transformations as well as orientations
of coordinate frame, in particular a robot’s EE orientation. However, for
orientation error considerations, also an angle-axis representation will be

utilized, cf. Section 3.5.2. Therein, a relative orientation is represented by a
rotation with an angle ϕ about an axis denoted by a unit vector uR pointing
in its direction, thus this representation consists of four parameters of which
only three are independent as |uR | = 1. For the identical transformation
R = I the angle yields ϕ = 0, thus no rotation occurs. Note that the
angle-axis representation is not unique as R(uR , ϕ) = R(−uR , −ϕ)
Quaternions
Another way of uniquely representing orientation are quaternions which
extend the complex numbers. As this representation will not be used in the
present work, only this brief overview is given and the reader is referred to
textbooks such as [86].
Similar to complex numbers that can represent planar rotations on the unit
circle in the complex plane, quaternions represent spatial rotations on the
unit sphere. Using four numbers, a quaternion
⎛ ⎞
q
⎜ 1⎟
⎜ ⎟
Q = (q0 , q) with q = ⎜q2 ⎟,
⎝ ⎠
qi ∈ R, i = 1, . . . , 3
q3
can be used to represent any point in SO(3). Therein, q0 is called the scalar
compontent, and q is called the vector component of the quaternion Q. It is
called a unit quaternion if q = 1 which was assumed in the introductory
explanation using the unit sphere.
Successive Rotations
In contrast to the above representations that provide a unique orientation
representation, spacial rotations can also be expressed by means of three
successive elementary rotations about alternating axes of the accordingly
3.2 Translation and Rotation Velocity 53
moving coordinate frame, or of a spatially fixed frame. These rotations are

quantified by sets of three angles denoted
⎛ ⎞
ϕ
⎜ 1⎟
⎜ ⎟
ϕ= ⎜ϕ2 ⎟,
⎝ ⎠
(3.1.4)
ϕ3
such as Euler angles, Kardan angles about axes of moving frames, or roll-
pitch-yaw angles about axes of fixed frames. There are in total 12 different
rotation orders. For the inverse problem, i.e. finding angles ϕi , i = {1, 2, 3},
all of these representations suffer from singularity issues if rotations about
the first and last axis cannot be distinguished.
3.2 Translation and Rotation Velocity

The rate of change the position vector of point B, denoted in coordinates
w.r.t. an inertially fixed frame of reference 0, is given by its time derivative
d
0 vB = 0r (3.2.1)
dt B
= 0 ṙB .
For a frame of reference R that is fixed to a body in motion, this relationship

can be reused as
d
R vB = RR0 (R0RR rB )
dt
d d
= RR0 (R0R ) 0 rB + RR0 R0R R rB
dt dt
I
=:R ω̃0R
d
= R r + R ω̃0R 0 rB (3.2.2)
dt B
= R ṙB + R ω̃0R 0 rB
with the spin tensor1 of the angular velocity R ω0R of the frame R w.r.t. the
inertially fixed frame 0.
Note that the angular velocity ω is in general different from the time
derivative of an orientation representation by means of a vector ϕ comprising
three angles, such as Euler angles, cf. Section 3.1.2.
The translation velocity of a point (E) on a body and the angular velocity
of a body-fixed frame, indicated by the index E, are collected in the twist
vector
⎛ ⎞
ωE ⎠
⎝
VE = . (3.2.3)
vE
3.3 Joint Space, Work Space, Task Space

3.3.1 Joint Space
The joint space of a robot composed of nR revolute joints and nP prismatic
joints is given as the variety C ⊆ RnP × TnR . For prismatic joints, the
corresponding joint coordinate qi ∈ R describes the joint position along a
directed axis. In the case of revolute joints, the joint coordinate qi ∈ T
gives the joint position on the topological space of a 1-torus T1 , sometimes
denoted as 1-sphere S 1 .
In practice, a robot’s joint space is obtained by sweeping its admissible

joint ranges and constructing C by evaluating the above variety.
Example 3.2: SCARA4 (joint space)

For the SCARA4 manipulator depicted in Figure 1.2, the joint space is
given by C = T4 with joint limits neglected. When the respective lower
and upper joint limits qi,min and qi,max , i = 1 . . . 4, respectively, need to
)
4
be considered, it reduces to C = [qi,min , qi,max ] ⊂ T4 .
i=1
⎛
⎞ ⎡ ⎤
ωx 0 −ωz ωy
1 spin tensor: ω̃ = ⎜ω ⎟ = ⎢ ω ⎥
−ωx ⎦
⎝ y⎠ ⎣ z 0
ωz −ωy ωx 0
3.3 Joint Space, Work Space, Task Space 55
Example 3.3: Stäubli TX90L (joint space)

In case of the Stäubli TX90L industrial robot mounted on a linear axis
shown in Figure 1.3, the joint space is given by the variety C = R × T6
with joint limits neglected. When the respective lower and upper joint
limits qi,min and qi,max , i = 1 . . . 7, respectively, need to be considered, it
)
7
reduces to C = [qi,min , qi,max ] ⊂ R × T6 .
i=1
3.3.2 Work Space

As described in [86], the work space of a robot is the set of EE configurations,
i.e. EE position rE and EE orientation RE , that can be reached by some
choice of joint positions q, i.e.
W = {(rE (q), RE (q))|q ∈ C} ⊂ SE(3). (3.3.1)
The reachable work space is the set of positions the EE can assume with
some orientation, i.e.
WR = {rE (q)|q ∈ C} ⊂ R3 . (3.3.2)
Such points may include intermediate points of via paths, where the orien-
tation of the EE is often irrelevant. Determining WR is a simple task as
only the joint coordinates need to be swept in their entire admissible range
while recording the respective resulting EE positions.
Example 3.4: SCARA4 (reachable work space)
In order to find the reachable work space WR of the SCARA4 manip-
ulator (with joint limits neglected), it is sufficient to first consider all
points the EE can assume along one radial line (here assumed as the
0 x axis w.l.o.g.). The rest of the reachable work space is then found by
circularly sweeping these reachable points around the first joint.
While the innermost reachable point is at radius l1 − l2 + l3 − l4 and thus

coincident with point (0), the outermost reachable point is at radius
rout = l1 + l2 + l3 + l4 , cf. Figure 3.2.
0y
WR
rout
(E)
(0) 0x
Figure 3.2: Reachable Work Space of SCARA4 Manipulator.
The dexterous work space is the set of positions the EE can assume with
arbitrary orientation, i.e.
WD = {rE (q)|∀RE (q) ∈ SO(3)∃q ∈ C} ⊂ R3 . (3.3.3)
Dexterous points are relevant for, e.g. machining tasks in which one
particular point is required to be accessible from all directions, or robot-
based photography for which arbitrary EE orientation is also of significance.
In contrast to the rather effortless task of finding WR , deriving WD is more
involved.
A notable academic example is an unconstrained manipulator with a spheri-

cal wrist. For this particular kinematic structure, the dexterous work space
is composed of all points that lie in the reachable work space of the wrist
point (not the EE), thus WD ⊂ WR . However, real manipulators often do
not have a dexterous work space at all, i.e. WD = ∅, due to joint limits or
self-collisions.
3.3 Joint Space, Work Space, Task Space 57
Example 3.5: SCARA4 (dexterous work space)

In order to find the dexterous work space WD of the SCARA4 manipu-
lator (with joint limits and self-collisions neglected), it is again sufficient
to consider all points the EE can assume along the 0 x with arbitrary
orientation. The rest of the dexterous work space is found by circularly
sweeping these dexterous points about the first joint axis.
The innermost dexterous point is coincident with point (0) (radius

l1 − l2 − l3 + l4 ), the outermost reachable point is at radius l1 + l2 + l3 + l4 ,
cf. Figure 3.3.
0y
rout WD
(E)
(0) 0x
Figure 3.3: Dexterous Work Space of SCARA4 Manipulator.
In general, understanding of work space is especially relevant for path

placement and path planning tasks in course of the preparations required
for operating a robot system. However, not all subtasks therein require
knowing an explicit representation of the entire spaces. In many cases, it is
sufficient to only be able to check whether or not a given EE configuration
(or part thereof) is in a specific type of work space. If an EE position is in the
dexterous work space, the existence of a corresponding joint configuration
is guaranteed.
Path planning involves choosing a suitable sequence of EE configurations

such that a given task is fulfilled. A subtask therein may be path placement,
i.e. a full path is prescribed and its relative position and orientation w.r.t.
the robot needs to be determined. This is a common process in industrial
applications in which a given contour needs to be tracked such as grinding,
or polishing. In this case, the evolution of the joint configuration for all
points along a desired path need to be determined. While in many cases
both, EE position and orientation, are prescribed, cf. path tracking in
Chapter 5, this is not necessary. In certain cases, e.g. intermediate steps in
path planning, some elements of the EE configuration are deliberately left
free, e.g. in order to allow for path optimization. Consider in this context,
for example, a machining task in which the rotation about the tool axis is
left free, cf. [115].
In any case, once path planning is completed for a specific task, the operation
of the manipulator is restricted to the task space WT ⊆ W. WT denotes
the EE space necessary to execute a given task.
However, before any planning schemes regarding paths or trajectories can

commence, elementary kinematic relationships between the a robot’s base
(which in this thesis is always considered fixed) and its EE must first be
established.
3.4 Forward Kinematics

3.4.1 Position-Level Forward Kinematics
The forward kinematics f : C → W of a robot is a mapping from its joint
space C to its work space W. More specifically, a robot’s EE configuration is
given in terms of the Cartesian EE position rE ∈ R3 and the EE orientation,
denoted as the rotation matrix R0E ∈ SO(3). While the former, rE , is the
position vector of the EE w.r.t. an inertially fixed point, the latter, R0E ,
is the coordinate transformation between a coordinate frame fixed to the
EE body and an inertially fixed frame of reference. Both quantities are
3.4 Forward Kinematics 59
expressed depending on the joint configuration of the robot, denoted as the

vector of joint positions q ∈ C, i.e.
zE (q) = (R0E (q), rE (q)) = f (q) ∈ SE(3). (3.4.1)
Note that not only the joint positions are relevant for the result of the
forward kinematics mapping, also constant geometric parameters along the
serial kinematic structure of the manipulator under consideration such as
link lengths are required.
In addition, the EE orientation may be expressed in terms of three angles

defining successive rotations, cf. (3.1.4), i.e.
⎛ ⎞
ϕE (q)⎠
zE (q) = ⎝ = f (q) ∈ SE(3). (3.4.2)
rE (q)
3.4.2 Velocity-Level and Higher-Order Forward

Kinematics
On velocity-level the forward kinematics describes the pose-dependent
relationship between the EE velocity, i.e. its angular velocity ωE and
its translation velocity vE , and the joint velocity q̇. This is conveniently
expressed by the EE twist
⎛ ⎞
⎝
ωE (q, q̇) ⎠
VE = . (3.4.3)
vE (q, q̇)
As VE is linear w.r.t. the joint velocity q̇, the geometric Jacobian matrix
∂VE
J(q) = (3.4.4)
∂ q̇
can be used to find the equivalent forward kinematics representation
VE = J(q)q̇. (3.4.5)
It should be noted that for practical calculations the EE twist VE is

expressed w.r.t. a frame of reference, thus the resulting Jacobian J is also
frame-dependent.
With the EE pose expressed using angles describing successive rotation ϕE ,

e.g. Euler angles, and the position vector rE , the analytical Jacobian
⎛ ⎞
E ∂ ϕ (q)
∂zE (q) ⎜ ∂q
JA (q) = = ⎝ ∂ rE (q) ⎟
⎠ (3.4.6)
∂q ∂q
is used to express the velocity-level forward kinematics as

⎛ ⎞
⎝
ϕ̇E ⎠
= JA (q)q̇. (3.4.7)
vE
Note that, in general, the EE rotation velocity expressed by the derivatives

of the successive angles ϕ̇E is different from the angular EE velocity ωE .
Consequently, the analytical Jacobian JA is in general different from the
geometric Jacobian J, cf. (3.4.5). A linear transformation between ϕ̇E and
ωE and thus JA and J can be found.
For alternative holistic kinematics representation using screw coordinates,

the reader is referred to textbooks such as [86].
The following example means to illustrate forward kinematics by regarding

a planar manipulator.
Example 3.6: SCARA4 (forward kinematics)

Consider the planar SCARA4 manipulator depicted in Figure 3.4.
Therein, a relative coordinate description is chosen, i.e. the joint angles
q2 , q3 , q4 are relative w.r.t. the respective previous link.
The EE configuration is expressed by means of the rotation matrix R0E ,

i.e. the coordinate transformation from the frame E attached to the EE
body to the inertially fixed frame 0, and the position vector 0 rE from
the origin to the EE resolved in coordinates of the I frame:
zE (q) = (R0E (q), 0 rE (q))

3.4 Forward Kinematics 61
with
⎡ ⎤ ⎛ ⎞
*
4 *
4 *
4 *
i
⎢cos qi − sin qi 0⎥ ⎜
⎜i=1
li cos qj ⎟
⎢ i=1 i=1 ⎥ ⎜ j=1 ⎟ ⎟
⎢ ⎥ ⎜* ⎟
R0E (q) = ⎢ *
4 *
4 ⎥ and ⎜
4 *
i
⎟.
⎢ sin qi cos qi 0⎥⎥ 0 rE (q) = ⎜ l sin q ⎟
⎢ ⎜ i=1 i j ⎟
⎣ i=1 i=1 ⎦ ⎝ j=1 ⎠
0 0 1 0
The velocity forward kinematics of this planar model is expressed as

the twist
⎛ ⎞
ω (q, q̇)⎠
⎝0 E
0 VE =
0 vE (q, q̇)
= J(q)q̇
with 0 ωE ∈ R and 0 vE ∈ R2 which is represented in terms of the 0-frame

with
4

0 ωE (q, q̇) = q̇i
i=1
and
⎛ ⎞
*4 *i *i
⎜− li sin q j q̇ j ⎟
d0 rE (q) ⎜ i=1
⎜ j=1 j=1 ⎟ ⎟
0 vE (q, q̇) = = ⎜ *4 *
i *i ⎟
⎝
dt li cos qj q̇j ⎠
i=1 j=1 j=1
and therefore
⎡
⎢
1 1 ···
⎢ *
4 *
i *
4 *
i
⎢
J(q) =
∂ 0 VE
= ⎢
⎢
− li sin qj − li sin qj ···
⎢ i=1 j=1 i=2 j=1
∂ q̇ ⎢ *
4 *
i *
4 *
i
⎣
li cos qj li cos qj ···
i=1 j=1 i=2 j=1
⎤
··· 1 1 ⎥
*
4 *
i *
4 ⎥
··· − li sin qj −l4 sin qj ⎥⎥⎥.
i=3 j=1 j=1 ⎥
*
4 *
i *
4 ⎥
⎦
··· li cos qj l4 cos qj
i=3 j=1 j=1
By regarding the Jacobian J it is obvious that columns become linearly

dependent if any of the joint angles qi = 0. However, if only one or two
columns are linearly dependent, the Jacobian is still full rank which is
limited by the number of its lines.
0y Ex
(E) q4
l4 q3
Ey
l3 q2
l2
l1 q1
0x
(0)
Figure 3.4: Forward Kinematics of the SCARA4 Manipulator.
3.5 Inverse Kinematics

Inverse kinematics (IK) represents a mapping from a robot’s EE configu-
ration to its joint positions, i.e. given the position and orientation of the
EE, the joint configuration is determined accordingly. While the forward
kinematics of a serial robot can be derived straightforwardly, the IK is
typically more involved as in general a system of nonlinear equations needs
to be solved.
Alternatively, the kinematics for some structures situation can be analyzed

by regarding the geometric properties of the kinematic structure. Of course,
as for forward kinematics, derivatives w.r.t. to time such as velocities and
accelerations can also be considered accordingly. Overviews of IK methods
are provided in [90, 125, 23].
In this thesis, the kinematics in general, but especially the IK of kinemati-

cally redundant manipulators are of particular interest.
3.5 Inverse Kinematics 63
3.5.1 Kinematically Redundant Manipulators

Kinematical redundancy of serial robots describes the property that the
robot has more degrees of freedom than necessary to assume a given EE
configuration. Several types of redundancy have been documented in the
past, a concise overview of useful defintions is given in [31] from which the
following statements are reproduced for convenience. The most noteworthy
fact about IK for kinematically redundant manipulators is that in general
there is an infinite amount of solutions to the IK problem.
Definition 3.1: (Inherent) Kinematical Redundancy

A manipulator is considered kinematically redundant if its work space
dimension is lower than its joint configuration space dimension, i.e.
m := dim W < n := dim C, and the work space is contained in the
configuration space in its entirety, i.e. W ⊂ C. Sometimes, such
a structure is also called inherently kinematically redundant as this
property is built into the robot and does not depend on the task to be
executed.
From this definition it is obvious that a spatial robot is kinematically

redundant if n = dim C > 6 and a planar manipulator is kinematically
redundant if n = dim C > 3.
Definition 3.2: Degree of Kinematical Redundancy
The degree of kinematical redundancy is given by r = dim C − dim W =
n − m.
Example 3.7: Stäubli TX90L (Kinematical Redundancy)

The (spatial) Stäubli TX90L industrial robot from Figure 1.3 is inher-
ently kinematically redundant as it has 7 joints with n = dim C = 7.
Example 3.8: SCARA4 (Kinematical Redundancy)

The (planar) SCARA4 robot from Figure 1.2 is inherently kinematically
redundant as it has 4 joints with n = dim C = 4. Its degree of kinematical
redundancy is r = dim C − dim W = 4 − 3 = 1.

The (planar) SCARA3 robot that will be later considered in Figure 3.5
is not inherently kinematically redundant as is has 3 joints with n =
dim C = 3.
Definition 3.3: Task Redundancy

A manipulator is considered kinematically redundant w.r.t. its assigned
task if its task space dimension is lower than its joint configuration
space dimension, i.e. l = dim T < n = dim C.
A consequence of this definition is of course, that all inherently kinematically

redundant manipulators are also task redundant, but not necessarily vice
versa.
Definition 3.4: Degree of Task Redundancy
The degree of task redundancy is given by rT = dim C − dim T = n − l.

Although the (planar) SCARA3 robot from Figure 3.5 is not inherently
kinematically redundant, it is task redundant if its task is limited
to pure positioning of the EE without regarding its orientation, i.e.
l = dim T = 2. In this case the degree of redundancy is rT = dim C −
dim T = n − l = 3 − 2 = 1.
In the following, IK schemes for kinematically non-redundant and redundant

structures are presented. Therein, for many cases, IK resolution schemes
for non-redundant kinematics are a special case of the redundant ones.
Approaches operating on position-level as well as differential IK methods,
i.e. methods on velocity, acceleration-level, etc., are detailed below.
3.5.2 Position-Level Inverse Kinematics

On position-level, an IK scheme yields joint configuration that depend on
the EE configuration. As apparent from forward kinematics, cf. Section 3.4,
kinematics relationships are nonlinear in most cases, not allowing for a
straight-forward inversion and therefore often requiring iterative, numerical
methods. However, in certain cases a symbolic solution can be found by
regarding the geometric properties of the structure.
Also depending on the robot’s structure, more than one joint configuration
can correspond to a particular admissible EE configuration. Depending
on the manipulator and the desired EE configuration, there may be one
single solution, or multiple or no solutions at all to the IK problem on
position-level. However, for kinematically redundant structures, an infinite
amount of configurations correspond to a given EE pose. Of course, now
the question arises which configuration is the best and should therefore be
desired. And also, what does the best actually mean in this case?
Closed-Form Solutions
In rare cases, such an IK relationship can be found in closed-form, i.e.
in terms of functions and mathematical operations, excluding iterations
and time integration. This is facilitated by means of thorough investiga-
tion of a manipulator’s geometry, which of course is only applicable for
kinematically non-redundant structures. In the following, two particular
types of manipulators are considered that are of great relevance for robotics
applications.
A prominent example for such a case is a planar 3 degree of freedom (DOF)

manipulator with only revolute joints for which a closed-form IK solution
exists.
Example 3.11: SCARA3 (Closed-form Inverse Kinematics)

Consider the planar manipulator depicted in Figure 3.5 that consists
of three revolute joints with relative angles q1 , q2 , q3 and three links of
lengths l1 , l2 , l3 .
0y
(E) q3
l3 ϕE
q2
yE
l2
l1
yG up
q1
0x
down
(0) xG
xE
Figure 3.5: Inverse Kinematics of the SCARA3 Manipulator.
For a given EE configuration zE = (ϕ0E , 0 rE ), denoted by means of the

EE position 0 rE and the EE orientation

given by
the angle ϕ0E , the
corresponding joint configurations q = q1 q2 q3 are found by
q1 = α + β
q2 = arctan(yG − l1 sin q1 , xG − l1 cos q1 ) − q1
q3 = ϕ0E − q1 − q2
⎛ ⎞ ⎛ ⎞
xG cos ϕ0E ⎠
with the auxiliary quantities ⎝ ⎠ = 0 rE − l3 ⎝ ,α =
yG
sin ϕ0E
l12 +l2 −l22
arctan(yG , xG ), l2 = x2G + yG
2
, β = ± arccos 2l1 l .
Note, that there are in general up to two joint configurations that solve
the IK problem. In the above case, exactly two solutions exist, i.e. the
depicted up/down configuration of the first two links. They collapse

into a single one for EE configurations when the first two links assume
their maximum stretch. Trivially, no solution exists for desired points
outside the manipulator’s work space in which case the IK problem is
ill-posed.
For a spatial manipulator with 6 DOF sufficient conditions for the existence
of a closed-form IK solution are [127]
• three consecutive axes of revolute joints intersect in the same point,

or
• three consecutive axes of revolute joints are parallel.
A notable structure that fulfills the first of the above criteria is a 6 DOF
standard industrial robot with a spherical wrist, cf. the robot on the
linear axis from Figure 1.3. Not only because of the EE positioning and
orientation capabilities, but also due to the simplicity of the corresponding
IK resolution problem, manipulators with this structure are widely used in
many applications. For a thorough investigation of the IK of such structures
the reader is referred to [62].
Inverse kinematics problems for kinematically redundant structures can

also be solved on position-level wherein some restrictions apply.
Joint Space Decomposition

The concept of joint space decomposition (JSD) was originally introduced
in [134]. Since its introduction, JSD has been applied in the field of optimal
path tracking for industrial manipulators, e.g. in [135, 50, 100, 115] as well
as inverse kinematics of mobile platforms [105].
The key idea is a separation of the joint coordinates q into two disjoint
coordinate sets: the non-redundant joint coordinates qnr and the redundant
coordinates qr . The relationship can be expressed by means of a selector
matrix S, i.e.
⎛ ⎞ ⎡ ⎤
q
⎝ nr ⎠
S
⎣ nr ⎦q.
=
qr Sr
Of course, this decomposition alone does not enable solving the IK problem.
However, a solution for the non-redundant coordinates parametrized in
terms of the redundant coordinates can be found, i.e. qnr = qnr (R0E , rE , qr).
For a non-redundant coordinate subset with certain geometric properties,

even a closed-form solution may be available as the following introductory
example will demonstrate.
Example 3.12: SCARA4 (JSD on position-level)

Consider the kinematically redundant planar manipulator depicted in
Figure 3.6.
0y
(E) q4
ϕ0E
l4 q3
yE q2
l3
l2
l1 q1
0x
(0) xE
Figure 3.6: Inverse Kinematics of the SCARA4 Manipulator.
The EE configuration zE = (ϕ0E , 0 rE ) is given in terms of the EE

position 0 rE and the EE orientationgiven by the angle ϕ0E . The vector
of joint angles is given by q = q1 q2 q3 q4 . In order to apply
JSD to this manipulator, the last joint angle is chosen to act as the
redundant coordinate qr , thus the first three joints are the non-redundant
coordinates qnr which is formalized by
⎛ ⎞ ⎛ ⎞
⎛ ⎞ ⎡ ⎤ q1 q
q 1 0 0 0⎥⎜ ⎟ ⎜ 1⎟
⎜ 1⎟ ⎢ ⎜ ⎟ ' (⎜ ⎟
⎜q2 ⎟ ⎜q 2 ⎟
qnr = ⎜ ⎟ ⎢
⎜q2 ⎟ = ⎢0 1 0
⎝ ⎠ ⎣
0⎥⎥⎦⎜
⎜ ⎟
⎟ and qr = q4 = 0 0 0 1 ⎜
⎜ ⎟.
⎟
⎜q3 ⎟ ⎜ ⎟
q3 0 0 1 0 ⎝ ⎠ ⎝q 3 ⎠
Sr
q4 q4
Snr
With qr treated as a (known) parameter, the problem is reduced to the

IK problem of a planar 3R manipulator as discussed in⎛Example ⎞
3.11.
cos qr⎠
Therein the substitutions ϕ0E − qr → ϕ0E and 0 rE − l4 ⎝ → 0 rE
sin qr
are used to accommodate the changes of the present example w.r.t. the
previous one.
A common application of JSD is the case in which a structure with known

closed-form IK solution is extended by additional joints, e.g. the above
Example 3.12, or a 6 DOF standard industrial robot with a spherical wrist
on top of a linear axis, cf. Figure 1.3.
While the above methods yield a solution to the IK problem in a single

step (which requries a closed-form solution of any kind), the next approach
is an iterative one.
Iterative Method
The definition of the velocity Jacobian (3.4.4) gives rise to an iterative IK
scheme that operates on position-level. It can be used for point-wise com-
putation of a joint configuration q that corresponds to an EE configuration
zE (q). Starting at a joint configuration that not necessarily already agrees
with the desired EE configuration, the EE configuration error
⎛ ⎞
er (q) ⎠
e(q) = ⎝ (3.5.1)
eR (q)
occurs. It consists of the EE position error er (q) = rE,d − rE (q) and its
orientation error eR (q). While the former is simply obtained from the EE
position vector difference, the latter is more involved. With the desired and
actual EE orientation in rotation matrix representation, i.e.
' (
R0E,d = 0 (E e1,d ) 0 (E e2,d ) 0 (E e3,d ) (3.5.2)
' (
R0E (q) = 0 (E e1 (q)) 0 (E e2 (q)) 0 (E e3 (q)) (3.5.3)
the rotation matrix error yields
Re (q) = R0E,d R 0E (q) (3.5.4)

= exp 0 ẽR (q) (3.5.5)
with the spin tensor of the orientation error2 0 ẽR = log Re and its explicit
[80] angle-axis representation, cf. Section 3.1.2,
1
0 eR (q) = (E e1 (q)) × 0 (E e1,d ) + 0 (E e2 (q)) × 0 (E e2,d ) + . . .
2 0
+ 0 (E e3 (q)) × 0 (E e3,d ) (3.5.6a)
= 0 uR (q) sin ϑ(q) (3.5.6b)
with the orientation error axis uR and the orientation error angle ϑ. Note
that this representation is w.r.t. the inertially-fixed frame of reference and
only unique for |ϑ| < π2 .
With the EE position error e(q) established, it can be related to the

approximate discrete change of the joint configuration Δq by
e(q) ≈ J(q)Δq. (3.5.7)
By inverting this relationship, the joint configuration q + Δq is calculated

that approximates the desired EE configuration zE (q).
From the forward kinematics relationship (3.5.7) the iterative scheme Algo-
rithm 3 is obtained. Therein, the joint angles q are updated until the EE
error is sufficiently reduced.
2 Note, that the time derivative of e(q) does in general not correspond to the EE velocity
difference 0 VE,d − 0 VE (q,q̇) as the orientation part is different [127].

Algorithm 3 Iterative Position-Level IK Scheme

1: initialize q ← qinit
2: compute error e(q)
3: while |e| > ethresh do desired accuracy
4: if redundant then
5: compute update: Δq = J+ (q)e(q) cf. (3.5.15)
6: else
7: compute update: Δq = J−1 (q)e(q)
8: end if
9: update q ← q + Δq
10: end while
11: return q
In the above Algorithm 3 a full-(row)rank Jacobian was assumed. However,

in general, in both, the non-redundant and the redundant case, the manip-
ulator may assume joint configurations that yield a rank-deficient Jacobian.
In such a case, the respective system of equations has no unique solution.
This means that, no Δq can be found (in the current configuration) that
corresponds to the given e(q) in the sense of (3.5.7). This does not mean,
that zE (q) cannot be reached at all from the current joint configuration, it
only means that it cannot be reached directly with the update q + Δq.
Note, that (3.5.7) is only a linearized form of the actual forward kinematics
relationship (3.4.1). Therefore even an updated joint configuration q + Δq
with a Δq that solves (3.5.7) only approximates, but not necessarily agrees
with the desired EE configuration zE (q).
A possible way to mitigate this issue is to obtain a solution to (3.5.7) by

means of singular value decomposition of J. Futhermore, as discussed after
the required velocity kinematics foundation has been established, it can be
avoided by application of damped least-squares solutions, cf. (3.5.15) for
the non-redundant and (3.5.10) for the redundant case. See also [87] for
further analysis.
3.5.3 Velocity-Level and Higher-Order Inverse

Kinematics
While IK schemes on position-level are complicated as they comprise highly
nonlinear functions due to the manipulator’s kinematic structure, on differ-
ential level a linear relationship between EE and joint quantities exists.
Not only the resolution of the differential IK problem is of interest, but also
joint positions that correspond to the desired EE pose are relevant. However,
the methods considered in this section only provide such information via
numerical time integration of the resulting joint velocities q̇. By nature
of computer implementations of numerical integration schemes, errors are
introduced that accumulate over (simulation) time yielding a non-negligible
deviation from the desired EE path. Therefore, a stabilizing feedback loop
is introduced that reduces these undesired effects by means of closed-loop
inverse kinematics (CLIK), originally introduced in [137].
It is worth to emphasize that the errors occuring in this stabilization

schemes do not originate from control errors of a real system but are of
numerical nature as a result of integration schemes used in the presented
IK algorithms, cf. [124].
In certain joint configurations, so-called kinematic singularities, of the

considered manipulator, the Jacobian J from (3.4.5) becomes rank-deficient.
Singularities occur if the EE mobility is reduced, i.e. the EE cannot assume
translational or angular velocities in certain directions. Not only the work
space bounds are prone to singular configurations, but also poses well
within the work space may result in such effects. In singular configurations,
infinitely many solutions to the IK problem exist.
Differential Inverse Kinematics

Differential IK (DIK) represents the computation of the instantaneous
joint motion in terms of joint velocities q̇ or higher derivatives due to
prescribed EE motion, i.e. of EE velocities VE or higher derivatives such
as accelerations V̇E . The basic concept of IK resolution on velocity level
was introduced by [104] and [136].
For kinematically non-redundant manipulators, the IK problem has as many

equations as variables. It is found by considering the forward kinematics
scheme (3.4.5) and solving for the joint velocities q̇, i.e.
q̇ = J(q)−1 VE,d . (3.5.8)
Note, that only the solution of a system of linear equation is required rather
than an explicit representation of the Jacobian inverse J(q)−1 .
Around singular configurations, the numerical condition of the above system

of linear equation degrades, resulting in high joint velocities. In singular
configurations, the EE mobility is degraded, J is rank-deficient and thus the
above system of linear equations cannot be uniquely solved. An approximate
solution can be obtained by relaxing the path tracking requirement in favor
of moderate joint velocities by solving the unconstrained QP problem
1 1
minimize J = ė ė + αq̇ q̇ (3.5.9)
q̇ 2 2
wherein a weighted combination of the squared EE velocity error ė and
the squared joint velocities q̇ is minimized. From the first-order necessary
condition

∂J 1 ∂
= (VE,d − J(q)q̇) (VE,d − J(q)q̇) + αq̇ q̇
∂ q̇ 2 ∂ q̇
= −J VE,d + J Jq̇ + αq̇ = 0
the solution
−1
q̇ = J J + αI J VE,d (3.5.10)
is obtained.

It is a minimum

as the second-order sufficient condition
2
H = ∂∂ q̇J2 = J J + αI > 0 is satisfied. The result (3.5.10) is also called
the damped least squares solution of the IK problem.
While the above holds for kinematically non-redundant robots, in the redun-
dant case, the IK problem is underdetermined as there are fewer equations
(for EE quantities) than variables (joint velocities). It is formulated by

means of the linear equality-constrained QP
1
minimize J(q̇) = q̇ Wq̇ (3.5.11a)
q̇ 2
s.t. g(q̇) = VE,d − J(q)q̇ = 0 (3.5.11b)
wherein the squared joint velocities, weighted by the diagonal matrix W > 0,
are minimized in (3.5.11a) with the path tracking constraint (3.5.11b). The
Lagrange function corresponding to this QP yields
L(q̇, λ) = J(q̇) + λ g(q̇) (3.5.12)
with the Lagrange multipliers λ. From the first-order necessary conditions

follows
⎛ ⎞
∂L(q̇, λ) ⎠
⎝ = Wq̇ + J λ = 0 (3.5.13)
∂ q̇
and thus
(3.5.13) → q̇ = −W−1 J λ (3.5.14a)

−1
(3.5.14a) → Jq̇ = −JW J λ (3.5.14b)

−1 −1
(3.5.14b), (3.5.11b) → λ = − JW J VE,d (3.5.14c)
the solution
−1
(3.5.13), (3.5.14c) → q̇ = W−1 J (q) J(q)W−1 J (q) VE,d (3.5.15)

J+
W (q)

2
is found. It is a minimum of (3.5.11a) as H = ∂∂ q̇L2 = W > 0. Therein,
J+W a generalized inverse that is weighted by W. With the substitution
W = I it is also called the right Moore-Penrose inverse that is denoted
J+ (q). A detailed overview of other generalized inverses is found in [10].
It should be noted that a closed, i.e. circular, work space path does not
necessarily yield a closed joint space path [68]. Thus, such IK schemes are
not suitable for the unsupervised execution of repetitive tasks [5]. This
issue will be addressed in the augmented IK schemes below where such
additional criteria can be incorporated.
Furthermore, as in the non-redundant case, this methodology fails in when

the manipulator is in a singular configuration. Moreover, it is shown in
[6] that such an IK scheme does not avoid singularities even if started
in non-singular configurations. However, for this IK solution, the path
tracking constraint is again relaxed by exploiting the EE velocity error ė as
a slack variable that is treated as an additional optimization variable, i.e.
1 1
minimize
w
J(w) = ė ė + αq̇ q̇ (3.5.16a)
2 2
s.t. g(w) = VE,d − J(q)q̇ − ė = 0 (3.5.16b)

with the vector of optimization variables w = q̇ ė . The corresponding
Lagrange function yields
L(w, λ) = J(w) + λ g(w) (3.5.17)

1 1
= ė ė + αq̇ q̇ + λ (VE,d − Jq̇ − ė).
2 2

∂ L(w,λ)
from which the first-order necessary conditions ∂w =0
αq̇ − J λ = 0 (3.5.18a)
ė − λ = 0 (3.5.18b)
are derived. From (3.5.18a) follows
(3.5.18a) → αq̇ − J λ = 0 (3.5.19a)

(3.5.19a) → αJq̇ − JJ λ = 0 (3.5.19b)

(3.5.19b), (3.5.16b) → α(VE,d − ė) − JJ λ = 0 (3.5.19c)

(3.5.19c), (3.5.18b) → α(VE,d − λ) − JJ λ = 0 (3.5.19d)
−1

(3.5.19d) → λ = α JJ + αI VE,d
(3.5.19e)
and thus the IK solution

−1
(3.5.19a), (3.5.19e) → q̇ = J (q) J(q)J (q) + αI VE,d (3.5.20)
is obtained which is called the damped least squares solution of the IK

problem for kinematically redundant manipulators. It is indeed a minimum
of (3.5.16a) as theHessian
of the Lagrange function (3.5.17) is positive
∂2L
definite, i.e. H = ∂w 2 = diag(αI, I) > 0.
It should be noted that both damped least squares IK schemes (3.5.10)

and (3.5.20) do not strictly solve the original problem of finding suitable
joint velocities such that Jq̇ = VE,d holds as it is not included as a strict
constraint. This issue can be mitigated by setting the regularization variable
α = 0 only in the vicinity of singularities.
Augmented/Extended Jacobian Method

In the augmented Jacobian method [42, 121], the issue of the in general
non-square Jacobian J of rank m < n is addressed. Therein, r functional
constraints gi (q) = 0, i = 1 . . . r, collected in the r-vector g(q) = 0, are
sought to be satisfied in addition to the path tracking requirement. This is
achieved by concatenating the forward kinematics scheme (3.4.3) with the
time derivative of above constraints, i.e.
⎛ ⎞
⎛ ⎞ ω (q, q̇)⎟
⎜ E
V
⎝ E⎠ = ⎜ ⎟
⎜ vE (q, q̇) ⎟
0 ⎝ ⎠
d
dt g(q)
⎡ ⎤
⎣
J(q) ⎦
= ∂ q̇, (3.5.21)
∂q g(q)

Jaug
such that the augmented Jacobian Jaug is square. As a result, the corre-
sponding IK problem yields a single solution
⎛ ⎞
V
q̇ = J−1
aug
⎝ E,d ⎠ (3.5.22)
0
instead of infinite solutions outside of singularities. These arise not only

where the EE mobility is limited, i.e. rank J < m, but also artificially.
Furthermore, note that (3.5.21) only holds if the trajectory is started in a
point that satisfies the additional constraints g(q) = 0 such as singularity
or obstacle avoidance [4], or including a manipulator’s admissible joint
range.
A special case is the extended Jacobian technique [5] wherein the condition

∂
∈
/ ker J holds. The extended Jacobian technique is not only used
∂q g
in industrial robotics as considered in this thesis, but also for mobile,
non-holonomic manipulators, e.g. in [131, 129].
In the application [74] of the characteristic length method [3], the Jacobian
is augmented such that its condition number is minimized. Therein, an
SQP method is employed to find point-wise optimal poses for a 6 DOF
robot performing a 5 DOF task.
While in the above derivation the result is obtained by solving a system

of linear equations of dimension n, in the version proposed in [24], JSD is
applied in order to reduce the dimension to r. As discussed in Section 3.5.3,
JSD provides an additional source of algorithmic singularities.
Transposed Jacobian Method

This control-oriented DIK scheme was firstly presented in [123] and later
applied to kinematically redundant manipulators in [122]. The joint veloci-
ties q̇ are found as the position error weighted with the velocity Jacobian
of the current joint configuration, i.e.
q̇ = J (q)We (3.5.23)
with the position error e = e(zE,d , q) and the diagonal matrix W =

diag(w1 , . . . , wm ) with weights wi > 0, i = 1, . . . , m.
In the special case of a resting EE, i.e. żE,d = 0 and a manipulator in

non-singular configuration, stability of this method can be shown by means
of the direct Lyapunov method with the function
1
V (e) = e Ke (3.5.24)
2
with K = K > 0 and thus V > 0 ∀ e = 0 and V (e = 0) = 0 and its time
derivative
V̇ (e, ė) = e Kė (3.5.25a)

= e K(VE,d − VE (q, q̇))
= e K(VE,d − J(q)q̇)

= e K VE,d − J(q)J (q)We
= −e KJ(q)J (q)We ≤ 0. (3.5.25b)
For kinematically non-redundant manipulators J(q)J (q) > 0 and thus

V̇ (e, ė) < 0 holds for e = 0, i.e. the DIK scheme (3.5.23) is asymptotically
stable. In the redundant case, it is only stable as for We ∈ ker J (q) pure
internal motion occurs and the EE position error e is not further reduced
until the prescribed EE velocity is changed, the EE gets stuck. For the
general case of a moving EE the selected Lyapunov candidate function
(3.5.24) is not suitable as e KVE,d is indefinite.
Jacobian Nullspace Augmentation by Projected Gradient

A simple means to exploiting the self motion capabilities of kinematically
redundant manipulators is the projected gradient method. This IK solution
is obtained by solving the linear equality constrained QP
1
minimize J(q̇) = (q̇0 − q̇) (q̇0 − q̇) (3.5.26a)
q̇ 2
s.t. g(q̇) = VE,d − J(q)q̇ = 0 (3.5.26b)
wherein the error w.r.t. the desired joint velocities q̇0 is minimized as a sec-
ondary goal to tracking the prescribed EE velocities VE,d . The Lagrange
function corresponding to the QP (3.5.26) yields
L(q̇, λ) = J(q̇) + λ g(q̇)

1
= (q̇0 − q̇) (q̇0 − q̇) + λ (VE,d − Jq̇)
2
1
= q̇ 0 q̇0 − 2q̇ 0 q̇ + q̇ q̇ + λ (VE,d − Jq̇) (3.5.27)
2
From the first-order necessary conditions to multipliers λ can be calculated:
⎛ ⎞
∂L(q̇, λ) ⎠
⎝ = −q̇0 + q̇ − Jλ = 0 (3.5.28a)
∂ q̇
(3.5.28a) → JJ λ = J(q̇ − q̇0 ) (3.5.28b)

−1
(3.5.28b), (3.5.26b) → λ = JJ (VE,d − Jq̇0 ). (3.5.28c)
With the velocities isolated from (3.5.28a) and the result (3.5.28c) for the
Lagrange multipliers λ, the IK scheme yields
(3.5.28a) → q̇ = J λ + q̇0
−1
(3.5.28c) → = J JJ (VE,d − Jq̇0 ) + q̇0
⎛ ⎞
⎜ ⎟
−1 ⎜ −1
= J JJ
VE,d + ⎜I
⎝
− J JJ
J⎟
⎟q̇0
⎠

J+ J+

q̇ = J+ (q)VE,d + I − J+ (q)J(q) q̇0 . (3.5.29)

N(q)
with the nullspace projector N : Rn → ker J. This can be straightforwardly

shown by pre-multiplication of N with J:
−1
JN = J I − J JJ J (3.5.30)

−1
= J − JJ JJ J = O. (3.5.31)

I
The resulting IK scheme (3.5.29) is a minimum of (3.5.26) as the Hessian

of
2
the Lagrange function (3.5.27) is positive definite, i.e. H = ∂∂ q̇L2 = I > 0.
With the relationship (3.5.29) established now regard Figure 3.7. It shows
an illustration of the domains occuring for transformations using the velocity
Jacobian J. It transforms joint velocities q̇ into its image im J resulting
in corresponding EE velocities. Non-zero joint velocities that lie in the
Jacobian nullspace ker J do not result in EE motion. This inherent property
of kinematical redundacy can now be exploited by purposefully assigning
nullspace joint velocities Nq̇0 that do not change the EE behavior but
improve the joint configuration.
Again, the question comes up, what improve means in this context. If
not only an instantaneous velocity relationship is considered but a whole
path, i.e. a sequence of points, is treated with IK methods, an improved
configuration allows the manipulator to track that path so that additional
goals are fulfilled. These typically range from a minimum traversal time
over motion reduction of the individual joints to energy-efficient operation
if additionally considering the system dynamics.
q̇
VE
ker J
0
im J
Figure 3.7: Domains of the Jacobian.
The scheme (3.5.29) is understood as an IK method that tracks the desired

EE velocities VE,d while locally approximating the joint velocities q̇0 using
its internal motion capabilities. As for the choice of a suitable target q̇0 ,
often the gradient of a pose-dependent, scalar quantity H(q), a so-called

performance index, w.r.t. the joint configuration q is used, i.e.
⎛ ⎞
∂H(q) ⎠
⎝
(3.5.29) with q̇0 = :
∂q
⎛ ⎞
∂H(q) ⎠
q̇ = J (q)VE,d + I − J (q)J(q)
+ + ⎝ . (3.5.32)
∂q
N(q)
Therefore this particular family of nullspace augmentation is called projected

gradient (PG) method. This methodology was originally introduced in [78].
Popular performance indices H are the kinematic [139] and dynamic [138,
27] manipulability3 . In [24], it is shown that the PG approach is identical
to that of the extended/augmented Jacobian method in certain cases.
A large number of survey of performance measures was conducted in the

past, a comprehensive study that was published recently is [99].An attempt
to construct such performance measures in a unified manner was made in
[40]. A further survey and a guideline to synthesizing resolution criteria
are found in [77].
Regarding the use of performance indices in a PG IK scheme, only quantities

with dependencies of the joint configuration q are relevant. Many indices
are isotropic, i.e. they represent quantities that do not distinguish between
spatial directions. The kinematic manipulability, for example, describes
the EE mobility without considering the direction of the pursued path.
However, for path tracking only the prescribed path is relevant, thus it is
questionable whether mobility in directions lateral to the path need to be
considered. Task-dependent indices such as directional manipulability [30,
28, 142], i.e. the projection of the manipulability in the path direction, can
be appropriately used in such applications.
3 Note on kinematical redundancy: It is theoretically sound to include the projected gradient
of kinematic manipulability in an DIK resolution scheme as a local optimization criteria, i.e. the
configuration is drawn to a local maximum of manipulability. It is a common statement, e.g. in
[126], that it represents a distance from singular configurations. However, in general it does not
provide a metric to do so, i.e. it cannot be used to quantify how singular a configuration is.
The idea of the result (3.5.32) that tracks a prescribed EE trajectory as its
primary goal while (locally) pursuing an additional, secondary objective
H(q) gives rise to IK schemes that incorporate priorities of different tasks,
e.g. [59, 81, 89]. Therein, tasks of lower priority are projected onto the
Jacobian nullspace of higher prioritized tasks. Therein, methods exist that
make a trade-off between accuracy and strict task prioritization, e.g. [29].
Joint Space Decomposition

Similar to the position-level approach discussed in Section 3.5.2, the dif-
ferential version of JSD is based on explicit separation of the joint space
C of a manipulator [134]. Such a decomposition is mostly performed by
dividing the set of joint coordinates into two groups, the non-redundant
and the redundant coordinates. The same decomposition of course holds for
the joint velocities and higher derivatives which yields the non-redundant
and the redundant joint velocities, i.e. q̇nr and q̇r , respectively. Thus the
velocity forward kinematics (3.4.5) is rewritten as
VE = J(q)q̇
= Jnr (q)q̇nr + Jr (q)q̇r (3.5.33)
with the corresponding columns of the full geometric Jacobian J arranged

in Jnr which is square and Jr .
Assuming the redundant joint velocities q̇r as another input next to the
desired EE twist VE,d , the DIK scheme (3.5.8) is rearranged as
q̇nr = Jnr (q)−1 (VE,d − Jr (q)q̇r ). (3.5.34)
As for the non-redundant case4 , the above formulation requires Jnr to be

full rank, i.e. the manipulator is outside of singularities. In addition to
the conventional types of singularities typically arising due to reduced
EE mobility, the JSD formulation also exhibits algorithmic singularities.
These result from the particular choice of redundant coordinates and do
4 Note that for a non-redundant manipulator the redundant joint set is empty, therefore
(3.5.34) becomes (3.5.8).

not limit the EE motion. Such singularities are artificial and can therefore
be avoided by selecting a decomposition that is suitable for the current
joint configuration of the considered manipulator, i.e. with a full rank Jnr .
The example below will demonstrate that algorithmic singularities are an

issue that can occur at any point along a trajectory. Moreover, choosing
a decomposition that is feasible in a particular point does not imply that
it is feasible in all points of a trajectory. As motivated in [115], the need
for seamless switching between different decompositions arises in order to
avoid algorithmic singularities.
Note that the joints selected to be in the non-redundant set are required to
be capable of performing the prescribed EE velocity motion without the
redundant set.
Example 3.13: SCARA4 (JSD algorithmic singularities)
In this example, JSD is performed for the inherently kinematically
redundant SCARA4 manipulator that is in the configuration depicted
below in Figure 3.8.
For a task that not only involves the EE position but also its orientation,
i.e. the manipulator has a degree of kinematical redundancy of r = 1,
cf. Example 3.8, thus the selection of each joint yields a possible choice
regarding JSD.
For the particular configuration depicted in Figure 3.8, only two of the
choices, namely qr = q1 and qr = q4 yield a full rank non-redundant
Jacobian Jnr . The other choices will result in an algorithmic singularity
of Jnr due to q3 = 0, i.e. the columns corresponding to q̇2 and q̇3 are
linearly dependent.
If this particular configuration occurs in course of a trajectory with

joint 2 or 3 selected as redundant, changing to a decomposition with
joint 1 or 4 as redundant allows to continue resolving the IK problem
at that point.
0y
q4
l4
q2
l3
l2
l1 q1
0x
Figure 3.8: Algorithmic Singularity in JSD of the SCARA4 Manipulator.
In order to avoid such algorithmic singularities that are only introduced

by the specific selection of redundant coordinates for a particular EE task
configuration, in [45] the idea of a generalization of the above approach
is conveyed. Therein, an additional velocity task is pursued for which
additional rows are added to the original Jacobian which is then full
rank. Although this methodology seems similar to the augmented Jacobian
method, the added part does not represent a conventional task but is rather
used to create a full-rank Jacobian. One of the remaining issues is the choice
of the additional task. Furthermore, for the resolution of the first-order
augmented Jacobian IK scheme, a second-order pseudoinverse-based scheme
is required, which increases the computational burden of the methodology.
Applications of DIK JSD are not only found in robots with fixed base
[41] but also in mobile robotics [37, 83]. One particularly interesting work
therein is [37], where DIK JSD is used in conjunction with the reduced
gradient method that is discussed below.
For higher derivatives, e.g. acceleration, jerk and jounce, first time deriva-
tives of (3.5.33) are considered, i.e.
V̇E = Jnr (q)q̈nr + Jr (q)q̈r + J̇(q)q̇ (3.5.35a)

... ...
V̈E = Jnr (q)qnr + Jr (q)qr + J̈(q, q̇, q̈)q̇ + 2J̇(q, q̇)q̈ (3.5.35b)
... ... ... ... ... ...
VE = Jnr (q)qnr + Jr (q)qr + J(q, q̇, q̈, q)q̇ + 3J̈(q, q̇, q̈)q̈ + 3J̇(q, q̇)q.
(3.5.35c)
JSD schemes similar to (3.5.34) are then found from solving these equations
for the desired non-redundant quantities, i.e.

q̈nr = J−1
nr V̇E,d − Jr q̈r − J̇q̇ (3.5.36a)
... ...
qnr = J−1
nr V̈E,d − Jr qr − J̈q̇ − 2J̇q̈ (3.5.36b)
.... ... ... ... ...
qnr = J−1
nr VE,d − Jr qr − Jq̇ − 3J̈q̈ − 3J̇q . (3.5.36c)
Therein, terms were collected and simplified as far as possible. Note that
these derivatives are indeed consistent, i.e. (3.5.34) is the time derivative
of (3.5.36a) and so on which is now exemplarily shown:
d d −1
(3.5.34) → q̇nr = Jnr (VE,d − Jr q̇r )
dt dt
d −1 d
= Jnr (VE,d − Jr q̇r ) + J−1 nr (VE,d − Jr q̇r )
dt dt
d
= −J−1 nr (Jnr ) J−1 −1
nr (VE,d − Jr q̇r ) +Jnr V̇E,d − J̇r q̇r − Jr q̈r
dt
q̇nr

= J−1
nr V̇E,d − J̇nr q̇nr − J̇r q̇r − Jr q̈r

(3.5.36a) : q̈nr = J−1
nr V̇E,d − Jr q̈r − J̇q̇ .
Reduced Gradient Method

One particularly noteworthy flavor of JSD is the reduced gradient (RG)
method [36]. Therein, JSD is performed and the redundant coordinates
are determined by a PG method. In contrast to conventional PG-based
approaches, the computationally expensive generalized inverse is avoided.
By rewriting the JSD IK scheme (3.5.34) as

⎛ ⎞ ⎛ ⎞
q̇
⎝ nr ⎠
J−1 (VE,d
⎝ nr
− Jr q̇r )⎠
q̇ = =
q̇r q̇r
⎡ ⎤ ⎡ ⎤
J−1 −J−1 Jr
= ⎣ nr ⎦VE,d + ⎣ nr ⎦q̇r (3.5.37)
0 I
the redundant joint velocities q̇r are found such that the time derivative
of a joint pose-dependent performance index H(q), cf. Section 3.5.3, is
maximized, i.e.
d
maximize J(q̇r ) = H(q). (3.5.38)
q̇r dt
⎡ ⎤
−J−1
⎣ nr Jr ⎦
Note, that is an orthogonal complement to J. With
I
d ∂H(q)
H(q) = q̇
dt ∂q
∂H(q) ∂H(q)
(3.5.34) → = q̇nr + q̇r
∂qnr ∂qr
∂H(q) −1 ∂H(q)
= J (VE,d − Jr q̇r ) + q̇r
∂qnr nr ∂qr
⎡ ⎤
∂H(q) −1 ∂H(q) ⎣−J−1nr Jr ⎦
= Jnr VE,d + q̇r (3.5.39)
∂qnr ∂q I
and thus the first-order necessary condition to (3.5.38) follows from (3.5.39)
as
⎛ ⎞
∂J(q̇r )

∂H(q) ⎠
= −(J−1
nr Jr ) I
⎝ = 0. (3.5.40)
∂ q̇r ∂q
Thereby, the joints are driven towards an optimal configuration q in which

∂H(q)
∂q ∈ ker −(J−1nr Jr ) I is satisfied. As a consequence the redundant
velocities q̇r are chosen as
⎛ ⎞

∂H(q) ⎠
q̇r = −(J−1
nr Jr ) I ⎝ (3.5.41)
∂q
which satisfies the second-order sufficient condition to (3.5.38)

⎛ ⎞
⎜ ⎡ ⎤ ⎟
⎜ ⎟
∂ 2 J(q̇r ) ∂2 ⎜ ⎜ ∂H(q) −1 ∂H(q) ⎣−J−1
nr Jr ⎦
⎟
= ⎜ Jnr VE,d + q̇r ⎟
⎟ = 2I > 0
∂ q̇r2 ∂ q̇r ⎜
2
⎜ ∂qnr ∂q I ⎟
⎟
⎝ ⎠
q̇
r
(3.5.42)
in which case the EE motion is only governed by the first term in (3.5.37).
With the result (3.5.41) the RG scheme (3.5.37) is rewritten as
⎡ ⎤ ⎡ ⎤ ⎛ ⎞

J−1 −J−1
nr Jr ⎦ ∂H(q) ⎠
q̇ = ⎣ nr ⎦V
E,d + ⎣ −(J−1 J
nr r ) I ⎝
0 I ∂q
⎡ ⎤ ⎡ ⎤⎛ ⎞

J−1
⎣ nr ⎦V
−1 −1
⎢Jnr Jr (Jnr Jr ) −J−1
nr Jr ⎥⎝ ∂H(q) ⎠
= E,d + ⎣ ⎦ (3.5.43)
0 −(J−1nr Jr ) I ∂q

NRG
This method was not only applied to planar [36] but also to spatial and
even mobile non-holonomic manipulators [37].
It can be observed that NRG is an orthogonal complement of the Jacobian

which can be straightforwardly shown by multiplication, i.e.
⎡ ⎤

' (J−1 Jr (J−1 Jr ) −J−1
nr Jr ⎥
JNRG = Jnr Jr ⎢⎣ nr −1nr ⎦
−(Jnr Jr ) I

= Jnr J−1 −1 −1 −1
nr Jr (Jnr Jr ) − Jr (Jnr Jr ) −Jnr Jnr Jr + Jr = 0.
It should be noted that the RG method does not only exhibit the same
issues regarding artificial singularities as JSD, cf. Section 3.5.3, but also
requires the challenging choice of a pose-dependent performance index H(q)
as in PG schemes discussed in Section 3.5.3.
Generalized Jacobian Nullspace Augmentation

As mentioned above, one of the major shortcomings of the projected gradient
methods discussed in Section 3.5.3 is that the self-motion property of
kinematically redundant manipulators is only exploited by means of a

gradient of a performance index w.r.t. the instantaneous joint configuration
q that is projected onto the Jacobian nullspace.
Any q̇∗ that solves VE,d = Jq̇∗ can be augmented by means of a linear
combination of basis vectors of the Jacobian nullspace, i.e.
r

q̇ = q̇∗ + γi ai (q) (3.5.44)
i=1
= q̇∗ + A(q)γ (3.5.45)
with the nullspace basis vectors ai ∈ ker J(q) columnwisely collected in the
nullspace basis matrix A(q), and the weights γi , collected in the weights
vector γ.
Note that any nullspace augmentation can be rewritten in that form, e.g. a
PG as
⎛ ⎞
∂H(q) ⎠
I − J(q)J (q)
+ ⎝ = A(q)γ. (3.5.46)
∂q
A pseudoinverse-based solution for the IK problem on velocity-level thus

yields
q̇ = J+ VE,d + A(q)γ. (3.5.47)
Compared to JSD, for GNA the explicit selection of a redundant set of joint
coordinates is unnecessary and thus artificial singularities do not occur.
Furthermore, GNA does not require the selection of a (pose-dependent)
performance index as for PG and RG methods.
A similar approach has been proposed in archival literature [88, 107, 25,
98]. In the extended Jacobian approach from [96], the weights γi are used
as an additional task that is explicitly prescribed.
Similar to JSD, where the redundant coordinates qr or their time derivatives

have to be appropriately determined, for GNA an open challenge remains
the choice of the weights γ. Chapter 5 details how they can be selected in
an optimal manner w.r.t. an optimization goal in path tracking applications

of this IK approach.
Example 3.14: GNA (Minimization of Joint Velocities)

Exemplarily a very common optimization goal is pursued: minimization
of joint velocities for which the IK solution is well-known, cf. (3.5.15).
Starting from the velocity-level IK scheme (3.5.47), the weights γ are
required to be chosen such that the squares of q̇ are minimized, i.e. the
corresponding unconstrained quadratic optimization problem reads
1
minimize
γ
J(γ) = q̇ (γ)q̇(γ) = (q̇∗ + A(q)γ) (q̇∗ + A(q)γ).
2
(3.5.48)
The corresponding first-order necessary conditions are

∂
J(γ) = A q̇∗ − A Aγ = 0 (3.5.49)
∂γ
−1
→ γ = − A A A q̇∗
which gives
q̇ = J+ VE,d + Aγ
−1
= J+ VE,d − A A A A J+ VE,d
−1
= I − A A A ∗
A q̇ . (3.5.50)
Assuming an IK solution q̇∗ = J+ VE,d for which it is well-known that

it already minimizes q̇, cf. (3.5.15), by expanding J+ it can be easily
verified that the result obtained by (3.5.50) and (3.5.15) are indeed
equivalent, i.e.
−1 −1
q̇ = I − A A A A J JJ VE,d
−1 −1
−1
= J JJ VE,d − A A A A J

JJ VE,d = J+ VE,d .

(JA) =O
In this particular case, γ = 0 holds which can be seen from (3.5.49) as

A J+ = O.
By virtue of time derivation of (3.5.47), the consistent IK solutions for
acceleration, jerk, and jounce are respectively

q̈ = J+ V̇E,d − J̇q̇ + J̇Aγ + Ȧγ+
−1
+Aγ̇ + NJ̇ JJ VE,d nullspace (3.5.51a)
...
q = J V̈E,d − J̈q̇ − 2J̇q̈ + J̈Aγ + 2J̇Ȧγ + 2J̇Aγ̇ + 2Ȧγ̇ + Äγ+
+
−1 ⎫
+Aγ̈

+ 2NJ̇ JJ V̇E,d +
⎪
⎬
−1 −1 −1 nullspace
+N J̈ − 2J̇ JJ JJ̇ − 2J̇ JJ J̇J JJ VE,d , ⎪
⎭
(3.5.51b)
.... ... ... ...
q = J+ VE,d − Jq̇ − 3J̈q̈ − 3J̇q + 6J̇Ȧγ̇ + +3J̇Äγ + 3J̈Aγ̇+
... ...
+3J̈Ȧγ + JAγ + 3J̇Aγ̈ + 3Ȧγ̈ + 3Äγ̇ + Aγ+
−1 ⎫
...
+Aγ

+ 3NJ̇ JJ V̈E,d +
⎪
⎪
⎪
⎪
−1 −1 −1 ⎪
⎪
⎪
+N 3J̈ − 6J̇ JJ JJ̇ − 6J̇ JJ J̇J JJ V̇E,d + ⎪
⎪
⎪
⎪
⎪
... −1 −1 −1 ⎪
⎪
⎪
⎪
+N J − 3J̇ JJ JJ̈ − 6J̇ JJ J̇J̇ − 3J̈ JJ JJ̇ + ⎬ null
−1 −1 −1 −1 ⎪
+6J̇ JJ JJ̇ JJ J̇J + 6J̇ JJ JJ̇ JJ JJ̇ + ⎪
⎪
⎪
⎪
space
−1 −1 ⎪
⎪
⎪
+6J̇ JJ J̇J+ JJ̇ + 6J̇ JJ J̇J+ J̇J + ⎪
⎪
⎪
⎪
−1 −1 −1 ⎪
⎪
⎪
−3J̇ JJ J̇J − 3J̇ JJ J̈J JJ VE,d ⎪
⎭
(3.5.51c)
with the nullspace projector N = I − J+ J and the identity dt d +

J =
−1 −1

J̇ JJ − J dt JJ JJ
+d
for the time derivative of the pseudoinverse.
The above IK expressions are simplified as far as possible and denoted in a
representation that explicitly shows terms in the primal and dual space of
the Jacobian J. It is particularly noteworthy, that, in contrast to JSD, cf.
(3.5.35), in general the above IK expressions cannot be obtained by simply
rewriting the corresponding forward kinematics expressions.
Joint Space Decomposition with Generalized Nullspace

Augmentation
JSD, cf. Section 3.5.3, and GNA, cf. Section 3.5.3, can be combined in
order to reduce the computational burden. This is facilitated by relying
on the inverse of the non-redundant Jacobian Jnr instead of a generalized
inverse of the full Jacobian J, i.e. by rewriting (3.5.34) as
⎛ ⎞ ⎛ ⎞ ⎡ ⎤ ⎡ ⎤
q̇
⎝ nr ⎠
J−1 (VE,d
⎝ nr
− Jr q̇r )⎠ J−1
⎣ nr ⎦V
−J−1
⎣ nr Jr ⎦
q̇ = = = E,d + q̇r
q̇r q̇r 0 I

∈ ker J
⎡ ⎤
J−1
= ⎣ nr ⎦VE,d + A(q)γ. (3.5.52)
0
Note, that this formulation still requires the non-redundant part to be

able to complete the task. In addition, it also exhibits the disadvantage of
algorithmic singularities due to the particular choice of the decomposition.
Other Approaches
Artificial neural networks (ANN) and its application in robotics have been
undergoing extensive research in the last few decades which was mainly
performed by the computer science community. Although many methods in
this field only operate under closed-set conditions, i.e. the situations faced
in real-world operation are already known from the training set wherein
they are included, ANN can also act as a controller instance. Therein,
environmental perception of relevant parameters to the operation conditions,
e.g. computer vision, or fusion of heterogenous sensoric information, plays
an important role. In such open-set conditions, dictated by real-world
operation, the parameters influencing the employment of robotics devices
undergo permanent change, so the training data does not include occuring
in practise. The survey paper [128] provides a comprehensive overview of
current challenges and limitations of the application of ANN in robotics
in general. Genetic algorithms, i.e. population-based stochastic methods,
such as [54] are also used in this context.
Chapter 4
Path and Trajectory

Planning
A path represents a geometric entity, think, for example, of all points in
space a point of a rock sweeps through when thrown. In a robotic motion,
it can exist in the joint space as the sequence of joint positions, and also
in the work space as the sequence of configurations the EE assumes. The
correspondence between a joint space path and a work space path is given
by the forward (and inverse) kinematics of the considered manipulator, cf.
Section 3.4.1.
A trajectory describes the temporal evolution of an object traversing a path,

think of the motion a rock performs when thrown. That is, in contrast to a
purely geometric path, a trajectory exhibits a time dependency. Typically,
trajectories are given in terms of the time evolution of a robot’s joint angles
while moving along a given joint space path, or its EE configuration tracking
a work space path.
In this chapter, path and trajectory planning problems are considered.

Due to practical requirements in robotics, in most use cases it is not
sufficient to only satisfy boundary constraints of a path or a trajectory,
e.g. initial and terminal joint configurations. It is necessary to find an
interpolation that is also admissible in terms of the technological limitations
of the machine executing the task. This demand alone rules out simple
planning methods as shown below. Furthermore, not only efficacy, but also

https://doi.org/10.1007/978-3-658-28594-4_4
94 4 Path and Trajectory Planning
efficiency of the generated plans in terms of select quality criteria, e.g. the
shortest transportation path, a minimum-time or minimum-energy execution
of a trajectory, is increasingly relevant for robot operation. Numerical
optimization can be used for both1 , ensuring admissibility of the results,
and also optimality regarding a specified goal.
In the next Section 4.1 a very simple trajectory planning approach is

presented. Therein, a boundary value problem (BVP) is solved to find an
interpolation of desired initial and terminal states of a system.
In Section 4.2 point-to-point planning regarding geometric paths and also

trajectories is considered. Therein, approaches that perform geometric and
temporal planning in a single step (just as the BVP mentioned above), and
also multi-stage approaches with successive computation of the spatial and
temporal aspects are presented.
Furthermore, the aspect of computational efficiency is discussed in this

section. Due to the numerical complexity of the optimization problems
arising from typical planning tasks and the resulting high computation
times, they are only applicable in offline settings. For a robot that is
employed in a process in which frequent changes occur, e.g. consider a
task in which objects are picked from a conveyor belt whose positions vary
slightly, such planning approaches are not feasible. However, with the
approximation methods for optimal solutions presented in Section 2.1.3,
the computational performance can be substantially improved (at the cost
of slight suboptimality). Approximation schemes based on parametric
sensitivities have been previously applied in robotics research [69, 112, 116].
In this section, the efficacy of the proposed methods is shown using various
numerical examples. While therein mostly fully actuated industrial robots
are considered, i.e. each joint is driven by its own actuator, the introductory
example is an underactuated model.
1 Note, that such approaches can also be exploited when only admissible, but not optimal
results are required. In this case, optimization is performed w.r.t. a dummy objective.
4.1 Boundary Value Problem 95
4.1 Boundary Value Problem

Problem Statement
In order to the use of optimal, constrained trajectory planning, as discussed
above, first a simple example is considered.
Example 4.1: Swing-Up Motion of Furuta Pendulum (BVP)

Consider the Furuta pendulum depicted in Figure 4.1.
q2
q1
gearbox
motor
u
Figure 4.1: Furuta Pendulum.
The goal of this example is to compute control and joint trajectories

to successfully perform
a swing-up

motion of the pendulum. With the
two joint angles q = q1 q2 , the pendulum exhibits two DOF while it
has only one control input u, namely the motor torque, thus the system
is considered underactuated. Furthermore, the system is constrained
by the maximum torque and speed of the motor, i.e. |u| ≤ umax and
|q̇1 | ≤ q̇1,max . Moreover, the torque is required to be continuous and
zero at the bounds.
Implementation
The swing-up procedure is thus formalized as the two-point boundary
value problem
ẋ = f (x, u) (4.1.1a)

q(0) = q0 = 0 π (4.1.1b)
q̇(0) = q̇0 = 0 (4.1.1c)
q2 (T ) = q2,T = 0 (4.1.1d)
q̇(T ) = q̇T = 0 (4.1.1e)

with the state x = q q̇ and the additional boundary conditions
u(0) = u(T ) = 0 due to the torque continuity requirement.
Similar to [56], the control function can be constructed as a consine

series, i.e.
5
πt iπt
u(t) = a0 + a1 cos + pi−1 cos (4.1.2)
T i=2 T

with the free parameters collected in p = p1 p2 p3 p4 . With this
particular choice of a function for u(t) the infinite dimensional BVP
(4.1.1) is transcribed into a finite dimensional problem. The control
trajectory u(t) also largely determines the properties of the resulting
joint trajectories. From the torque boundary conditions a0 = −p1 − p3
and a1 = −p2 − p4 follow. As the time evolution of the control u and
thus the state x can be determined from p, the goal of this example
is to compute these parameters such that the boundary conditions are
satisfied. This can be facilitated with standard techniques for solving
DAEs, e.g. with the collocation-based BVP solver bvp4c [67].
Discussion
Of course the function class for u(t) is not restricted to the above
cosine series (4.1.2). Alternatively, for example, a B-spline curve, cf.
Section 2.2, with 6 control points could be used to represent u(t).
4.2 Optimal Point-to-Point Trajectory Planning 97
As the technological constraints of the systems, e.g. a limit of the motor

torque, are not considered in the above DAE formulation, it becomes
apparent that these may be violated by the result. Therefore, this
approach is very unflexible as it is not able to handle typical inequality
constraints occuring in technical systems. In [57] inequality constraints
are handled by means of saturation functions and augmentation of the
dynamic system by additional states. This procedure again yields a
DAE formulation that can be solved using standard frameworks, e.g.
with bvp4c.
Alternatively, such tasks can be handled by means of the OCP techniques

discussed in Section 2.3. This topic will later be revisited in Example 4.2
where it will be treated as an inequality-constrained OCP in the next
Section 4.2.
4.2 Optimal Point-to-Point Trajectory

Planning
Trajectory planning is the task of finding suitable time evolutions of a
robot’s motion in order to solve a particular task, such as transportation
of items or material processing. Therefore, a suitable evolution of the
manipulator’s joint angles q(t) must be found, which inherently specifies
the motion of the EE due to forward kinematics, cf. Section 3.4. Planning
tasks are constrained by the physical and technological limitations of the
robot, e.g. those of joint angles, motor turning velocities, or motor torques.
Further constraints are due to the work process the manipulator is employed
in, e.g. geometric EE path constraints, restriction of the entire robot to a
confined work space.
Summarizing, a trajectory planning problem is composed of both, geometric

and kinematic as well as dynamic aspects. Examples for point-to-point
trajectory planning tasks are not only found in typical robotic transporta-
tion, but also in the field of ball catching, e.g. [120], (airborne) object
interception, e.g. [73], missile guidance, or space maneuvers [48]. Therein,
often fast computation of the trajectories is required, which is typically
achieved by omitting or simplifying constraints, e.g. by neglecting friction
effects. Therefore, the resulting trajectories often lack admissibility and

actual optimality. Other methods for instantaneous motion planning, e.g.
[70, 66], and also model predictive control techniques suffer from similar
limitations in some cases, e.g. [55]. The paper [26] considers optimal spline-
based interpolation of initial and terminal states in joint space. In [35],
B-spline robot trajectories are computed wherein parametric sensitivities
are exploited to speed up calculations. However, instead of the control
points which are used in the presents thesis, the knot intervals are the
optimization variables.
Trajectory planning methods can be classified in single-step approaches

that solve the entire problem in a single optimization stage, and in multi-
step approaches which, for example, treat the kinematic and dynamic
subproblems sequentially. In the following, both types will be discussed.
4.2.1 Single-Step Approach

As path planning and also motion planning are performed in a single
optimization step, the OCP (2.3.1) must be reformulated in order to accom-
modate point-to-point (PTP) trajectory planning. The trajectory planning
OCP reads
&T
minimize L(q(t), u(t))dt + E(q(T )) (4.2.1a)
q(t),u(t)
0
s.t. q0 − q(0) = 0, q̇0 − q̇(0) = 0, . . . (4.2.1b)
r(q(T )) ≤ 0, r̃(q(T ), q̇(T )) ≤ 0, . . . (4.2.1c)
qmin ≤ q(t) ≤ qmax , q̇min ≤ q̇(t) ≤ q̇max , . . . (4.2.1d)
umin ≤ u(t) ≤ umax (4.2.1e)
M(q(t))q̈(t) + g(q(t), q̇(t)) = Q(q(t), u(t)) (4.2.1f)
h(q(t), u(t)) ≤ 0, ∀t ∈ [0, T ] (4.2.1g)
wherein q(t) describes the time evolution of the robot’s joint angles and
u(t) represents the input trajectory, i.e. actuator forces and torques. The
objective (4.2.1a) is kept very general in order to account for goals frequently
used in robotics such as minimum energy, minimum time, or minimum
component wear. For such a PTP problem, particular interest lies in the
specification of initial and terminal conditions (4.2.1b) and (4.2.1c), respec-
tively. The in general nonlinear function r represents the terminal conditions
on position-level, r̃ is used for formalize a velocity-level constraint. Therein,
typically not only the initial and terminal joint angles are considered, but
also their time derivatives which are here (and in the following) omitted
for brevity. The inequality constraints (4.2.1d) and (4.2.1e) represent limi-
tations imposed onto the joint angles (and their derivatives) and onto the
inputs, respectively. In the original OCP, these were included in the path
constraints (2.3.1e) but are explicitly mentioned here due to their relevance
in this application. While previously the system dynamics was considered
in (2.3.1d) in state-space representation, here it is equivalently denoted as
the equations of motion in (4.2.1f). Therein, M(q) is the generalized mass
matrix, g(q, q̇) is a vector of in general nonlinear terms including general
Coriolis, centrifugal and friction forces. The generalized input forces Q(q, u)
are in practice often given by scaled inputs u. Furthermore, the motion is
subjected to actual path constraints (4.2.1g) that impose restrictions arising
from the task to be executed, e.g. work space constraints in multi-robot
cooperation environments, maximum EE velocity in machining tasks, or
limited EE acceleration to avoid sloshing in liquid transportation.
The following examples will demonstrate possible applications of the single-

step approach.
Example 4.2: Swing-Up Motion of Furuta Pendulum (OCP)

Here, Example 4.1 is revisited in the context of OCP.
Implementation
The trajectory planning task is formalized as a direct multiple shooting
OCP, cf. (2.3.4), not only to solve the BVP considered previously,
but also to minimize the torque required to drive the pendulum and
incorporate technonogical constraints of the system. The corresponding

NLP is derived from (4.2.1) and reads
N
&tj
minimize
w ,w
u2 (t)dt (4.2.2a)
x u
j=1 tj−1
s.t. q0 − q0 = 0, q̇0 − q̇0 = 0 (4.2.2b)

q2,T − q2 (T ) = 0, q̇T − q̇(T ) = 0 (4.2.2c)
&tj
xj − f (x(t), u(t))dt = 0, j = 1, . . . , N (4.2.2d)
tj−1
− Pmax ≤ P (xk , uk ) = uk q̇1,k ≤ Pmax (4.2.2e)

uk
− UA,max ≤ UA,k = RA + q̇1,k kM ≤ UA,max , k = 1, . . . , Nch
kM
(4.2.2f)

wherein the desired initial configuration is q 0 = 0 π , see (4.2.2b). As
prescribed in (4.2.2c), the desired terminal configuration is q2 (T ) = q2,T .
The motion is required to be executed as a rest-to-rest motion, i.e. the
joint velocities at the beginning and the end of the motion are zero,
cf. (4.2.2b) and (4.2.2c). The execution time is fixed to T = 2.6 s, the
time domain is discretized in N = 261 uniform intervals. As the setup
is sufficiently stiff to be considered rigid for the swing-up operation,
it is adequate to use a piecewise linear function for the motor torque
trajectory u(t) in order to avoid excitation of unmodeled stiffness effects
of the system.
Results
The above NLP problem (4.2.2) was implemented using CasADi and
solved with Ipopt. The resulting joint trajectories, velocities and torques
are shown in Figures 4.2 to 4.4. It can be observed, that the result
accomplishes the swing-up motion, i.e. a rest-to-rest motion that drives
both joint angles to zero, and is admissible w.r.t. the technological
constraints of the system (the plots of UA and P are neglected for the
sake of brevity).
q in rad q1 q2
0
t in s
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6
Figure 4.2: Swing-Up Motion of Furuta Pendulum: Joint Angles q(t).
q̇ in rad/s q̇1 q̇2
−5
−10 t in s
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6
Figure 4.3: Swing-Up Motion of Furuta Pendulum: Joint Velocities q̇(t).
u in Nm
0.4
0.2
−0.2
t in s
−0.4
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6
Figure 4.4: Swing-Up Motion of Furuta Pendulum: Motor Torque u(t).
The trajectories computed with the above scheme were successfully

experimentally verified with the lab prototype shown in Figure 4.1.
Similar to [56], a combined feed-forward/feed-back control structure is
used. It comprises a model-based feed-forward part with the control
trajectory resulting from the NLP problem (4.2.2), and PD feed-back
control which is linear quadratic design [72]. Details about the imple-
mentation and experiments on the lab prototype are documented in
[140].
The following two examples will demonstrate trajectory planning OCP

in the case of synchronization tasks of robotic manipulators with their
environment. In the field of robotics, such synchronization or rendezvous
problems have been researched in ball catching. Similar tasks have been
previously investigated in the (non-robotic) context of space landing and
docking maneuvers and object interception.
Example 4.3: SCARA4 (minimum-time trajectory planning)
Problem Statement
Consider the synchronization task depicted in Figure 4.5. Therein, the
SCARA4 manipulator known from the previous examples is required to
pick items from a conveyor belt that is moving with constant velocity
vB . It is assumed, that the object to be picked has the same velocity, i.e.
vObj = vB . In this example, only the computation of the pick trajectory
is considered. Therein, the lateral placement of an item, denoted with
its distance Δx from the base of the robot, is uncertain and therefore
can only be determined immediately before the start of the picking
process. While the initial rest configuration at q0 is the same for all pick
processes, the terminal joint configuration q(T ) varies depending on the
position of the item on the conveyor at pick time T . The pick motion
needs to be executed in minimum time, i.e. the item needs to be picked
as soon as possible. The motion is subjected to physical constraints
regarding the joint angles q, velocities q̇ and motor torques MMot or
accelerations q̈. Moreover, the work space is constrained such that the
EE is not allowed to enter the area behind the conveyor as well as the
area right of the detection line.
x
object detection line
vB
terminal
Δx configuration q(T )
y
initial
configuration q0
Figure 4.5: SCARA4 Minimum-Time Trajectory Planning.
Implementation
The general OCP (4.2.1) is rewritten in order to fit the present minimum-
time example and reads now
&T
minimize 1dt (4.2.3a)
q(t),T
0
s.t. q0 − q(0) = 0, q̇0 − q̇(0) = 0, q̈(0) = 0 (4.2.3b)
rE (q(T )) − rObj (T ) = 0,
vE (q(T )) − vObj = 0, ωE (q(T )) = 0, q̈(T ) = 0 (4.2.3c)
qmin ≤ q(t) ≤ qmax , q̇min ≤ q̇(t) ≤ q̇max , q̈min ≤ q̈(t) ≤ q̈max
(4.2.3d)
MMot,min ≤ MMot ≤ MMot,max (4.2.3e)
⎛ ⎞ ⎛ ⎞
x
⎝ min ⎠
xmax ⎠
≤ rE (q(t)) ≤ ⎝ (4.2.3f)
ymin ymax
wherein the objective (4.2.3a) only consists of a Lagrange term

L(q(t), u(t)) = L = 1. While the initial conditions are considered
in (4.2.3b) in terms of joint space quantitites, the terminal conditions
can be found in (4.2.3c) in work space notation. Box constraints for

joint angles, velocities, accelerations and torques are found in (4.2.3d)
and (4.2.3e), respectively. The work space limits are imposed using
(4.2.3f).
With the choice for q(t) to be B-spline curves, OCP (4.2.3) is once again
rewritten in order to exploit various properties of this function class.
Therefore, each qi (t), i = 1, . . . , n = 4 is a B-spline curve of degree p = 3
and NCP = 20 control points. The control points of each B-spline curve
describing the time evolution of the i-th joint angle are collected in the
vector dqi ∈ RNCP . Furthermore, the respective j-th control points of
all curves are collected in the vector dq,j ∈ Rn . Thus, the j-th control
point of the i-th curve is denoted dqi ,j . The same notation holds for
time derviatives of the curves. All control points and also the terminal
time T are collected in the vector of optimization variables

w = dq1 ,0 · · · dqn ,0 dq1 ,1 dq2 ,1 · · · dqn ,1 · · · dqn ,0 dqn ,1 · · · dqn ,NCP −1 T .
The corresponding optimization problem reads
minimize
w
T (4.2.4a)
s.t. q0 − dq,0 (w) = 0, q̇0 − dq̇,0 (w) = 0, dq̈,0 (w) = 0
(4.2.4b)
rE (q(T ) = dq,NCP −1 (w)) − rObj (T ) = 0, (4.2.4c)
vE (q(T ), q̇(T ) = dq̇,NCP −2 (w)) − vObj = 0,
ωE (q(T ), q̇(T )) = 0, (4.2.4d)
dq̈,NCP −3 (w) = 0 (4.2.4e)
qi,min 1 ≤ dq,j (w) ≤ qi,max 1 ≤ 0, i = 1, . . . , n, j = 3, . . . , NCP − 1
(4.2.4f)
q̇i,min 1 ≤ dq̇,j (w) ≤ q̇i,max 1 ≤ 0, i = 1, . . . , n, j = 2, . . . , NCP − 2
(4.2.4g)
q̈i,min 1 ≤ dq̈,j (w) ≤ q̈i,max 1 ≤ 0, i = 1, . . . , n, j = 1, . . . , NCP − 3
(4.2.4h)
⎛ ⎞ ⎛ ⎞
x
⎝ min ⎠
x
⎝ max ⎠
≤ rE (q(t)) ≤ (4.2.4i)
ymin ymax
wherein 1 denotes a vector of ones of appropriate size. In this minimum-

time OCP, cf. (4.2.4a), the initial rest state is prescribed in (4.2.4b). The
pick point, i.e. the EE configuration at which the pick process begins,
is assumed at rE (q(T )) (gripper above object).The greatest difficulty in
this example is posed by the fact that the target item is moving, i.e. the
(longitudinal) position rObj (t) of the item on the belt varies over time.
Only at the pick time the object and EE positions conincide, and there-
from the terminal joint angles q(T ) are determined which is reflected
in the nonlinear equality constraints (4.2.4c). Note, that due to the
kinematic redundancy of the manipulator w.r.t. to this positioning task
this relationship is ambigous, i.e. multiple joint configurations satisfy
these constraints. The corresponding velocity relationship is formulated
by means of the nonlinear equality constraints with the velocity-level
forward kinematics (4.2.4d). Also the terminal joint acceleration needs
to be zero in order to enable a smooth picking process, i.e. (4.2.4e). As
for the corresponding initial condition, this constraint is implemented by
exploiting the boundary point interpolation property of B-spline curves
resulting in linear equality constraints. The OCP (4.2.4) is subjected
to box constraints limiting the joint angles in (4.2.4f), the velocities
in (4.2.4g) and accelerations in (4.2.4h). In addition to the fact, that
control points of curve derivatives are linear combinations of the control
poitns of the original curves, these constraints exploit the convex hull
property of B-spline curves, resulting in linear inequality constraints.
The work space constraints are formulated with the box constraints
(4.2.4i) which are due to the forward kinematics relationship nonlinear

inequality constraints. Note, that these constraints are unlikely to be
reached as they are far off the initial and final EE positions.
Although this approach is termed single-step, the preparation of the

solution of the above NLP problem (4.2.4) comprises multiple steps. In
order to properly initialize the NLP problem and thus enable its efficient
solution, multi-stage trajectory planning with relaxed constraints is
performed. First, in order to determine a terminal joint configuration,
the iterative IK scheme Algorithm 3 is employed, the terminal velocity is
found by application of (3.5.15). The trajectories are then planned with
a B-spline approximation scheme similar to (2.2.11), incorporating the
geometric constraints of (4.2.4). In order to speed up the computation,
the work space constraints (4.2.4i) are excluded from the initial guess
computation, which ultimately yields a convex QP.
As the torque constraints are nonlinear, the solution of the above

NLP becomes more involved and thus more time-consuming if they
are included. Alternatively, pure (linear) joint acceleration constraints
can be used as an approximation. It is difficult to provide general
acceleration limits as the torques not only depend on the q̈, but also
on the current joint configuration q as well as on the joint velocities q̇.
To this end, the actual torques must be underestimated, so the results
are more conservative. A practical method is to assume acceleration
limits, solve the above NLP problem and check the admissibility w.r.t.
the torque constraints afterwards (and adjust the acceleration limits if
necessary).
Results
In the above Figures 4.6 and 4.7 it can be seen that the velocity and
torque constraints are active during most of the trajectory except for
the initial acceleration and terminal deceleration phases in which the
prescribed boundary points are dominant.
q (normalized) q1 q2 q3 q4
1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5
q̇ (normalized) q̇1 q̇2 q̇3 q̇4

1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5
q̈ (normalized) q̈1 q̈2 q̈3 q̈4

1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5
Figure 4.6: SCARA4 Minimum-Time Motion (Constrained Accelerations q̈(t)),

Δx = 0. Limits: qi,max : 1.65 rad, q̇1,max = q̇2,max : 2.51 rad/s,
q̇3,max = q̇4,max : 5.81 rad/s, q̈1,max = q̈2,max : 12.6 rad/s2 , q̈3,max = q̈4,max : 29.1 rad/s2 .
Note that the terminal motor torques are not zero despite the terminal
accelerations are restricted to q̈(T ) = 0 due to the non-zero terminal
velocity and the viscous friction effects included in the model for the
system dynamics.
While the above friction torque is proportional to the joint speeds,

another effect result from the stiction model. Regard therefore the jitter
behavior visible in the torque curves in Figure 4.7, especially of M3 and
M4 where q̇3 and q̇4 change their respective signs.
Moreover, it is apparent that the torques violate their respective con-

straints at multiple times along the trajectory between the Nch torque
check points. This issue can be mitigated by increasing the number
of torque check points Nch , but cannot be completely avoided by this

approach. For practical applications, one needs to check whether the
peak torque constraint and the thermal constraints are violated during
these times.
Table 4.1 shows the optimal terminal times T and computation times
tCPU for all considered cases Δx. The selected acceleration bounds
appear to be too low, also investigation of the motor torques resulting
from the trajectories reveals that the torque constraints are violated
during multiple times which supports this indication. However, proper
choice of acceleration bounds as a replacement for torque bounds is
complex due to the aforementioned reasons.
Furthermore, T decreases with the distance from the robot base as

the robot is unfolded by centrifugal forces whereas the computation
time rises because the KKT system grows in size due to the increasing
amount of active constraints.
Table 4.1: SCARA4 Minimum-Time Trajctory Planning: Results.
constrained q̈ constrained MMot

Δx T tCPU T tCPU
1.00 0.6229 0.2876 0.7553 310.2455
0.95 0.5889 0.3410 0.7328 373.2159
0.90 0.5566 0.4481 0.7169 347.5644
0.85 0.5328 0.4409 0.6969 562.5836
0.80 0.5131 0.4631 0.6724 359.5886
0.75 0.4966 0.4576 0.6520 346.5954
0.70 0.4812 0.4142 0.6303 521.2011
0.65 0.4674 0.4842 0.6083 463.7495
0.60 0.4542 0.5240 0.5929 460.2238
1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65

1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65
MMot (normalized) M1 M2 M3 M4
1
0.5
0
−0.5
t in s
−1
0 5· 10−2 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65
Figure 4.7: SCARA4 Minimum-Time Motion (Constrained Motor Torques MMot (t)),
Δx = 0. Limits: qi,max : 1.65 rad, q̇1,max = q̇2,max : 2.51 rad/s,
q̇3,max = q̇4,max : 5.81 rad/s, M1,max = M2,max : 0.336 Nm,
M3,max = M4,max : 0.142 Nm.
The next example will be a variation of the previous, now minimizing the
joint velocities (and thus the energy dissipated via viscous friction).
Example 4.4: SCARA4 (minimum-energy trajectory planning)
Problem Statement
Consider the previous Example 4.3 with the cost function rewritten
such that the minimum-time trajectory planning task is converted into
a minimum-(pseudo-)energy task, i.e. (4.2.4a) becomes
4

minimize
w
JE = di d q̇i dq̇i .
i=1
Minimizing JE , i.e. the sum of squares of the control points of the

B-spline curves, representing the joint velocities q̇(t), minimizes the
joint velocities due to the convex hull property. The sum of squared joint
velocities is sometimes referred to as kinetic pseudo-energy. Furthermore,
the velocities are weighted with the respective viscous friction coefficients
di , cf. Rayleigh’s dissipation function. As in the previous example,
the terminal time T is left free and will thus be optimally adjusted.
Discussion
Figure 4.8 shows a typical scenario in which the robot’s EE is synchro-
nized with an object on the conveyor belt. Note, that in this example
the farmost path in the x direction is limited by the path constraints
(4.2.4i). Furthermore, the minimum-energy paths tend to be longer
than the minimum-time ones due to the significantly higher execution
times which can be seen in Table 4.2. Again, paths further away from
the robot base yield higher energy dissipation due to decreasing execu-
tion times. As the rest of the optimization problems is the same, the
trajectories for both kinds of constraints are the equal.
x
object detection line
vB
terminal
Δx configuration q(T )
y
initial
configuration q0
Figure 4.8: SCARA4 Minimum-Energy Trajctory Planning.

Table 4.2: SCARA4 Minimum-Energy Trajctory Planning: Results.
Δx JE T tCPU
1.00 14.56 3.307 13.83
0.95 13.05 3.431 18.29
0.90 11.94 3.526 31.98
0.85 11.10 3.600 18.30
0.80 10.46 3.656 17.95
0.75 9.96 3.697 18.02
0.70 9.60 3.726 23.93
0.65 9.33 3.746 28.68
0.60 9.16 3.758 31.72
For this example, only the torque-constrained plots are shown in Fig-
ure 4.9, as neither the (artificial) acceleration limits, nor the motor
torque limits are reached due to the low speed of the operation. Again,
note the sudden drops and increases of the motor torques in Figure 4.9.
These are a results of stiction effects included in the dynamics model of
the robot.
1
0.5
0
−0.5
t in s
−1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3
1
0.5
0
−0.5
t in s
−1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3
MMot (normalized) M1 M2 M3 M4
1
0.5
0
−0.5
t in s
−1
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 3
Figure 4.9: SCARA4 Minimum-Energy Motion (Constrained Motor Torques

MMot (t)), Δx = 0. Limits: qi,max : 1.65 rad, q̇1,max = q̇2,max : 2.51 rad/s,
q̇3,max = q̇4,max : 5.81 rad/s, M1,max = M2,max : 0.336 Nm,
M3,max = M4,max : 0.142 Nm.
Example 4.5: Comau Racer R3 (synchronization trajectory plan-

ning with parametric sensitivities)
Problem Statement
In this example that was originally published in the author’s work
[116], a rendezvous problem of two identical Comau Racer3 industrial
robots A and B is considered, cf. Figure 4.10. In contrast to robot A,
robot B is mounted on a linear axis and has therefore an increased work
space. Robot B is used to simulate the synchronization target, i.e. it
remains in the depicted pose during the operation, only the linear axis
moves with constant velocity.
Robot A’s task is to perform a minimum-energy motion to synchronize

both robots’ end-effectors at the terminal time T = 0.8 s. The motion
starts from rest and is acceleration-free at the beginning and the end,
i.e. q̇0 = q̈0 = q̈N = 0. The setup is considered to be used in an
industrial process where the exact rendezvous position and velocity in
the direction of the linear axis is only known at run time, immediately
before executing the motion. Changes of Δx and Δvy = −0.05 ms , cf.
Figure 4.10, result in a perturbation of robot A’s desired

terminal

state
(qf , q̇f ) that is represented by the parameters p = p qN p q̇N . In this
particular example, the target moves along a straight line in work space.
The terminal joint configuration qf of robot A varies nonlinearly with the
perturbation Δx due to the kinematic chain of the serial manipulator.
Another consequence is the change of robot A’s terminal joint velocity

q̇f which is influenced by both, the position perturbation Δx and the
velocity perturbation Δvy .
vy + Δ
Δx
B
A
Figure 4.10: Comau Racer R3 Trajectory Planning: Experimental Setup.
Thus, linear work space motion of the EE does not constitute a special
case.
Implementation
In the collocation scheme
N

minimize
w
J(w) = minimize
w
q̇ k q̇k + αM k Mk (4.2.5a)
k=0
s.t. 1qi,min ≤ dqi ≤ 1qi,max (4.2.5b)
1q̇i,min ≤ dq̇i ≤ 1q̇i,max (4.2.5c)
... ...
1q i,min ≤ d...qi ≤ 1q i,max ∀i = 1 . . . n (4.2.5d)
Mk,min ≤ Mk ≤ Mk,max ∀k = 0 . . . N (4.2.5e)
q0 = q0 , q̇0 = q̇0 , q̈0 = q̈0 (4.2.5f)
qN = pqN , q̇N = pq̇N , q̈N = q̈N (4.2.5g)
the joint trajectories qi (t) = n (t)dqi are represented by B-spline curves

with a degree of p = 3 and 50 control points and solved for a nominal set
of parameters pnom . The cost function as well as the inverse dynamics for
the inequality constraints regarding the motor torques M are evaluated
for a time grid of tk = kΔt, k = 0 . . . N with Δt = tf −t0
N . The trajectory
planning problem was implemented using CasADi [2] and solved with
Ipopt [133]. The cost function J(w) (4.2.5a) penalizes the joint velocities
and motor torques with a weight α and is minimized w.r.t. w. Further
on, the NLP is subjected to (in)equality constraints (4.2.5b) to (4.2.5g)
and depends on the parameters p. The initial and terminal conditions
(4.2.5f) and (4.2.5g) can be conveniently expressed in terms of the
boundary control points which are interpolated by the B-spline curve.
In the above formulation the column vector 1 of appropriate size consists
of 1 entries.
Due to computation time constraints, an exact solution of (4.2.5) is not

feasible, but a nearly-optimal solution can be obtained by means of the
improved iterative feedback method from Section 2.1.3. Therefore, an
offline optimal solution for nominal parameters pqN = qN and pq̇N = q̇N
with the corresponding parameter sensitivities in the optimal point is
obtained beforehand.
While small perturbations have no effect on the active set, for larger
perturbations the active set of the optimal solution to the perturbed
problem may be different from that of the nominal problem.
Results
Table 4.3: Comau Racer R3 Trajectory Planning: Results (perturbations Δx, relative
cost increase JΔJ
pert
, number of iterations).
Collocation
Δx in m ΔJ/Jpert # iter
−0.04 +5.65% 4
−0.02 +2.6% 3
−0.01 +1.48% 2
+0.01 +0.39% 2
+0.02 +5.13% 3
+0.04 +8.35% 4
Due to the particular choice of the cost function and the terminal time,
the joint velocities q̇ as well as the motor torques QM rarely reach their
respective limits in this example. The limiting factor is found to be the
...
joint jerks q. Table 4.3 shows an overview of the results wherein the
relative cost increase of J w.r.t. the exact cost function value in the
perturbed case Jpert as well as the number of iterations of Algorithm 2
are reported. The method was found to converge for perturbations
up to Δx = ±0.04 m in a low number of iterations. The results were
successfully validated in experiments featuring the described setup, i.e.
the planned trajectories were executed with negligible joint tracking
error.
Evaluation of the computation time required to solve the above NLP
problems reveals that such formulations can hardly be used in practical
applications. However, by revisiting the methods to approximate solutions
of parametric NLPs discussed in Section 2.1.3 and especially the academic
examples considered in Section 2.1.3.4, these trajectory planning examples
seem to form a suitable basis for the optimal solution approximations as
demonstrated in the above examples.
4.2.2 Multi-Step Approach

The staggered methodology consists of the following main stages that are
executed in an iterative manner:
• geometric planning, i.e. determining the geometric path that the EE

(and the robot’s joints) will pursue during execution, and
• dynamic planning, i.e. finding the time evolution of the joint angles
such that the dynamics of the robotic system are considered.
Both of the above stages can be further decomposed in substages, e.g. the
geometric planning stage may include determining the initial and terminal
joint configuration for a given EE pose. That way, the problem complexity
of path planning is lowered, allowing efficient joint space formulations of
path planning strategies to be applied.
Pose Planning Subproblem (Optimal Inverse Kinematics)

Before the kinematic aspect of trajectory planning is treated, first the
geometric joint path needs to be determined. While in many cases the
boundary conditions regarding the joint path, i.e. the initial and terminal
joint configurations, are specified, some path planning tasks require com-
puting such a suitable joint configuration for a prescribed EE configuration.
Of course, a corresponding joint configuration needs to be admissible w.r.t.
the joint limits as well as other geometric constraints such as collisions with
the robot’s environment.
For kinematically non-redundant robots, the number of possible configu-

rations that meet the EE pose constraints is finite. Furthermore, some
of the solutions of a corresponding IK problem violate joint limits and
must therefore be excluded from the search space. Other configurations
may interfere with the process the manipulator is used in. Due to the low
number of admissible solutions, an exhaustive determination of the best, i.e.
in some sense optimal, configuration is often indeed viable.
However, for manipulators that are kinematically redundant, infinitely

many joint configurations correspond to the prescribed EE configuration.
In order to select the best, numerical optimization is employed wherein

the optimal joint configuration can be selected in order to pursue goals
such as maximizing dexterity, centering the joint angles, or maximizing
force transfer capabilities, possibly in specified directions. A corresponding
optimization problem can be formulated as
minimize
q
J(q) (4.2.6a)
s.t. zE,d − zE (q) = 0 (4.2.6b)
qmin ≤ q ≤ qmax (4.2.6c)
h(q) ≤ 0 (4.2.6d)
wherein a pose-dependent quantity captured in J(q) is minimized. The

problem is subjected to the forward kinematics constraint regarding the
EE pose (4.2.6b), i.e. in general EE position and orientation, as well as box
constraints of the joint limits, cf. (4.2.6c). Additionally, further constraints
(4.2.6d) may be incorporated, e.g. in order to avoid work space violations
of other parts of the robot than the EE.
Example 4.6: SCARA4 (terminal configuration)
Problem Statement (Part 1)

In this example, the EE position is prescribed for which a correspond-
ing joint configuration is sought. While the kinematic redundancy of
the considered manipulator increases the difficulty of this positioning
problem, it also enables pose optimization regarding specific criteria. In
this case it is chosen to maximize the dexterity
/
of the EE in the form
of the kinematic manipulability [139] H(q) = abs det(J(q)J (q)).
Implementation (Part 1)
The NLP problem (4.2.6) thus changes into

maximize
q
H 2 (q) = abs det J(q)J (q) (4.2.7a)
s.t. rE,d − rE (q) = 0 (4.2.7b)
qmin ≤ q ≤ qmax (4.2.7c)
wherein a joint configuration q is sought that maximizes the squared

kinematic manipulability in the objective (4.2.7a). The equality con-
straint (4.2.7b) enforces the prescribed EE position rE,d and (4.2.7c)
are the box constraints of the joint angles. It is assumed that the EE
orientation is irrelevant in this example. The situation is depicted in
Figure 4.11 wherein one particular optimal pick configuration.
0x
conveyor belt discretization point
vB
WR
0y
Figure 4.11: SCARA4 Path Planning: Optimal Picking Poses for the SCARA4
Manipulator. Red: Nominal Solutions. Green: Admissible Approximate Solutions.
Results (Part 1)
The above NLP problem (4.2.7) is solved for all points on the uniform
grid shown in Figure 4.11 to determine the respective optimal joint
configuration. As the initial guess, all angles were set to zero (joints
centered). Of course, only points within the manipulator’s reachable
work space WR , cf. Section 3.3.2, will yield a corresponding joint con-
figuration. Due to the manipulator’s dexterity owed to its kinematic

redundancy, indeed all discretization points on the conveyor are reach-
able. However, it is assumed that desired points closer to the origin,
will not yield a solution due to the joint angle limitations (4.2.7c). The
circles shown in Figure 4.11 represent EE configurations for which an
admissible corresponding joint configuration can be found. An imple-
mentation that solves the above problem on the test platform takes,
111 ms to 1.55 s (238 ms on average) to converge, rendering this opti-
mization approach unsuitable to act as a part of a real-time path or
trajectory planning approach.
Problem Statement (Part 2)

Next, this example will investigate whether the iterative+ method based
from Section 2.1.3 can be applied for such purposes if a solution to (4.2.7)
is considered a nominal solution to a perturbed problem. Therefore, the
desired EE configuration is regarded as the parameter that is subjected
to perturbations.
Implementation (Part 2)
NLP (4.2.7) is thus rewritten as

maximize
q
J(q) = H 2 (q) = abs det J(q)J (q) (4.2.8a)
s.t. g1 (q, p) = p − rE (q) = 0 (4.2.8b)
g2 (q) = qmin − q ≤ 0 (4.2.8c)
g3 (q) = q − qmax ≤ 0 (4.2.8d)
in which the parameter perturbation occurs linearly in (4.2.8b). This

equality constraint as well as the inequality constraints (4.2.8c) and
(4.2.8d) are collected in g (q, p) = g 1 g 2 g 3 ≤ 0.
Results (Part 2)
In order to allow for a comprehensive comparison of the methods dis-
cussed in Section 2.1.3, each of the solution points from the first part
of this example is regarded as a nominal solution. Now the differences

to all other points are used as possible pertubations of the desired EE
configuration. Intuitively, one may suggest that the closer a desired
point is to the nominal position, the more accurate the solution ap-
proximation gets which indeed holds for most cases. Two particular
cases are depicted in Figure 4.11 where the red points are positions
at which a nominal solution was obtained and the sourrounding green
points represent all points for which convergence is achieved using the
iterative+ method. Summarizing all results, the amount of admissible
perturbation points by the iterative approach grows around the center of
the manipulator’s work space and thins out towards the bounds where
hardly any points are reachable, cf. the outer right solution.
The iterative+ method converges for considerable perturbations around

a nominal point, as not only the scheme for larger perturbations but also
the QP-based approach do not yield admissible results. Two particular
situations are shown in Section 2.1.3. The computation time for a
successful approximation process using the improved iterative method
varies between 3.5 ms and 79 ms with an average of 11.5 ms.
Next, the actual path planning process for the synchronization task is
considered.
Path Planning Subproblem

With the initial and terminal configurations either fixed or determined
by optimizing the pose, the path planning task is to find an evolution of
the joint angles to optimally interpolate these given boundary constraints.
Moreover, since in most cases the motion direction at the bounds is defined,
also the path derivatives are prescribed and must be fulfilled. In addition,
intermediate constraints regarding the work space are typically part of such
a planning problem. Such a joint path can be parametrized using a path
parameter s ∈ [0, 1] with s = 0 at the beginning and s = 1 at the terminal
point in order to synchronize the motion of the robot’s joints.
Consider the NLP path planning problem
minimize J(q(s)) (4.2.9a)

q(s)
s.t. q(0) = qinit , q(1) = qterm (4.2.9b)

q (0) = qinit , q (1) = qterm (4.2.9c)

q (0) = qinit , q (1) = qterm

(4.2.9d)
..
.
qmin ≤ q(s) ≤ qmax (4.2.9e)
⎛ ⎞ ⎛ ⎞
x
⎜ min ⎟
x
⎜ max ⎟
⎜ ⎟
⎜ ymin ⎟
⎝ ⎠
≤ rE (q(s)) ≤ ⎜
⎝
⎟
⎜ ymax ⎟
⎠
(4.2.9f)
zmin zmax
formulated in terms of the joint paths q(s) in which a suitable parametriza-

tion w.r.t. the path parameter s is required. While there are many function
classes that are capable of smoothly interpolating these points, B-spline
curves are a typical choice due to their versatility, cf. Section 2.2. The ob-
jective that is minimized in (4.2.9a) is typically a geometric property of the
path, e.g. the integral over the path curvature, or the resulting work space
path length. The boundary conditions regarding the joint position and its
derivatives are considered in (4.2.9b) to (4.2.9d). Constraints regarding
higher derivatives can be formulated in a similar manner. Joint limits are
incorporated as the box constraints (4.2.9e). The admissible work space is
here formulated as being a block-shaped object which is expressed as the
box constraints (4.2.9f) in terms of the EE position (forward kinematics).
Further constraints may include more complex work space shapes, or work
space constraints of other parts of the robot such as intermediate links, and
also possible EE orientation limitations for transporting objects that must
not be tilted, among others.
The following examples show how optimal path planning is performed for
pick and place tasks. Therein, not only the often computationally involved
exact solution of such problems but also fast solution approximation using
parametric sensitivities, i.e. the iterative+ method, is considered.
Example 4.7: SCARA4 (path planning)
Problem Statement and Proposed Solution

This example will continue the pick motion planning for the SCARA4
manipulator started in Example 4.6. Assuming the terminal joint
configuration is either prescribed or otherwise determined such as in
the previous example, the question about an admissible interpolation of
the robot’s initial and terminal configurations arises. B-spline curves
in terms of the path parameter s are selected as the function class for
the joint paths q(s) due to their convenient properties exploitable in
constrained approximation as detailed below. The shape of these curves
is not only influenced by the values of the respective control points, but
also depending on the distribution of knots across the path parameter
range. In this example it is assumed to be sufficient to only manipulate
the control points by considering them the optimization variables.
The robot with n = 4 joints considered in this example is assumed

to be rigid, thus paths that are twice continuously differentiable are
acceptable. Therefore, a B-spline curve with degree p = 3, NCP = 20
control points and open-uniform knot distribution is used for each joint
path. The path planning objective is to generate joint paths with
maximum smoothness, i.e. minimizing the highest non-zero derivative.
Rewriting NLP problem (4.2.9) yields

n

minimize
w
J(w) = d qi (w)dqi (w) (4.2.10a)
i=1
s.t. dq,0 (w) − qinit (p) = 0 (4.2.10b)
dq,NCP −1 (w) − qterm (p) = 0 (4.2.10c)

dq ,0 (w) − qinit (p) = 0 (4.2.10d)

dq ,NCP −2 (w) − qterm (p) = 0 (4.2.10e)

dq ,0 (w) − qinit (p) = 0 (4.2.10f)

dq ,NCP −3 (w) − qterm (p) =0 (4.2.10g)
qi,min 1 ≤ dq,j (w) ≤ qi,max 1, i = 1, . . . , n, j = 3, . . . , NCP − 4
(4.2.10h)
fx (q(sk )) ≤ xmax (4.2.10i)
zmin ≤ fz (q(sk )), k = 1, . . . , Nch (4.2.10j)
with 1 being a vector of ones of appropriate size. Therein the (quadratic)

objective is expressed as the squared sum of control points of the highest
non-zero derivative spline in (4.2.10a). Note, that this NLP problem is
designed to be benign in order to find paths, easily useable with path
tracking methods. Furthermore, it should be exploitable by solution
approximation methods using parametric sensitivities in order to allow
for rapid path planning.
Derivatives of B-spline curves w.r.t. the path parameter s yield B-

spline curves with control points that are a linear combination of the
control points of the original curve. Joint level boundary constraints
are therefore trivially expressed as linear equality constraints (4.2.10b)
to (4.2.10g) by virtue of the boundary control point interpolation prop-
erty. The initial conditions are given by the depicted rest position of
the manipulator. Therein, the terminal position-level threshold qterm
was determined in the previous example. The terminal slope constraint

qterm depends on the joint configuration as well as on the known con-
stant target velocity of the object to be picked. At the initial and the
terminal point, the second path derivative is required to be zero, i.e.

qinit = qterm = 0. Box constraints regarding the joint positions are
formulated in terms of the convex hull property of each joint angle
qi (s), i = 1, . . . , n in (4.2.10h). As the present example features a planar
path planning problem, the work space constraints (4.2.9f) are reduced
to (4.2.10i) and (4.2.10j) wherein f{x,z} represent the respective forward
kinematics functions in x and z direction. These are the only nonlinear
constraints in the above NLP. They are verified at Nch equally spaced
points in the range of the path parameter s ∈ [0, 1]. The initial and
terminal constraints are expressed in terms of the parameter vector

p = q init qinit

qinit q term qterm

qterm
that is later used for solution approximation with parametric sensitivi-

ties.
Results
Figure 4.12 shows the grid of reachable picking positions known from
the previous Example 4.6 with circles for each position for which an
admissible path was found by virtue of (4.2.10). In addition, one
particular optimal EE path is shown in red color. It is used as the
nominal solution to (4.2.10) that serves as the basis for approximations
of optimal paths to other picking position. These are displayed as
blue circles in contrast to the black ones that cannot be reached by
an approximation. It is apparent, that many picking positions can
be reached by an appoximation which is mainly due to the benign
NLP problem (4.2.10), regard therefore the objective and the mostly
linear constraints. The reasons for failures of convergence are twofold:
On the one hand, there is the particular combination of work space
constraints and joint limits, on the other hand some of the terminal joint
configurations determined in the preceding example are not suitable
as some joint angle qi,term may be at its lower or upper bound and the

corresponding joint slope qi,term is non-zero. Although these terminal
configurations are admissible w.r.t. the NLP (4.2.8) of the previous
example, they have to be effectively discarded. Modification of (4.2.8)
with narrowed joint angle bounds may mitigate this issue. Alternatively
(or additionally) the objective function may be augmented by a term
that penalizes joint displacement and centers the angles, i.e. (4.2.8a) is
modified to J(q) = H 2 (q) + αq q with the weight α.
0x
vB
WR
0y
Figure 4.12: Admissible Picking Positions (Black) with One Nominal Path (Red) and
All Possible Picking Position Reachable by Path Approximation (Blue).
Figure 4.13 shows all paths obtained by solution approximation with

parametric sensitivities using the improved iterative method discussed in
Algorithm 2. It is apparent that they show the same principal behavior.
0x
vB
WR
0y
Figure 4.13: Nominal Path (Red) and All Paths to Possible Picking Position
Reachable by Path Approximation (Green).
For the nominal solution shown in Figures 4.12 and 4.13, the success
rate is 78.8% (152/193 samples) which is the highest of all terminal
configurations. An overview of the computational performance in this
particular case – the average, median and maximum computation times
tCPU,avg , tCPU,med and tCPU,max , respectively, and the average, median
and maximum relative cost increases ΔJrel,avg , ΔJrel,med and ΔJrel,max ,
respectively – can be found in Table 4.4. Most paths can be computed
with no or one iterations, which yields low computation times on average
and also little to no cost increase.
Table 4.4: SCARA4 Path Planning: Computation Time and Cost Increase in
Synchronization Tasks (152 Samples).
tCPU,avg tCPU,med tCPU,max ΔJrel,avg ΔJrel,med ΔJrel,max

exact 6.43 s 6.31 s 12.4 s - - -
iterative+ 85.9 ms 4.4 ms 1.8 s 0.7% 0 30.3%
For an overview of the total performance, all feasible 37442 samples were
analyzed, returning a success rate of 57.7% (21618/37442 samples). An
overview of the total computational performance is shown in Table 4.5.
Increased cost, i.e. curvature, is shown for the paths right to the initial
position while increase computation times were found for the longer
paths ending on the far left of the conveyor.
Table 4.5: SCARA4 Path Planning: Computation Time and Cost Increase in
Synchronization Tasks (21618 Samples).

exact 6.38 s 6.24 s 63.7 s - - -
iterative+ 99 ms 10.6 ms 3.5 s 26.5% 0 1 031%
Example 4.8: Comau Racer R3 (path planning)
sync2
sync1
Figure 4.14: Synchronization Tasks of Two Comau Racer R3 Industrial Robots

(adapted from [65] with the author’s permission).
Now the example from [65] is revisited wherein real-time computation

of admissible synchronization trajectories for an industrial 6R robot is
performed, cf. Figure 4.14.
Problem Statement
One subtask therein is to perform optimal path planning under geomet-
ric constraints such that the joint path curvature is minimized. The
joint evolutions are expressed by means of B-spline curves that are pa-
rameterized using a path parameter s ∈ [0, 1], i.e. q = q(s). Successful
synchronization of a manipulator’s EE with a target imposes require-
ments on the initial and terminal joint configuration, q(0) and q(1),
d d
on the path slope ds q(0) and ds q(1) as well as on the path curvature
d2 d2
ds2 q(0) and ds 2 q(1), respectively. Note, that in [65] the curvature
d2
ds2 q(s)
κ(s) = 2 3/2
d
1+ ds q(s)
2
of a single curve q(s) is approximated with κ(s) ≈ ds
d
2 q(s) which is also
used in the referenced publication to allow for comparison of the results.
Further constraints are imposed on the joint angles due to the joints’
physical turn limits. For collision avoidance, the EE motion is required
to be confined within an admissible region such that no collision with
the second robot occurs beyond xmax = 0.6 m as well as the floor occurs
at zmin = 0.3 m.
In the cited work, in order to reduce the computation time, the path
planning problem is solved in its unconstrained form as only a QP re-
mains for which a closed-form exists. Admissibility w.r.t. the constraints
is then restored with a two-stage iterative process. Firstly, admissibility
w.r.t. the joint limits is repaired by iteratively scaling initial and final
conditions. Then, the work space limits are considered by iteratively
adjusting the curves’ control points.
As numerical examples, two synchronization tasks are performed, termed

sync1 and sync2. In each task, both robots’ EEs move along predefined,
cyclic trajectories, for sync1 a circular EE motion, for sync2 an arc-like
motion. At any point in time, the Comau Racer R3 performing either
the circular or arc-like EE motion can be commanded to synchronize its
EE with the EE of the other robot. Therefore infinitely many possible
initial and terminal state pairs for the trajectory of the considered
robot exist and it is not reasonable to precompute and store these
synchronization trajectories.
However, although no issues occurred in the example from [65], this

methodology exhibits two shortcomings that possibly limit its applica-
bility in practical applications: Firstly, in the second iterative process,

the joint limits may become violated. This issue can be mitigated by
another pass of the first iterative part which however may destroy the
admissibility w.r.t. the work space constraints. As this enhancement is
not included in the cited reference, further research may be required
to investigate the properties of such a multi-stage iterative approach.
Secondly, optimality is generally neglected in the iterative processes
and therefore the full potential of the manipulator’s performance is not
exploited.
Proposed Solution
By revisiting the methods to approximate solutions of parametric NLPs
discussed in Section 2.1.3, this path planning example seems to be
efficiently solvable with the improved iterative approximation approach
from Algorithm 2.
In order to apply Algorithm 2, first the optimization problem needs to

be set up. As in the original example in [65], the B-spline control points
serve as optimization variables w, thus dim w = nNCP .
Therefore, the NLP problem describing the geometric path planning

task reads
n

minimize
w
J(w) = d qi (w)dqi (w) (4.2.11a)
i=1
s.t. dq,0 (w) − qinit (p) = 0 (4.2.11b)

dq ,0 (w) − qinit (p) = 0 (4.2.11c)

dq ,0 (w) − qinit (p) =0 (4.2.11d)
dq,NCP −1 (w) − qterm (p) = 0 (4.2.11e)

dq ,NCP −2 (w) − qterm (p) = 0 (4.2.11f)

dq ,NCP −3 (w) − qterm (p) = 0 (4.2.11g)
qi,min 1 − dq,j (w) ≤ 0 (4.2.11h)

dq,j (w) − qi,max 1 ≤ 0, i = 1, . . . , n, j = 3, . . . , NCP − 4
(4.2.11i)
fx (q(sk )) − xmax ≤ 0 (4.2.11j)
zmin − fz (q(sk )) ≤ 0, k = 1, . . . , Nch (4.2.11k)
wherein the path curvatures is sought to be minimal, cf. the objective

function in (4.2.11a). The initial and terminal conditions, cf. (4.2.11b)
to (4.2.11g), are conveniently expressed as linear equality constraints
(boundary control point interpolation) by means of the parameters

p = q init qinit
qinit qterm qterm qterm
which are set according to the synchronization task at hand. The joint
angle limits are incorporated as (4.2.11h) and (4.2.11i). Note, that only
the intermediate control points dqi ,3 , . . . , dqi ,NCP −4 are not influenced by
the boundary conditions which specify the first and last 3 control points.
The robot’s forward kinematics functions are denoted fx and fz for the
inertial x and z direction in the work space constraints (4.2.11j) and
(4.2.11k), respectively. Only the NCP − d − 5 knot spans between td+3
and tNCP −3 are not impacted by the boundary conditions and thus need
to be checked which is facilitated by means of uniformly distributed
check points. Due to this careful selection of constraints, the NLP
problem is kept as small as possible. Furthermore, LICQ violations
between inequality and equality constraints are avoided but they may
arise within the work space limits if both constraints become active in
certain poses of the manipulator. However, these cases are practically
irrelevant as the EE is unlikely to assume a position along this boundary
line as the target path is far away.
Results
In order to allow for comprehensive evaluation of this example, both,
the initial and the target path of the considered manipulators, are
discretized in 32 points each and all resulting 1024 combinations of
initial and terminal configurations were investigated. The B-spline

curves representing the joint angle paths q(s) of degree d = 3, describing
the joint paths, have NCP = 18 control points each.
In order to provide a reference for all approximate results, exact solu-

tions to the perturbed problems are computed with the nominal solution
as the initial guess. Solutions do not exist for all possible combinations
due to the joint bounds limiting the admissible terminal joint configura-
tion. Also, some of the results found using this example exhibit joint
configuration with one or more joint angles at the respective lower or
upper bounds. Such configurations may be problematic for path or
trajectory planning as discussed in Example 4.7.
Table 4.6 gives an overview of the success rate for both paths. Not all
tasks, only 768 out of 1024, were feasible due to the imposed joint angle
limits (4.2.11h) and (4.2.11i). However, it can be seen that nearly all
of the feasible synchronization tasks can be fulfilled using approximate
solutions, i.e. with the iterative+ approach. Note, that admissibility
w.r.t. the constraints of the optimization problem is a property inherent
to this method.
Table 4.6: Success Rates in Synchronization Tasks.
sync1 (768 samples) sync2 (768 samples)

# successful % # successful %
iterative+ 741 96.48% 763 99.35%
Tables 4.7 and 4.8 show details of the computation time (average tCPU,avg ,
median tCPU,med , maximum tCPU,max ) of the exact solution of the path
planning tasks, as well as the relative cost increase (average ΔJrel,avg ,
median ΔJrel,med , maximum ΔJrel,max ) when using the improved iterative
approach with one particular set of initial and terminal configuration
used as the nominal solution. The iterative+ approach is significantly
faster than the exact solution and yields admissible, but suboptimal
results. More detailed analysis shows a correlation between computation
time and relative cost increase, i.e. the longer the methods typically
takes to converge, the more expensive the resulting solution is. Note,
that the high cost increase in this example is mainly due to the broad
variety of requested synchronization paths that are mostly unresembling
to the nominal solution, e.g. consider a terminal desired path slope that
is directly opposed to that of the nominal solution.
Table 4.7: sync1 : Computation Time and Cost Increase in Synchronization Tasks
(768 Samples).

exact 676 ms 658 ms 911 ms - - -
iterative+ 11.1 ms 10.6 ms 57.7 ms 467% 203% 14 600%
Table 4.8: sync2 : Computation time and cost increase in synchronization tasks (768
samples).

exact 712 ms 632 ms 1.19 s - - -
iterative+ 4.3 ms 0.9 ms 22.4 ms 174% 26% 60 200%
For practical implementation of the approximation methods, compre-

hensive investigation of a larger number of possible nominal solutions
may be useful in order to improve convergence and optimality as well
as to reduce computation times. Multiple exact solutions to perturbed
parameter sets can be stored, e.g. at points on a grid of the work space.
Then, the closest exact solution to a perturbation can be selected to
act as a suitable nominal solution.
0.9 0.8
0.8
0.7
0.7
0.6
0.6
0.5
0.5
z
z
0.4 0.4
0.3 0.3
0.2 0.2
0.8 0.8
0.6 0.4 0.6
0.4
0.2 0.6 0.2
0 0 0.6
y −0.2 0.2
0.4
y −0.2 0.4
x 0.2 x
0.8 1
0.7 0.9
0.6 0.8
0.7
0.5
0.6
z
0.4
0.5
0.3
0.4
0.2 0.3
0 0.2
0.6 0.8 0.6
0.4
0.2 0.4
0 0.6 0.2 0.6
y −0.2 0.4 0
0.2 x y −0.2 0.2
0.4
x
Figure 4.15: Synchronization Tasks of Two Comau Racer R3, sync1 : Nominal Path
(Red) and Some Approximations of Optimal Paths (Blue). Measures in m.
1 1
0.9 0.9
0.8 0.8
0.7 0.7
z
z
0.6 0.6
0.5 0.5
0.4 0.4
0.3 0.3
1 0.8 1 0.8
0.6 0.6
0.4 0.6 0.4 0.6
0.2 0.2
0 0.2 0.4 0 0.2 0.4
y x y x
1 1
0.9 0.9
0.8 0.8
0.7 0.7
z
0.6 0.6
0.5 0.5
0.4 0.4
0.3 0.3
1 0.8 1 0.8
0.6 0.6
0.4 0.6 0.4 0.6
0.2 0.2
0 0.2 0.4 0 0.2 0.4
y x y x
Figure 4.16: Synchronization Tasks of Two Comau Racer R3, sync2 : Nominal Path
(Red) and Some Approximations of Optimal Paths (Blue). Measures in m.
As demonstrated in these optimal control examples, due to the high computa-

tional burden and the resulting notable computation time, these approaches
are hardly useful in real-time applications. Instead, methods that yield
faster convergence are required, possibly at the cost of exactness of the
solution regarding optimality. These approaches feature a trade-off between
reduced computation time and quality of the resulting trajectory w.r.t. the
trajectory optimization goal. The iterative+ approach has been confirmed
a viable method to achieve this goal. Online solutions are required in cases
in which the task to be executed is only known shortly before its execution,
leaving only little time for optimal trajectory planning. Examples are pick
& place application where the location of the parts to be picked is not
known in advance, or general changes to the operation environment such
as unpredictably moving work space boundaries occur.
Path Tracking Subproblem

In the second stage, the previously obtained geometric path is used as the
basis to find a suitable time evolution of s = s(t) that considers the dynamic
requirements. The topic of path tracking is discussed in Section 5.1 of this
thesis.
4.2.3 Summary
Summarizing, an OCP such as (4.2.1) includes both, geometric and dynamics
planning subtasks, it represents a trade-off between the quality of results
and its use for real-time applications. Note, that such a staggered strategy,
although computationally efficient, may yield different, possibly degraded
results in terms of the optimization goal compared to those of comprehensive
single-step methods. This is an effect of the optimization goal of the
kinematic part often being different than that the originally desired one
as no time-dependent goals (such as minimum traversal time) or dynamic
objectives can be pursued. The examples in this chapter have shown, that
the iterative+ method based on parametric sensitivities is especially suitable
for fast path and trajectory re-planning.
Chapter 5
Optimal Path Tracking

This chapter discusses the important task of optimal path tracking, i.e.
situations where the path of the is prescribed while its EE trajectory is
not. Therein, joint space and work space path tracking approaches are
distinguished, cf. Section 5.1 and Section 5.2, respectively.
In the former case, the sequence of a robot’s joint positions is given and a
corresponding temporal interpolation is required to be found such that a
given criteria is optimal. Kinematic redundancy is irrelevant for joint space
path tracking approaches.
In the latter setting, a robot’s EE follows a prescribed path. Here the

property of kinematic redundancy is crucial for the complexity of path
tracking methods. For non-redundant manipulatators usually the path
tracking problem can be transformed into a more simple parameterized
joint space formulation, cf. Section 5.2.1. Redundant structures prove to
be more challenging in this regard, cf. the IK schemes from Section 5.2.2.
In this thesis, the discussion about path tracking methods concerns mainly
kinematically redundant manipulators.
An additional family of methods can be summarized as tube following, e.g.

[38, 94]. Therein, although an EE path is prescribed, slight deviations
from it are allowed in order to increase the task execution performance, e.g.
reduce the execution time. However, such approaches cannot be strictly
regarded as path tracking methods, but may be termed path following.

https://doi.org/10.1007/978-3-658-28594-4_5
138 5 Optimal Path Tracking
5.1 Joint Space Approaches

A very early work that investigates exploiting a robot’s capabilities in joint
path tracking is [61]. Therein, a joint trajectory q(t) is prescribed and a
time warp t(τ ), i.e. a reparameterization of time t, is used such that the
robot’s limits such as those of motor speeds and velocities are not violated.
If time-optimality is required, the methods from [102, 101] can be employed.

The latter guarantees successful solution in case of a well-posed path tracking
problem. However, it requires a readily available parameterization of the
joint paths in terms of a path parameter s, i.e. q(s). Finally, the temporal
evolution of s(t) is determined such that the resulting joint angle trajectory
q(s(t)) is optimal.
The path tracking problem in joint space is not in the scope of this thesis.
5.2 Work Space Approaches

In this section, approaches are discussed in which a path for a manipulator’s
EE is given, e.g. the EE orientation R0E and the EE position rE are
prescribed in terms of a path parameter s. The robot is required to track
this path such that not only admissibility but also optimality criteria are
satisfied.
5.2.1 Kinematically Non-Redundant Manipulators

A key element in solving the optimal path tracking problem is the solution
of the IK problem. For kinematically non-redundant manipulators, a given
work space path can be straightforwardly converted into a joint space path
by means of IK. After the IK resolution step, the problem can be solved
with joint space trajectory planning techniques discussed in the previous
Section 5.1.
Therein, a phase-plane representation of the path tracking task can be

formulated, yielding a convex optimization problem [132]. Furthermore,
dynamic programming [93, 64] can be employed to find the optimal path
parameter trajectory.
5.2 Work Space Approaches 139
As the present thesis is focused on path tracking for kinematically redundant

manipulators, investigating this type of path tracking is not one of its aims.
5.2.2 Kinematically Redundant Manipulators

Kinematically redundant manipulators are increasingly employed in path
tracking applications because of their larger work space and better kinematic
dexterity compared to non-redundant robots, and the ability to avoid
singularities.
In [82, 88] the path tracking problem for kinematically redundant manipula-
tors is formulated in terms of a two point boundary value problem. Therein,
a PG method locally maximizing the kinematic manipulability is employed
that allows to reduce the dimension of the problem which is then solved
with calculus of variations. Further approaches using PG methods in com-
bination with a directed manipulability index are found in [30, 28, 142]. As
discussed in Section 3.5.3, it is difficult to select an appropriate performance
index that locally corresponds to the global trajectory optimization goal.
JSD was previously applied for time-optimal path tracking in [50, 135].
However, these phase-plane-based methods exhibit severe shortcomings as.
For example, the terminal joint velocities at the end of a path cannot be
prescribed, i.e. internal motion at the terminal point cannot be avoided.
Furthermore, the approaches were only applied to manipulators with degree
of redundancy r = 1 which does not confirm their general applicability.
In [8], the dimension of the optimization problem is reduced by employing

pseudoinverse-based IK methods which gives rise to the application of phase
plane path tracking methods. Therein, several simplifications are made such
as leaving out velocity-dependent and gravitational terms in the dynamics
model of the considered manipulators.
In the recent multi-step approach [1], first the IK problem is solved on a

pure geometric level, i.e. without time-dependency, in a PG-like manner
in order to obtain a joint path parameterization q(s). Then, standard
time-optimal planning, c.g. [14, 102, 101] is applied to find a suitable
s(t). However, in the derivation only the EE position is considered and
also in the included examples no computation times are reported. It is

especially unclear how the method could be applied in a case in which
the EE orientation is required as well. Thus it is unclear if the presented
approach can be applied in a practical example.
Time-optimality is an important optimization criterion because of economi-

cal reasons. When executing such minimum-time trajectories, the robot’s
capabilities are typically used to their full potential. As a consequence,
additional, possibly previously unaccounted effects such as oscillation due
to excited structural elasticity appear, cf. Example 5.2. As argued in [7,
12], higher-order continuity of trajectories is relevant for avoiding undesired
vibrations and mechanical stress in the structure of the manipulator.
In this section, the path tracking problem for kinematically redundant

manipulators will be investigated. Therein, IK methods discussed in Sec-
tion 3.5.3, namely JSD and GNA, are applied in order to resolve the
redundancy. Thus, the actual motion of the EE along the prescribed EE
path, and hence of the robot, can be determined so to be optimal in some
sense. Using the time needed to accomplish the motion as the optimal-
ity criterion, two examples are used to show the efficacy of the proposed
approaches.
Minimum-time Path Tracking

In order to express minimum-time path tracking as an OCP, (2.3.1) is
rewritten as
&T
minimize 1dt = T (5.2.1a)
x(t),u(t),T
0
s.t. x0 − x(0) = 0 (5.2.1b)
r(x(T )) ≤ 0 (5.2.1c)
ẋ(t) − f (x(t), u(t)) = 0 (5.2.1d)
h(x(t), u(t)) ≤ 0, ∀t ∈ [0, T ] (5.2.1e)
While in the examples in the previous chapter a B-spline-based approach

was selected, here the respective implementations of the optimization prob-
lems closely feature the direct multiple shooting approach discussed in

Section 2.3.2. To this end, the time domain [0, T ] is discretized into N
uniform intervals [tk , tk+1 ], k = 0, . . . , N − 1 with t0 = 0 and tN = T . A
shorthand notation is used such that (·)k = (·)(tk ). The joint position/an-
gles q(t) and the path parameter s(t) are represented by the optimization
variables

w = x 0 u 0 x 1 u 1 · · · x N u N T .
Note, that the time grid is being scaled when T changes in w. The
choice of the IK resolution method determines the state vector x
and the control vector u, e.g. in the case of C 4 trajectories x k =
...
(sk , ṡk , s̈k , sk , sk , q k , q̇ r,k , q̈ r,k , q r,k , qr,k ), u k = (sk , qr,k ) for JSD, and
(3) (4) (4) (5) (5)
...
x k = (sk , ṡk , s̈k , sk , sk , q k , γ k , γ̇ k , γ̈ k , γ k ), u k = (sk , γk ) for GNA.
(3) (4) (5) (4)
Therein, the IK expressions (3.5.34) and (3.5.36) for JSD and (3.5.47)
and (3.5.51) for GNA are used. The control trajectories are piecewise
constant. Therefore, the NLP problem corresponding to minimum-time
path tracking reads
minimize
w
T (5.2.2a)
s.t. s0 = 0, sN = 1 (5.2.2b)
(i) (i) (i) (i)
q0 = q0 , qN = qf , i = 0, . . . , ν (5.2.2c)
xk+1 − RK4(xk , uk ) = 0 (5.2.2d)
IK: JSD or GNA (5.2.2e)
0 ≤ sk ≤ 1 (5.2.2f)
ṡk ≥ 0 (5.2.2g)
(i) (i)
qmin ≤ qk ≤ qmax
(i)
, i = 0, . . . , ν + 1 (5.2.2h)
τk = M(qk )q̈k + g(qk , q̇k ) (5.2.2i)
τmin ≤ τk ≤ τmax (5.2.2j)
wherein the terminal time T is minimized, c.f. (5.2.2a).

The initial and final conditions regarding the path parameter and the
joint coordinates are given by (5.2.2b) and (5.2.2c). Therein, the overlined
quantities, e.g. qf , represent desired values at the respective bounds.
The equality constraint (5.2.2d) enforces the compatibility at the boundaries

of the shooting intervals. On each interval, the state xk is time integrated
under the influence of the control uk . In this example, this is facilitated by
means of a fourth-order Runge-Kutta scheme denoted RK4.
Choosing an IK method used for path tracking, i.e. JSD or GNA, is reflected
in (5.2.2e).
(5.2.2f) and (5.2.2g) constrain the time evolution of the path parameter
s(t). (5.2.2h) represents box constraints imposed on the joint coordinates
and its time derivatives denoted with their respective orders i = 0 for
q(0) = q(t), i = 1 for q(1) = q̇(t), etc. The system dynamics is considered
as the robot’s equations of motion in inverse dynamics form (5.2.2i) and
the torque constraint (5.2.2j).
Initial Guess
The above NLP problem (5.2.2) is in general non-convex and nonlinear
due to the system dynamics and the effect modeled therein. Therefore, the
selection of an initial guess is important, i.e. the closer it is to a solution
to the problem, the lower the computational load. Admissibility w.r.t. the
constraints also improves the quality of an initial guess.
A possible choice for the initial guess is to compute the joint angles by time
integration of the non-augmented IK solution
(4) (3) (3) (3)
k (VE,d,k − Jk q̇k − 3J̈k q̈k − 3J̇k qk ).
qinit,k = J+
Thereby, the path tracking requirement is fulfilled, and also the box con-
straints regarding the joint quantities and the motor torques (5.2.2h)
to (5.2.2j) are satisfied when initialized with a large enough terminal time
T . This also yields the initial guess for the redundant coordinates qr and
their time derivatives for JSD. For GNA, the scaling factors γ are initialized
with zeros.
Example 5.1: SCARA4 (higher-order path tracking for kinemati-

cally redundant manipulators)
Task Description
In this example, which was published in the author’s previous publica-
tions [114, 115], the kinematically redundant SCARA4 lab prototype
from Figure 5.1 is considered. The goal is to compute joint trajectories
q(t) with a continuity requirement up to C 4 such that the EE performs
a minimum-time rest-to-rest motion, tracking the quarter circle path
shown in Figure 5.1 in counterclockwise direction. Therein, JSD as well
as the GNA approach will be used to formulate the IK as a means to
satisfy the path following requirement.
terminal configuration q(T )
path rE (s)
initial configuration q0
Figure 5.1: SCARA4 Minimum-Time Path Tracking.
In order to reduce complexity, only the planar motion of the manipulator

is relevant for this trajectory planning task. The four-link manipulator
is inherently kinematically redundant which is further increased as only
the EE position coordinates and not the orientation are considered task
space coordinates of the path tracking task at hand, i.e. the degree
of redundancy is rE ∈ T ⊂ R2 with l = dim T = 2 < n. The path is
specified as desired EE position in terms of the path parameter s ∈ [0, 1],

i.e. rE (s). The forward kinematics is given by rE (q). To that end, the
EE twist has only translational components, i.e. VE = vE and the
Jacobian yields J = ∂∂vq̇E .
Optimization Problem
The formulation of the optimization problem reflecting the problem
described above follows the direct multiple shooting approach (5.2.2)
using N = 200 uniform time intervals. Therein, the boundary conditions
for the path parameter trajectory are specified in (5.2.2b). As a rest-
to-rest motion is required, the initial and final constraints (5.2.2c) are
(3) (4) (3) (4)
specified as q0 = q0 and q̇0 , q̈0 , q0 , q0 , q̇N , q̈N , qN , qN = 0. The
final joint configuration qN is subject to the optimization. In (5.2.2h),
q and its time derivatives are constrained except for the acceleration
which is indirectly limited by the torque constraints (5.2.2i) and (5.2.2j).
Regarding the IK resolution, for JSD all six possible decompositions of
the joint set are tested, as well as GNA.
Initial Guess
For the present example, a B-spline curve of degree 6 is used for s, the
duration of the trajectory is conservatively initialized with tf,init = 3 s.
Results
The optimization problem (5.2.2) was implemented using CasADi [2]
and solved with Ipopt.
Table 5.1 shows the trajectory planning results for the above example.
Therein, optimal trajectory durations for all joint space decompositions
as well as for GNA are reported. The numbers denote the choice of
redundant joints, i.e. JSD (1,2) means q r = (q1 , q2 ). Additionally,
the corresponding computation times tCPU are listed. It should be
mentioned that the resulting minima may not be global as (5.2.2) is
non-convex.
Figures 5.2 to 5.4 show the optimal C 4 time evolutions of the path
parameter s, the joint angles q and the motor torques τ , respectively.
Note that the axes of all joint quantities are normalized w.r.t. their
respective limits and the time axes are normalized w.r.t. T . The
visible steps in the motor torques in Figure 5.4 are due to Coulomb
friction effects, i.e. they occur at sign changes of the corresponding
joint velocities, c.f. Figure 5.3. The optimal final configuration of the
robot in the C 4 case is depicted in Figure 5.1. The time evolutions are
only reported for GNA as the results for JSD are very similar which is
mainly due to the common initial guess.
As expected, the trajectory duration is slightly increased with the

order of continuous differentiability. The optimal trajectory duration
is very similar in all cases of a particular choice. The convergence,
i.e. the number of iterations, is similar for JSD and GNA. However,
the computational burden and thus the solution time is significantly
higher for GNA due to the complexity of the IK expressions. Not
all joint space decompositions yielded an admissible result. For the
decomposition with q r = (q2 , q4 ) the Jacobian Jnr is singular in the
initial configuration. JSD switching algorithms that choose a suitable
separation and thus avoid algorithmic singularities should be therefore
investigated. The optimization problem with q r = (q3 , q4 ) does not
converge within 1000 iterations.
s s 2 · 10−1 ṡ 4 · 10−2 s̈
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.2: SCARA4 Minimum-Time Path Tracking: Path Parameter s and

Derivatives.
q/qmax q1 q2 q3 q4
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
q̇/q̇max
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(3)
q(3) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(3)
q(3) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(5)
q(5) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.3: SCARA4 Minimum-Time PathTracking: Joint Angles q and Derivatives.

Symmetric Limits: qi,max = π2 rad, q̇max = π2 , π2 , π, π rad/s, qi,max =
(3)
(4) (5)
103 rad/s3 , qi,max = 104 rad/s4 , qi,max = 105 rad/s5 .
τ /τmax τ1 τ2 τ3 τ4
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.4: SCARA4 Minimum-Time Path Tracking: Motor Torques τ . Symmetric

Limits: τmax = (0.336, 0.336, 0.142, 0.142) Nm.
Table 5.1: SCARA4 Minimum-Time Path Tracking: Results (times in seconds).
Mode T #iter tCPU

JSD (1,2) 0.9635 199 31.1
JSD (1,3) 0.9635 215 33.7
JSD (1,4) 0.9635 170 26.0
C2
JSD (2,3) 0.9635 210 32.6
JSD (2,4) infeasible
JSD (3,4) no convergence
GNA 0.9635 166 69
Mode T #iter tCPU
JSD (1,2) 1.0030 203 43.4
JSD (1,3) 1.0030 197 42.2
JSD (1,4) 1.0030 225 47.9
C3
JSD (2,3) 1.0030 138 29.4
GNA 1.0030 148 327
Mode T #iter tCPU
JSD (1,2) 1.0747 245 67.9
JSD (1,3) 1.0747 221 61.2
JSD (1,4) 1.0748 216 58.8
C4
JSD (2,3) 1.0747 240 63.4
GNA 1.0747 279 5578
Example 5.2: Stäubli TX90L (path tracking for kinematically re-

dundant manipulators)
Problem Statement
Consider the redundant 7 DOF manipulator in Figure 5.5. It will serve
as another use case of higher order optimal path tracking. This example
has been largely adopted from the author’s previous publications [114,
115].
tool axis
Figure 5.5: Stäubli TX90L Minimum-Time Path Tracking (from [114]).
The manipulator consists of a 6 DOF industrial robot (Stäubli TX90L)

mounted on a linear axis. The EE is required to move along an
ellipse as shown in Figure 5.5. The tool axis is required to stay
perpendicular to the ellipse, but the EE can freely rotate about the
tool axis. This corresponds to a five axis manipulation task, such as
grinding, milling, spray painting, or gluing. The robot is operated with
a model-based control scheme consisting of a nonlinear feed-forward
and linear feedback control. A time optimal solution was computed,
where the joint trajectories are required to be C 2 continuous, i.e. twice
continuously differentiable. This solution trajectory satisfies the limits
on velocity, acceleration, and actuator forces/torques according to the
robot specification (reduced to 90% to account for some control margin).
qlag,1 q1 q2 q3 qlag,1
q4 q5 q6 q7 qlag,2...7
2 200
1 100
0 0
−1 −100
t in s
0 5 · 10−2 0.1 0.15 0.2 0.25 0.3
Figure 5.6: Stäubli TX90L Minimum-Time Path Tracking, Experiment C 2

Trajectories: Tracking Error of Joint Positions qlag , qlag,1 in mm, qlag,2...7 in mdeg.
τ /τmax τ1 τ2 τ3 τ4 τ5 τ6 τ7
1
0
−1 t in s
0 5 · 10−2 0.1 0.15 0.2 0.25 0.3
Figure 5.7: Stäubli TX90L Minimum-Time Path Tracking, Experiment C 2

trajectories: Measured Motor Torques τ . Symmetric torque limits according to the
robot specification.
The problem becomes obvious from Figure 5.6 that shows the tracking
error qlag = qd −q with qd being the desired joint position. More crucial
than this low tracking accuracy is the violation of the actual motor
torque limits as apparent from Figure 5.7 that causes an emergency
stop of the robot at around 0.07 s. The latter is due to the bounded
but discontinuous jerk and the unbounded jounce of the trajectories.
This example investigate the application of trajectories with higher

continuity such that above problems are avoided.
Task Description
The goal is to determine joint trajectories such that a prescribed elliptic
EE path is tracked in a minimum time rest-to-rest motion while the last
axis of the spherical wrist (tool axis) is required to stay perpendicular
to the ellipse (the EE is free to rotate about this axis). The path is
defined in closed-form in terms of a path parameter s ∈ [0, 1]. The joint
trajectories are required to be continuously differentiable up to four times
in order to avoid undesired excitation of elasticity effects in the robot

structure. The manipulator depicted in Figure 5.5 consists of a serial
chain with one prismatic joint and six revolute joints, i.e. C = R × T6 .
The corresponding joint coordinate vector is q = (q1 , . . . , q7 ), and
n = dim C = 7. The EE pose is described by coordinates zE =
(R0E , rE ) ∈ SE(3), and the work space is W ⊂SE(3), m = dim W = 6.
The manipulator is considered kinematically redundant with degree
(n − m) = 1. The task space T is of dimension l = dim T = 5 as
the EE position and only the rotation about two axes are considered.
Therefore, the robot is task redundant with degree rT = (n − l) = 2. For
convenience, the angular EE velocity is expressed in the local coordinate
frame where only two components

are relevant, i.e. E ω E = (ωx , ωy ).
Thus the EE twist V E = E ω E , v E consists of only five elements.
Optimization Problem
See the previous Example 5.1.
Initial Guess
The initial guess is computed as in the previous Example 5.1 with the
terminal time conservatively initialized to Tinit = 5 s.
Results
The optimization results for the spatial example are found in Table 5.2.
JSD was performed for all 21 possible combinations of redundant coordi-
nates. Table 5.2 only shows the results for separations with the largest
(worst) and the smallest (best) final time. Not all choices converged,
some encountered algorithmic singularities. The corresponding occur-
rences as well as all computation times tCPU are also listed. Again, note
that the resulting minima may not be global as (5.2.2) is non-convex.
The NLP problem (5.2.2) was implemented using CasADi 3.1 [2] avail-
able at the time of the initial submission of [115] and solved with Ipopt.
Computation times for GNA are higher than for JSD as the evalua-
tion of the constraint functions, especially the IK, is computationally

expensive.
The C 2 and C 3 trajectories obtained by JSD and GNA are very similar
which is mainly due to the common initial guess. Figures 5.8 to 5.10
show the optimal C 4 time evolutions of the path parameter s, the joint
angles q and the motor torques τ , respectively, that were obtained with
JSD. Note that the axes of all joint quantities are normalized w.r.t. their
respective limits as are the time axes w.r.t. T . The visible steps in the
motor torques in Figure 5.10 are again due to Coulomb friction effects,
i.e. they occur at sign changes of the corresponding joint velocities, c.f.
Figure 5.9.
The C 4 trajectory obtained using JSD was experimentally verified using

the robot depicted in Figure 5.5 that uses PD joint control with feed-
forward torques. The recorded lag error shown in Figure 5.11 is deemed
negligible for most applications, therefore the trajectory planning result
is admissible. Note that these results heavily depend on the accuracy
of the model used to compute the feed-forward torques by means of
inverse dynamics. Therein, knowledge of accurate model parameters is
crucial. Details about the identification process for the robot considered
in this example are found in [91].
This example indicates the importance of a certain continuity of the

time optimal trajectory. The necessary order of continuity of the joint
trajectories, and thus of the EE trajectory, can be deduced by regarding
the robot as a dynamical control system. A realistic model of an
industrial robot is that of a rigid link manipulator with serial-elastic
actuators, i.e. the links are modeled as rigid bodies connected by ideal
joints to which elastic motor/gear units are attached [118]. This is a
differentially flat control system with the EE position as its flat output.
It was shown that it is differential and feedback linearizable [34, 52, 97].
Moreover, the vector relative degree is 4. Consequently, the joint (and
thus EE) trajectory and its time derivatives up fourth order determine
the actuator forces/torques. In other words, the prescribed trajectories
must be C 4 continuous for the actuator forces/torques to be continuous.
If C 4 continuity is not satisfied, then vibrations of the elastic components

of the robot (gear/motor unit) will be excited. Time-optimal trajectories
make full use of the dynamic capabilities of the robot so that actuator
forces/torques, determined from the feed-forward control, are close to
the robot-specific limits. When vibrations excited, due to insufficiently
continuous trajectories, feedback control forces/torques occur in addition
to the feed-forward terms (which are already close to the respective
limits). As a further consequence, the path tracking performance is
degraded. This can be seen in Figure 5.7 normalized w.r.t. the specified
limits.
s s 4 · 10−1 ṡ 5 · 10−2 s̈
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.8: Stäubli TX90L Minimum-Time Path Tracking: Path Parameter s and
Derivatives.
(q − qmin )/(qmax − qmin ) q1 q2 q3 q4 q5 q6 q7

1
0.8
0.6
0.4
0.2 t/tf
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
q̇/q̇max
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(3)
q(3) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(4)
q(4) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(5)
q(5) /qmax
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.9: Stäubli TX90L Minimum-Time Path Tracking: Joint Angles q and
Derivatives Normalized w.r.t. Their Respective Limits
(q1,min = 0, q2,...,7,min = (−π, −2.3, −2.5, −4.7, −2, −3.5) rad,
q1,max = 2.55 m, q2,...,7,max = (π, 2.6, 2.5, 4.7, 2.4, 3.5) rad. Symmetric derivative limits:
q̇max = (4, 7, 7, 7, 8.7, 7.9, 12.6) rad/s, qi,max = 103 rad/s3 , qi,max = 104 rad/s4 , qi,max =
(3) (4) (5)
5 5
10 rad/s .).
τ /τmax τ1 τ2 τ3 τ4 τ5 τ6 τ7
1
0.5
0
−0.5 t/tf
−1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.10: Stäubli TX90L Minimum-Time Path Tracking: Motor Torques τ .

Normalized w.r.t. Symmetric Limits: τmax = (69, 42, 42, 17.5, 4.5, 3.4, 2.2) Nm.
qlag,1...2 q1 q2 qqlag,1
3 q4 qlag,2
q5 q6 q7 qlag,3...7
1000 100
0 0
−1000 −100
t/tf
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 5.11: Stäubli TX90L Minimum-Time Path Tracking: Tracking Error of Joint
Positions qlag , qlag,1 in μm, qlag,2...7 in mdeg.
Table 5.2: Stäubli TX90L Minimum-Time Path Tracking: Results (times in

seconds).
Mode T #iter tCPU

JSD (best) 0.7557 (1,2) 398 551.0
JSD (worst) 0.7601 (3,5) 256 291.5
C2
(1,{3,4,6}), (2,{3..7}), (3,{4,6}),
JSD (issues)
(4,6), (5,{6,7}), (6,7)
GNA 0.7586 248 2521
Mode T #iter tCPU
JSD (best) 0.8827 (1,5) 1045 1854.4
JSD (worst) 0.9037 (2,3) 533 1307.6
C3
(1,{3,4,6,7}), (2,{5,6,7}), (3,{4..7}),
JSD (issues)
(4,{6,7}), (5,{6,7}), (6,7)
GNA 0.8827 587 14287
Mode T #iter tCPU
JSD (best) 1.0253 (1,7) 152 319.2
JSD (worst) 1.0812 (1,2) 261 545.7
C4
(1,{3,4,6}), (2,{5,7}), (3,{4,6}),
JSD (issues)
(4,6), (5,{6,7}), (6,7)
GNA 1.094 270 2515
For GNA, due to the problem size in the C 4 case, cf. the IK expressions
(3.5.51), a limited-memory quasi-Newton approximation of the Hessian
matrix was used instead of exact derivatives. Convergence was achieved
for relaxed optimality tolerances.
Chapter 6
Conclusion
As its title suggests, this thesis is concerned with optimal path and trajec-
tory planning for serial manipulators. For the treatment of this subject, the
mathematical background regarding numerical optimization and therein es-
pecially parametric problems, B-spline curves and optimal control problems
is required which is provided in this thesis. In addition, robot kinematics, i.e.
forward and inverse kinematics on position, velocity and higher derivative
level, is also examined rigorously in this thesis as a preparation for more
advanced topics.
On the one hand, point-to-point tasks are treated such that trajectories
are computed that are optimal in some given sense, e.g. so that a robot
consumes minimum energy during a transportation task.
On the other hand, optimal trajectory planning includes optimal path

tracking tasks in which the task space is reduced to a prescribed path, i.e.
the EE is required to track a given path in minimum time. Therein, inverse
kinematics is used to resolve the path given in terms of EE coordinates into
joint coordinates. For kinematically redundant robots this is especially of
interest as this class of robots exhibits internal motion capabilities which is
optimally exploitable in that regard.

https://doi.org/10.1007/978-3-658-28594-4_6
156 6 Conclusion
Optimal Path and Trajectory Planning

The present thesis covers particular aspects of robotic path and trajectory
planning. Therein, especially point-to-point problems are considered for
which initial and final conditions are specified along with technological
limits of the robot employed in a process or the process itself. To that end,
the corresponding numerical optimization problem may either cover the
entire process at once, i.e. the inputs are only initial and final conditions
as well as technical constraints and the result are joint trajectories, cf.
Examples 4.3 and 4.4. Or, as an alternative, a multi-step approach can
be pursued wherein the geometric and the dynamics aspects are treated
subsequently. In the first, geometric part of this iterative process, path
planning is performed, consider for instance Examples 4.6 and 4.7. Therein
for a kinematically redundant manipulator first the terminal configuration
is determined by maximizing a joint configuration-dependent performance
index, and then a geometrically admissible joint path is planned from
a given initial configuration. Therefore, the geometric initial and final
conditions as well as geometric work space constraints are required. In the
second step, the robot’s EE is required to track the path which gives rise
to path tracking methods depending on the type of robot employed in the
process. Therein, the constraints regarding kinematics, e.g. velocities, and
dynamics, e.g. input torque, are required. The process is complete once an
admissible, optimal solution is found. Otherwise, the geometric path needs
to be re-planned or the optimization problem relaxed.
In this context, also the topic of fast computation of approximations to

optimal solutions using parametric sensitivities is discussed. Therein, given
a solution to an optimization problem for a nominal set of parameters,
parametric sensitivities are employed to compute an approximation for the
case of parameter perturbations. A novel iterative method is proposed
that allows large perturbations that inflict a change of the active set of
constraints compared to the nominal problem. This represents an advantage
over established method which is shown in comparative examples. Moreover,
the efficacy of fast planning methods using this iterative approach is shown
by means of numerical examples in simulations and experiments. Future
6 Conclusion 157
work will investigate whether the methods described in [108] can improve
the convergence of the presented active set update approach.
Optimal Path Tracking for Kinematically

Redundant Robots
In this thesis, two methods regarding inverse kinematics in optimal path
tracking for kinematically redundant robots are presented: JSD and GNA.
While the former is an existing approach and known for simple cases, it
is extended in the present thesis and the author’s previous publications.
The latter is a novel method that was proposed by the author. While in
JSD the joint set is separated into two parts, allowing a parameterized
IK solution at modest computational expense, there are several shortcom-
ings. A notable drawback therein are algorithmic singularities, rendering
particular decomposition combinations unusable. An open challenge is a
switching strategy between various decomposition combinations during an
optimization process if such an algorithmic singularity occurs, resulting
in a multi-stage optimization scheme. The challenge therein lies in the
initialization of subsequent stages due to major differences between the
formulation arising from different decompositions. GNA allows IK treat-
ment on derivative-level. Therein, an IK solution is augmented by weighted
nullspace basis vectors whose weights are computed to be optimal w.r.t.
the objective of the path tracking problem. While the methods directly
exploits the redundancy and does not suffer from algorithmic singularities,
its computational demands are higher than those of JSD which is subject to
further research. The efficacy of these approaches is shown using numerical
examples and experiments.
Furthermore, regarding high-performance path tracking applications, this

thesis addresses the IK methods JSD and GNA in higher derivative-level
formulation, i.e. on acceleration, jerk, or jounce-level, etc. These particular
methods are applied in order to ensure continuous differentiability of the
resulting joint trajectories and thus avoid unnecessary excitation of elastic
effects unmodeled in the system dynamics. For both considered IK methods,
JSD and GNA, the required computational effort and therefore time required
158 6 Conclusion
to compute a solution grows with the derivative order. This can be possible
overcome by a recursive IK formulation. Alternatively, this task can be
formalized with a modified OCP, cf. Section 2.3 based on the system’s
forward dynamics. Note, that this approach requires time derivatives of the
EoM which is computationally involved as well. Comparative investigations
about the computational trade-off between an OCP formulation and an
IK-based approach as proposed in this thesis are an open challenge.
Dynamic programming can also be used to express and solve such path
tracking problems. Such approaches are successfully applied for path track-
ing with non-redundant robots as the dynamics can be uniquely re-written
in terms of a path parameter, reducing the problem to finding a single
trajectory. For kinematically redundant structures this is not straightfor-
wardly possible, as the IK resolution is ambiguous and only a parameterized
solution can be obtained. In both, the JSD and the GNA methods, the
number of states grows linearly with the degree of kinematic redundancy.
However, in dynamics programming, the computational complexity grows
exponentially with the number of states which may limit this idea’s practical
feasibility, especially regarding higher derivative levels.
In this thesis, single, stationary, serial robots are considered. Future

investigations might include collaboration tasks of multiple serial robots,
forming a kinematically parallel structure. Therein, the concept of nullspace
augmentation by weighting nullspace basis vectors may be applied in the
context of actuation redundancy, which is dual to the kinematic redundancy
treated in the previous chapters. Another possible application lies in the
field of mobile robotics. Consider, for example, a robotic arm attached to a
mobile platform such as a car, a flying drone or a spaceship. As the degree
of kinematic redundancy is increased, optimal inverse kinematics within
the optimal path tracking framework from Section 5.2.2 is enabled.
Bibliography
[1] Khaled Al Khudir and Alessandro De Luca. “Faster Motion on Carte-
sian Paths Exploiting Robot Redundancy at the Acceleration Level”.
In: IEEE Robotics and Automation Letters 3.4 (2018), pp. 3553–
3560.
[2] Joel A E Andersson et al. “CasADi – A software framework for
nonlinear optimization and optimal control”. In: Mathematical Pro-
gramming Computation (2018).
[3] Jorge Angeles. “The design of isotropic manipulator architectures
in the presence of redundancies”. In: The International Journal of
Robotics Research 11.3 (1992), pp. 196–201.
[4] John Baillieul. “Avoiding obstacles and resolving kinematic redun-
dancy”. In: Robotics and Automation. Proceedings. 1986 IEEE In-
ternational Conference on. Vol. 3. IEEE. 1986, pp. 1698–1704.
[5] John Baillieul. “Kinematic programming alternatives for redundant
manipulators”. In: Robotics and Automation. Proceedings. 1985 IEEE
International Conference on. Vol. 2. IEEE. 1985, pp. 722–728.
[6] John Baillieul, John Hollerbach, and Roger Brockett. “Programming
and control of kinematically redundant manipulators”. In: Decision
and Control, 1984. The 23rd IEEE Conference on. Vol. 23. IEEE.
1984, pp. 768–774.
[7] Pierre-Jean Barre et al. “Influence of a jerk controlled movement law
on the vibratory behaviour of high-dynamics systems”. In: Journal
of Intelligent and Robotic Systems 42.3 (2005), pp. 275–293.

https://doi.org/10.1007/978-3-658-28594-4
160 Bibliography
[8] Francesco Basile and Pasquale Chiacchio. “A contribution to

minimum-time task-space path-following problem for redundant
manipulators”. In: Robotica 21.2 (2003), pp. 137–142.
[9] Joachim Baumgarte. “Stabilization of constraints and integrals of
motion in dynamical systems”. In: Computer methods in applied
mechanics and engineering 1.1 (1972), pp. 1–16.
[10] Adi Ben-Israel and Thomas NE Greville. Generalized inverses: theory
and applications. Vol. 15. Springer Science & Business Media, 2003.
[11] John T Betts. Practical methods for optimal control and estimation
using nonlinear programming. Vol. 19. Siam, 2010.
[12] Corrado Guarino Lo Bianco. “Minimum-jerk velocity planning for
mobile robot applications”. In: IEEE Transactions on Robotics 29.5
(2013), pp. 1317–1326.
[13] Lorenz T Biegler. Personal Communication. Jan. 2018.
[14] James E Bobrow, Steven Dubowsky, and JS Gibson. “Time-optimal
control of robotic manipulators along specified paths”. In: The in-
ternational journal of robotics research 4.3 (1985), pp. 3–17.
[15] Hans Georg Bock and Karl-Josef Plitt. “A multiple shooting algo-
rithm for direct solution of optimal control problems”. In: IFAC
Proceedings Volumes 17.2 (1984), pp. 1603–1608.
[16] Stephen Boyd and Lieven Vandenberghe. Convex optimization. Cam-
bridge university press, 2004.
[17] Hartmut Bremer. Dynamik und Regelung mechanischer Systeme.
Teubner Studienbücher, 1988.
[18] Hartmut Bremer. Elastic Multibody Dynamics: A Direct Ritz Ap-
proach. Heidelberg: Springer Verlag, 2008.
[19] Christof Büskens. “Echtzeitoptimierung und Echtzeitoptimals-
teuerung parametergestörter probleme”. In: (2002).
[20] Christof Büskens. “Optimierungsmethoden und sensitivitäts-
analyse für optimale steuerprozesse mit steuer-und zustands-
beschränkungen”. PhD thesis. Universität Münster, 1998.
Bibliography 161
[21] Christof Büskens. “Real-time solutions for perturbed optimal control

problems by a mixed open-and closed-loop strategy”. In: Online
optimization of large scale systems. Springer, 2001, pp. 105–116.
[22] Christof Büskens and Helmut Maurer. “Sensitivity analysis and real-
time optimization of parametric nonlinear programming problems”.
In: Online Optimization of Large Scale Systems. Springer, 2001,
pp. 3–16.
[23] Samuel R Buss. “Introduction to inverse kinematics with jacobian
transpose, pseudoinverse and damped least squares methods”. Un-
published survey. Oct. 2009. url: http://www.math.ucsd.edu/
~sbuss/ResearchWeb/ikmethods/index.html.
[24] Pyung Chang. “A closed-form solution for inverse kinematics of robot
manipulators with redundancy”. In: IEEE Journal on Robotics and
Automation 3.5 (1987), pp. 393–403.
[25] Y-C Chen and Ian D Walker. “A consistent null-space based ap-
proach to inverse kinematics of redundant robots”. In: Robotics and
Automation, 1993. Proceedings., 1993 IEEE International Confer-
ence on. IEEE. 1993, pp. 374–381.
[26] T Chettibi et al. “Minimum cost trajectory planning for industrial
robots”. In: European journal of mechanics. A, Solids 23.4 (2004),
pp. 703–715.
[27] P Chiaacchio and Mariano Concilio. “The dynamic manipulability
ellipsoid for redundant manipulators”. In: Proceedings. 1998 IEEE
International Conference on Robotics and Automation (Cat. No.
98CH36146). Vol. 1. IEEE. 1998, pp. 95–100.
[28] Pasquale Chiacchio. “Exploiting redundancy in minimum-time path
following robot control”. In: American Control Conference, 1990.
IEEE. 1990, pp. 2313–2318.
[29] Stefano Chiaverini. “Task-priority redundancy resolution with ro-
bustness to algorithmic singularities”. In: IFAC Proceedings Volumes
27.14 (1994), pp. 453–459.
[30] Stephen L Chiu. “Task compatibility of manipulator postures”. In:
The International Journal of Robotics Research 7.5 (1988), pp. 13–21.
162 Bibliography
[31] E Sahin Conkur and Rob Buckingham. “Clarifying the definition of

redundancy as used in robotics”. In: Robotica 15.5 (1997), pp. 583–
586.
[32] Maurice G Cox. “The numerical evaluation of B-splines”. In: IMA
Journal of Applied Mathematics 10.2 (1972), pp. 134–149.
[33] Carl De Boor. “On calculating with B-splines”. In: Journal of Ap-
proximation theory 6.1 (1972), pp. 50–62.
[34] A De Luca. “Decoupling and feedback linearization of robots with
mixed rigid/elastic joints”. In: Int. J. Rob. Nonlin. Cont. 8 (1998),
pp. 965–977.
[35] Alessandro De Luca, Leonardo Lanari, and Giuseppe Oriolo. “A
sensitivity approach to optimal spline robot trajectories”. In: Auto-
matica 27.3 (1991), pp. 535–539.
[36] Alessandro De Luca and Giuseppe Oriolo. “The reduced gradient
method for solving redundancy in robot arms”. In: Robotersysteme
7.2 (1991), pp. 117–122.
[37] Alessandro De Luca, Giuseppe Oriolo, and Paolo Robuffo Giordano.
“Kinematic modeling and redundancy resolution for nonholonomic
mobile manipulators”. In: Robotics and Automation, 2006. ICRA
2006. Proceedings 2006 IEEE International Conference on. IEEE.
2006, pp. 1867–1873.
[38] Frederik Debrouwere et al. “Time-optimal tube following for robotic
manipulators”. In: Advanced Motion Control (AMC), 2014 IEEE
13th International Workshop on. IEEE. 2014, pp. 392–397.
[39] Moritz Diehl and Sébastien Gros. Numerical Optimal Control (pre-
liminary draft). University of Freiburg, 2017. url: https://www.
syscop.de/files/2017ss/NOC/script/book-NOCSE.pdf.
[40] Kees van den Doel and Dinesh K Pai. “Performance measures for
robot manipulators: A unified approach”. In: The International
Journal of Robotics Research 15.1 (1996), pp. 92–111.
Bibliography 163
[41] Rajiv V Dubey, James A Euler, and Scott M Babcock. “An effi-
cient gradient projection optimization scheme for a seven-degree-
of-freedom redundant robot with spherical wrist”. In: Robotics and
Automation, 1988. Proceedings., 1988 IEEE International Confer-
ence on. IEEE. 1988, pp. 28–36.
[42] Olav Egeland. “Task-space tracking with redundant manipulators”.
In: IEEE Journal on Robotics and Automation 3.5 (1987), pp. 471–
475.
[43] Francisco Facchinei, Andreas Fischer, and Christian Kanzow. “On
the accurate identification of active constraints”. In: SIAM Journal
on Optimization 9.1 (1998), pp. 14–32.
[44] H.J. Ferreau et al. “qpOASES: A parametric active-set algorithm
for quadratic programming”. In: Mathematical Programming Com-
putation 6.4 (2014), pp. 327–363.
[45] Enrico Ferrentino and Pasquale Chiacchio. “Redundancy
Parametrization in Globally-Optimal Inverse Kinematics”.
In: International Symposium on Advances in Robot Kinematics.
Springer. 2018, pp. 47–55.
[46] Anthony V Fiacco. Introduction to sensitivity and stability analysis
in nonlinear programming. Elsevier, 1983.
[47] Anthony V Fiacco. “Sensitivity analysis for nonlinear programming
using penalty methods”. In: Mathematical programming 10.1 (1976),
pp. 287–311.
[48] Angel Flores-Abad et al. “Optimal Capture of a Tumbling Object
in Orbit Using a Space Manipulator”. In: Journal of Intelligent &
Robotic Systems 86.2 (2017), pp. 199–211.
[49] M Galassi et al. GNU Scientific Library Reference Manual , ISBN
0954612078. GSL. url: https://www.gnu.org/software/gsl/.
[50] Miroslaw Galicki. “Time-optimal controls of kinematically redundant
manipulators with geometric constraints”. In: IEEE Transactions
on Robotics and Automation 16.1 (2000), pp. 89–93.
164 Bibliography
[51] Hubert Gattringer et al. “Dynamical Analysis of a Tip Balancing

Cube”. In: PAMM 17.1 (2017), pp. 147–148.
[52] Hubert Gattringer et al. “Recursive methods in control of flexible
joint manipulators”. In: Multibody System Dynamics 32.1 (2014),
pp. 117–131.
[53] Christoph Glocker. “Dynamik von Starrkörpersystemen mit Reibung
und Stößen”. PhD thesis. ETH Zürich, 1995.
[54] Maria da Graça Marcos, JA Tenreiro Machado, and T-P Azevedo-
Perdicoúlis. “Trajectory planning of redundant manipulators using
genetic algorithms”. In: Communications in nonlinear science and
numerical simulation 14.7 (2009), pp. 2858–2869.
[55] Knut Graichen and Nicolas Petit. “Incorporating a class of con-
straints into the dynamics of optimal control problems”. In: Optimal
Control Applications and Methods 30.6 (2009), pp. 537–561.
[56] Knut Graichen, Michael Treuer, and Michael Zeitz. “Swing-up of the
double pendulum on a cart by feedforward and feedback control with
experimental validation”. In: Automatica 43.1 (2007), pp. 63–71.
[57] Knut Graichen et al. “Handling constraints in optimal control with
saturation functions and system extension”. In: Systems & Control
Letters 59.11 (2010), pp. 671–679.
[58] M Hajžman and P Polach. “Application of stabilization techniques
in the dynamic analysis of multibody systems”. In: Applied and
Computational Mechanics 1.2 (2007), pp. 479–488.
[59] Hideo Hanafusa, Tsuneo Yoshikawa, and Yoshihiko Nakamura. “Anal-
ysis and control of articulated robot arms with redundancy”. In:
IFAC Proceedings Volumes 14.2 (1981), pp. 1927–1932.
[60] GA Hicks and WH Ray. “Approximation methods for optimal control
synthesis”. In: The Canadian Journal of Chemical Engineering 49.4
(1971), pp. 522–528.
[61] John M Hollerbach. “Dynamic scaling of manipulator trajectories”.
In: Journal of Dynamic Systems, Measurement, and Control 106.1
(1984), pp. 102–106.
Bibliography 165
[62] M. Husty, M. Pfurner, and H. Schröcker. “A new and efficient algo-

rithm for the inverse kinematics of a general serial 6R manipulator”.
In: J. Mechanism and Mach. Theory 42.1 (Jan. 2007), pp. 66–81.
[63] Matthias Jörgl, Philip Hörmandinger, and Andreas Müller. “Dy-
namical Modeling and Swing-Up Control of a Self-balancing Cube”.
In: Advances in Robot Design and Intelligent Control: Proceedings
of the 25th Conference on Robotics in Alpe-Adria-Danube Region
(RAAD16). Vol. 540. Springer. 2016, p. 144.
[64] Dominik Kaserer, Hubert Gattringer, and Andreas Müller. “Nearly
Optimal Path Following with Jerk and Torque Rate Limits Using
Dynamic Programming.” In: IEEE Robotics and Automation Letters
(2018).
[65] Dominik Kaserer, Hubert Gattringer, and Andreas Müller. “Online
Robot-Object Synchronization With Geometric Constraints and
Limits on Velocity, Acceleration, and Jerk”. In: IEEE Robotics and
Automation Letters 3.4 (2018), pp. 3169–3176.
[66] Robert Katzschmann et al. “Towards online trajectory generation
considering robot dynamics and torque limits”. In: IEEE/RSJ IROS
2013. IEEE. 2013, pp. 5644–5651.
[67] Jacek Kierzenka and Lawrence F Shampine. “A BVP solver based
on residual control and the Maltab PSE”. In: ACM Transactions on
Mathematical Software (TOMS) 27.3 (2001), pp. 299–316.
[68] Charles A Klein and Ching-Hsiang Huang. “Review of pseudoinverse
control for use with kinematically redundant manipulators”. In:
IEEE Transactions on Systems, Man, and Cybernetics 2 (1983),
pp. 245–250.
[69] Matthias Knauer and Christof Büskens. “Real-Time Trajectory Plan-
ning of the Industrial Robot IRB6400”. In: PAMM: Proceedings in
Applied Mathematics and Mechanics. Vol. 3. 1. Wiley Online Library.
2003, pp. 515–516.
[70] Torsten Kröger. On-line Trajectory Generation in Robotic Systems:
Basic Concepts for Instant. Reactions to Unforseen Events. Springer,
2012.
166 Bibliography
[71] Vyacheslav Kungurtsev and Moritz Diehl. “SQP Methods for Para-
metric Nonlinear Optimization”. In: Optimization Online (2014),
pp. 1–35.
[72] Huibert Kwakernaak and Raphael Sivan. Linear optimal control
systems. Vol. 1. Wiley-Interscience New York, 1972.
[73] Roberto Lampariello et al. “Trajectory planning for optimal robot
catching in real-time”. In: Robotics and Automation (ICRA), 2011
IEEE International Conference on. IEEE. 2011, pp. 3719–3726.
[74] J Leger and J Angeles. “A Redundancy-resolution Algorithm for Five-
degree-of-freedom Tasks via Sequential Quadratic Programming”. In:
TrC-IFToMM Symposium on Theory of Machines and Mechanisms„
Izmir. 2015.
[75] Daniel B Leineweber. “Efficient reduced SQP methods for the op-
timization of chemical processes described by large sparse DAE
models”. PhD thesis. VDI Verlag Düsseldorf, 1999.
[76] Renhui Li. “Strukturierte Modellbildung und Implementierung von
mechanischen Systemen”. MA thesis. Johannes Kepler University,
2009.
[77] Zhi Li et al. “Synthesizing redundancy resolution criteria of the
human arm posture in reaching movements”. In: Redundancy in
Robot Manipulators and Multi-robot systems. Springer, 2013, pp. 201–
240.
[78] Alain Liégeois. “Automatic Supervisory Control of the Configuration
and Behavior of Multibody Mechanisms”. In: IEEE Transactions on
Systems Man and Cybernetics 12 (1977), pp. 868–871.
[79] J. Löfberg. “YALMIP : A Toolbox for Modeling and Optimization
in MATLAB”. In: In Proceedings of the CACSD Conference. Taipei,
Taiwan, 2004.
[80] J Luh, M Walker, and R Paul. “Resolved-acceleration control of
mechanical manipulators”. In: IEEE Transactions on Automatic
Control 25.3 (1980), pp. 468–474.
Bibliography 167
[81] Anthony A Maciejewski and Charles A Klein. “Obstacle avoidance

for kinematically redundant manipulators in dynamically varying
environments”. In: The international journal of robotics research 4.3
(1985), pp. 109–117.
[82] Daniel P Martin, John Baillieul, and John M Hollerbach. “Resolution
of kinematic redundancy using optimization techniques”. In: IEEE
Transactions on Robotics and Automation 5.4 (1989), pp. 529–533.
[83] Mustafa Mashali, Redwan Alqasemi, and R Dubey. “Task priority
based dual-trajectory control for redundant mobile manipulators”.
In: Robotics and Biomimetics (ROBIO), 2014 IEEE International
Conference on. IEEE. 2014, pp. 1457–1462.
[84] Johannes Mayr et al. “Exploiting the Equations of Motion For Biped
Robot Control with Enhanced Stability”. In: Multibody Dynamics.
Springer, 2016, pp. 299–321.
[85] Andreas Müller. “Forward dynamics of variable topology mecha-
nisms—The case of constraint activation”. In: Proceedings of the
Institution of Mechanical Engineers, Part K: Journal of Multi-body
Dynamics 230.4 (2016), pp. 442–454.
[86] Richard M Murray. A mathematical introduction to robotic manipu-
lation. CRC press, 1994.
[87] Yoshihiko Nakamura and Hideo Hanafusa. “Inverse kinematic solu-
tions with singularity robustness for robot manipulator control”. In:
Journal of dynamic systems, measurement, and control 108.3 (1986),
pp. 163–171.
[88] Yoshihiko Nakamura and Hideo Hanafusa. “Optimal redundancy
control of robot manipulators”. In: The International Journal of
Robotics Research 6.1 (1987), pp. 32–42.
[89] Yoshihiko Nakamura, Hideo Hanafusa, and Tsuneo Yoshikawa. “Task-
priority based redundancy control of robot manipulators”. In: The
International Journal of Robotics Research 6.2 (1987), pp. 3–15.
[90] Dragomir N Nenchev. “Redundancy resolution through local op-
timization: A review”. In: Journal of robotic systems 6.6 (1989),
pp. 769–798.
168 Bibliography
[91] Matthias Neubauer, Hubert Gattringer, and Hartmut Bremer. “A

persistent method for parameter identification of a seven-axes ma-
nipulator”. In: Robotica 33.5 (2015), pp. 1099–1112.
[92] Jorge Nocedal and Stephen J Wright. Numerical Optimization.
2nd ed. Springer, 2006.
[93] M Oberherber, H Gattringer, and A Müller. “Successive dynamic
programming and subsequent spline optimization for smooth time
optimal robot path tracking”. In: Mechanical Sciences 6.2 (2015),
pp. 245–254.
[94] Matthias Oberherber et al. “A Task Space Approach for Planar
Optimal Robot Tube Following”. In: Proceedings of the 13th In-
ternational Conference on Informatics in Control, Automation and
Robotics. SCITEPRESS-Science and Technology Publications, Lda.
2016, pp. 327–334.
[95] Christina Oberlin and Stephen J Wright. “Active set identification
in nonlinear programming”. In: SIAM Journal on Optimization 17.2
(2006), pp. 577–605.
[96] Yonghwan Oh, WK Chung, and Y Youm. “General task execution
of redundant manipulators with explicit null-motion control”. In:
Industrial Electronics, Control, and Instrumentation, 1996., Proceed-
ings of the 1996 IEEE IECON 22nd International Conference on.
Vol. 3. IEEE. 1996, pp. 1902–1908.
[97] G. Palli, C. Melchiorri, and A. De Luca. “On the Feedback Lineariza-
tion of Robots with Variable Joint Stiffness”. In: Proc. IEEE Int.
Conf. Rob. Aut. (IROS), Pasadena, CA, USA, May 19-23. 2008.
[98] Jonghoon Park, WanKyun Chung, and Youngil Youm. “Weighted
decomposition of kinematics and dynamics of kinematically redun-
dant manipulators”. In: Robotics and Automation, 1996. Proceedings.,
1996 IEEE International Conference on. Vol. 1. IEEE. 1996, pp. 480–
486.
[99] Sarosh Patel and Tarek Sobh. “Manipulator performance measures-a
comprehensive literature survey”. In: Journal of Intelligent & Robotic
Systems 77.3-4 (2015), pp. 547–570.
Bibliography 169
[100] Martin Pfurner. “Closed form inverse kinematics solution for a redun-
dant anthropomorphic robot arm”. In: Computer Aided Geometric
Design 47 (2016), pp. 163–171.
[101] Hung Pham and Quang-Cuong Pham. “A new approach to time-
optimal path parameterization based on reachability analysis”. In:
IEEE Transactions on Robotics (2018).
[102] Quang-Cuong Pham. “A general, fast, and robust implementation
of the time-optimal path parameterization algorithm”. In: IEEE
Transactions on Robotics 30.6 (2014), pp. 1533–1540.
[103] Les Piegl and Wayne Tiller. The NURBS book. Springer Science &
Business Media, 2012.
[104] Donald Lee Pieper. “The kinematics of manipulators under computer
control”. PhD thesis. Stanford Univ., Dept. Of Computer Science,
1968.
[105] François G Pin et al. “Motion planning for mobile manipulators
with a non-holonomic constraint using the FSP (full space param-
eterization) method”. In: Journal of robotic systems 13.11 (1996),
pp. 723–736.
[106] Hans Pirnay, Rodrigo López-Negrete, and Lorenz T Biegler. “Opti-
mal sensitivity based on IPOPT”. In: Mathematical Programming
Computation 4.4 (2012), pp. 307–331.
[107] Ron P Podhorodeski, Andrew A Goldenberg, and Robert G Fenton.
“Resolving redundant manipulator joint rates and identifying spe-
cial arm configurations using Jacobian null-space bases”. In: IEEE
transactions on robotics and automation 7.5 (1991), pp. 607–618.
[108] Daniel Ralph and Stephan Dempe. “Directional derivatives of the
solution of a parametric nonlinear program”. In: Mathematical pro-
gramming 70.1-3 (1995), pp. 159–172.
[109] Alexander Reiter. “An explicit approach for time-optimal trajectory
planning for kinematically redundant robots”. MA thesis. Johannes
Kepler University Linz, Institute of Robotics, Dec. 2014.
170 Bibliography
[110] Alexander Reiter. Time-Optimal Trajectory Planning for Redundant

Robots: Joint Space Decomposition for Redundancy Resolution in
Non-Linear Optimization. Springer, 2016.
[111] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “On
Redundancy Resolution in Minimum-Time Trajectory Planning of
Robotic Manipulators Along Predefined End-Effector Paths”. In:
Informatics in Control, Automation and Robotics. Springer, 2018,
pp. 190–206.
[112] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “Real-
time computation of inexact minimum-energy trajectories using
parametric sensitivities”. In: International Conference on Robotics
in Alpe-Adria Danube Region. Springer. 2017, pp. 174–182.
[113] Alexander Reiter, Hubert Gattringer, and Andreas Müller. “Re-
dundancy Resolution in Minimum-time Path Tracking of Robotic
Manipulators.” In: ICINCO (2). 2016, pp. 61–68.
[114] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “Inverse
kinematics in minimum-time trajectory planning for kinematically
redundant manipulators”. In: Industrial Electronics Society, IECON
2016-42nd Annual Conference of the IEEE. IEEE. 2016, pp. 6873–
6878.
[115] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “On
Higher-Order Inverse Kinematics Methods in Time-Optimal Tra-
jectory Planning for Kinematically Redundant Manipulators”. In:
IEEE Transactions on Industrial Informatics (2018).
[116] Alexander Reiter, Andreas Müller, and Hubert Gattringer. “Rapid
Nearly-Optimal Rendezvous Trajectory Planning Using Parame-
ter Sensitivities”. In: ROMANSY 22–Robot Design, Dynamics and
Control. Springer, 2019, pp. 515–522.
[117] Alexander Reiter et al. “An explicit approach for time-optimal
trajectory planning for kinematically redundant serial robots”. In:
PAMM 15.1 (2015), pp. 67–68.
Bibliography 171
[118] Roland Riepl, Hubert Gattringer, and Hartmut Bremer. “Decentral-

ized Control of an Industrial Robot with Elastic Gears”. In: PAMM
10.1 (2010), pp. 51–52.
[119] Stephen M Robinson. “Perturbed Kuhn-Tucker points and rates of
convergence for a class of nonlinear-programming algorithms”. In:
Mathematical programming 7.1 (1974), pp. 1–16.
[120] Seyed Sina Mirrazavi Salehian, Mahdi Khoramshahi, and Aude
Billard. “A dynamical system approach for softly catching a flying
object: Theory and experiment”. In: IEEE Trans. on Robotics 32
(2016), pp. 462–471.
[121] Lorenzo Sciavicco and Bruno Siciliano. “A dynamic solution to the
inverse kinematic problem for redundant manipulators”. In: Robotics
and Automation. Proceedings. 1987 IEEE International Conference
on. Vol. 4. IEEE. 1987, pp. 1081–1087.
[122] Lorenzo Sciavicco and Bruno Siciliano. “A solution algorithm to the
inverse kinematic problem for redundant manipulators”. In: IEEE
Journal on Robotics and Automation 4.4 (1988), pp. 403–410.
[123] Lorenzo Sciavicco and Bruno Siciliano. “Coordinate transformation:
A solution algorithm for one class of robots”. In: IEEE transactions
on systems, man, and cybernetics 16.4 (1986), pp. 550–559.
[124] Bruno Siciliano. “A closed-loop inverse kinematic scheme for on-line
joint-based robot control”. In: Robotica 8 (1990), pp. 231–243.
[125] Bruno Siciliano. “Kinematic control of redundant robot manipulators:
A tutorial”. In: Journal of intelligent and robotic systems 3.3 (1990),
pp. 201–212.
[126] Bruno Siciliano and Oussama Khatib. Springer Handbook of Robotics.
Springer, 2016.
[127] Bruno Siciliano et al. Robotics: Modelling, Planning and Control.
Springer-Verlag London, 2009.
[128] Niko Sünderhauf et al. “The limits and potentials of deep learning
for robotics”. In: The International Journal of Robotics Research
37.4-5 (2018), pp. 405–420.
172 Bibliography
[129] Krzysztof Tchoń and Janusz Jakubiak. “Extended Jacobian inverse

kinematics algorithms for mobile manipulators”. In: Journal of Field
Robotics 19.9 (2002), pp. 443–454.
[130] Bipop Team. Siconos. INRIA Grenoble Rhone Alpes. url: http:
//siconos.gforge.inria.fr/4.1.0/html/users_guide/index.
html.
[131] Gaurav Tevatia and Stefan Schaal. “Inverse kinematics for humanoid
robots”. In: Robotics and Automation, 2000. Proceedings. ICRA’00.
IEEE International Conference on. Vol. 1. IEEE. 2000, pp. 294–299.
[132] Diederik Verscheure et al. “Time-energy optimal path tracking for
robots: a numerically efficient optimization approach”. In: Proceed-
ings of the 10th international workshop on advanced motion control.
2008, pp. 727–732.
[133] Andreas Wächter and Lorenz T Biegler. “On the implementation of
an interior-point filter line-search algorithm for large-scale nonlinear
programming”. In: Mathematical programming 106.1 (2006), pp. 25–
57.
[134] Charles Wampler. “Inverse kinematic functions for redundant ma-
nipulators”. In: Robotics and Automation. Proceedings. 1987 IEEE
International Conference on. Vol. 4. IEEE. 1987, pp. 610–617.
[135] Mitsuru et al. Watanabe. “Time optimal path-tracking control of
kinematically redundant manipulators”. In: JSME International
Journal Series C Mechanical Systems, Machine Elements and Man-
ufacturing 47.2 (2004), pp. 582–590.
[136] D. E. Whitney. “Resolved Motion Rate Control of Manipulators
and Human Prostheses”. In: IEEE Transactions on Man-Machine
Systems 10.2 (Feb. 1969), pp. 47–53.
[137] William A Wolovich and H Elliott. “A computational technique for
inverse kinematics”. In: Proc. 23rd IEEE Conference on Decision
and Control. Vol. 23. IEEE. 1984, pp. 1359–1363.
[138] Tsuneo Yoshikawa. “Dynamic manipulability of robot manipulators”.
In: Transactions of the Society of Instrument and Control Engineers
21.9 (1985), pp. 970–975.
Bibliography 173
[139] Tsuneo Yoshikawa. “Manipulability of robotic mechanisms”. In: The

international journal of Robotics Research 4.2 (1985), pp. 3–9.
[140] Ralf Zehetner. Trajektorienplanung und Regelung des Furuta-Doppel-
Pendels. Bachelor’s Thesis, JKU Linz. June 2018.
[141] Jakob Ziegler et al. “Generating realistic trajectories for robotic
hippotherapy from 3D captured horseback motion”. In: The 5th
Joint International Conference on Multibody System Dynamics. June
2018.
[142] Leon Zlajpah. “Dexterity measures for optimal path control of re-
dundant manipulators”. In: Proceedings of International Conference
on Automatisation and Control (ICAR). 1996.
Appendix A
B-Spline Curves
In Section 2.2, the numerical basics regarding B-spline curves have been
reviewed. Therein, not only the simple evaluation, but also the optimization-
based construction of such functions have been discussed. In this appendix
chapter, although they are not required for the topics in the main part,
additional features of B-splines are documented that find application in a
software tool that was developed as a by-product of the work leading to
the present thesis.
In many applications, a parameterization of a B-spline curve in terms of

its arc length can be useful, e.g. in the field of mobile robotics for path
planning purposes as the parameter then represents the distance traveled
along the path. Such a parameterization will be derived for the simple one-
and also for the multi-dimensional case in Appendix A.1.
Construction and evaluation of B-spline curves in curve parameter as well as

in arc length representation have been implemented in a software libary, see
Appendix A.2. It is not only intended to be primarily used in robotics tasks
such as path and trajectory planning, but also in more general applications
as it is built upon the framework for symbolic computation and numerical
optimization CasADi [2].

https://doi.org/10.1007/978-3-658-28594-4
176 A B-Spline Curves
A.1 Arc Length Parameterization

Arc length parameterization refers to an alternative representation of the
B-spline curve q(t) in terms of its arc length l, i.e. q(l). Therein, the
original spline definition (2.2.1) can be re-used by utilizing an intermediate
parameterization of the spline parameter in terms of the arc length length,
i.e. t(l). In the following such a function will be derived, again using
constrained B-spline approximation.
Consider a B-spline curve q(t) ∈ Rn that is parameterized w.r.t. its native

parameter t ∈ [a, b]. Its arc length at a path parameter value l is
⎧ 1
⎪ 0t 2
⎪
⎪
⎨ 1 + dτd q(τ ) dτ n=1
l(t) = ⎪
a 1
t ∈ [a, b]. (A.1.1)
⎪ 0t
⎪ d d
dτ q(τ ) dτ q(τ ) dτ n>1
⎩
a
The total length of the spline curve is thus found by setting t = b in (A.1.1).
For the parameterization t(l), the inverse of the above function needs to
be found which is non-trivial. However, an approximation t̂(l), can be
calculated as shown in the following Algorithm 4.
Algorithm 4 Computation of arc length parameterization as B-spline
curve.
q(t) ∈ Rn : B-spline curve parameterized in terms of parameter t

τ ← t 1 t 2 . . . t n : collect knots of all B-spline curves
remove duplicates in τ τ : vector of unique knots
nτ ← length of τ

l ← l l(τ1 ) l(τ2 ) . . . l(τnτ ) : vector of arc lengths at knots
τ ← ( ): vector of intermediate grid points
l ← ( ): vector of arc lengths at intermediate points
for all knot intervals [τk , τk+1 ] ∈ τ , k = 1, . . . , nτ − 1 do
compute N intermediate points τk < τk,1 < τk,2 < · · · < τk,N < τk+1

τ ← τ τk,1 τk,2 . . . τk,N : collect intermediate grid points

l ← l l(τk,1 ) l(τk,2 ) . . . l(τk,N ) : compute arc lengths at interme-
diate grid points
end for
return τ , l, nτ , τ , l
A.1 Arc Length Parameterization 177
Therein, first the knots of all splines are collected in the vector of unique
knots τ of length nτ that has only unique, monotonically increasing el-
ements. The resulting function t̂(l) will interpolate the arc lengths the
knots contained in τ . Therefore, the arc length value at these points are
computed and stored in the vector l of the same length nτ . Then, on each
interval of τ , N intermediate points are computed and stored in the vector
τ of length N (nτ − 1). The corresponding arc lengths are stored in the
vector l of the same length. These will be used to approximate the evolution
of the arc length between the knots.
The arc length parameterization as a B-spline curve t̂(l) with degree p, NCP
control points (which are collected in vector dt̂ ) and open-uniform knot
distribution is finally found by solving the constrained approximation QP
problem
τ −1)
1 N (n 2
minimize τ i − t̂ li (A.1.2a)
dt̂ 2 i=1
s.t. t̂(li ) = τi , ∀ li ∈ l, ∀ τi ∈ τ (A.1.2b)
that is a special form of (2.2.11) wherein τi , li , τ i and li are the i-th elements
of the vectors τ , l, τ and l, respectively.
While the objective in (A.1.2a) is used to quadratically approximate the

arc length function at the grid points collected in the vector τ , the equality
constraints (A.1.2b) represent the arc lengths at the knots collected in
τ that are interpolated. In addition, a term that smoothes the resulting
function t̂(l) can be included, cf. (2.2.11).
The degree p should be chosen as the highest degree (or higher) that occurs
in any element of the original curve q(t) in order to preserve the original
degree of continuous differentiability. The number of intermediate points N
and the number of control points dt̂ need to be chosen in order to sufficiently
capture the arc length evolution.
178 A B-Spline Curves
A.2 B-Spline Software Package

As a side-project to the work leading up to the present thesis, a software
tool to construct and evaluate B-spline n-D curves in multiple development
environments was developed. Since its internal release, it has been used in
various industrial projects and theses.
Overview of functionality
• Evaluation: recursive (slow, but accurate) or matrix-vector (fast, but

inaccurate) mode.
• Evaluation: path parameterization.
• Construction: simple mode (with all elements known).
• Construction: constraint approximation mode.
The functionality is available as a native C++ implementation with an

easy-to-use MATLAB interface (matrix-vector evaluation/construction),
and also partly as an easy-to-use class-based MATLAB-CasADi library
(recursive evaluation/construction).
Furthermore, evaluation as well as construction by constrained approxima-

tion of B-spline surfaces are also implemented in the tool. This functionality,
however, is not documented here as it is entirely out of scope of the present
thesis.
A release as open-source software is planned for 2019.
A.2.1 Open Challenges

Performance Improvement, Code Refactoring
In order to increase the performance of the provided library, refactoring it
as a native C++ CasADi-based implementation is advisable. This would
also increase its versatility and enable its use on more platforms.
A.2 B-Spline Software Package 179
Path Parameter Constraints

In certain cases, it is required to prescribe a particular arc length to a curve.
This, however, yields a nonlinear programming problem as the arc length is
nonlinear w.r.t. the B-spline curve value q(t), cf. (A.1.1). Examples for
possible applications are path and trajectory planning in mobile robotics.
Appendix B
Mechatronic Multibody
Systems
In this chapter, the essential subject of computational modeling of mecha-
tronic systems such as robots as well as the solution of the resulting
differential-algebraic equations (DAE) is discussed.
For robotic systems, a model of their kinematic and dynamic behavior

is required. Model stands for a mathematical representation of inherent
physical processes.
Deriving a mathematical model representing a system’s dynamics is facili-

tated by means of well-known analytic, e.g. Lagrange’s formalisms, or
synthetic techniques, e.g. Newton-Euler equations, or Bremer’s Projec-
tion Equation [17, 18]. Once such a dynamics model has been established,
it can be applied for various purposes such as
• simulation, i.e. numerical computation of the solution of the

differential-algebraic equations of motion (EoM),
• system analysis, e.g. investigation of inherent system properties such
as stability,
• parameter identification and
• model-based feed-forward and feed-back control

https://doi.org/10.1007/978-3-658-28594-4
182 B Mechatronic Multibody Systems
A model is derived with its designated purpose in mind. To this end, a

certain level of detail has to be carefully chosen that appropriately balances
computational cost with physical accuracy by considering only relevant
effects. As a consequence, practical models suffer from inherent inaccuracies
limiting their applicability. Reducing the level of detail of a once-derived
model is enabled by means of model reduction techniques.
The EoM are given as differential equations, or possibly differential-algebraic

equations. To this end, the question about their solution, i.e. finding a
time-dependent function that solves the EoM, arises which is treated in the
following section.
Example B.1: AWARO (modeling the dynamics of a two-legged

walking machine)
An example for model reduction is presented in
[84] where the dynamics of a two-legged walking
robot from Figure B.1 is considered.
Therein, at first, a rather detailed model of the
mechanical system is derived. It incorporates not
only the torso and the limbs represented as rigid
bodies, but also actuator units that include struc-
tural gear elasticity. This model with as many
as 34 degrees of freedom (DOF) is intended for
use in (offline) parameter identification and for
(online) inverse dynamics-based computation of
feed-forward control torques in order to increase
joint trajectory tracking accuracy.
However, the resulting model is not suited for real-
time tasks such as numerical simulation required
for pre-computing the near future step-sequence in
Figure B.1: an iterative manner due to its high computational
Schematics of cost. Therefore, system reduction measures are
Autonomous Walking taken such as omitting gear elasticity, yielding
Robot (AWARO). 20 DOF.
B.1 Solution of the Equations of Motion 183
Additionally assuming ideal joint position tracking, justified by the

application of the mentioned feed-forward control, and thus avoiding
joint dynamics, only the 6 position and orientation torso coordinates
are left.
Furthermore, dropping also angular momentum results in a mere 3 DOF,

i.e. the system is modeled as an inverted three-dimensional pendulum.
This simple model is then used for real-time iterative gait pattern
generation.
B.1 Solution of the Equations of Motion

The EoM of a mechatronic system are denoted
M(q)s̈ + h(q, ṡ) = Q(q, u), (B.1.1a)

subjected to q̇ = H(q)ṡ, (B.1.1b)
wherein q denotes the vector of minimal (position) coordinates, ṡ is the

vector of non-holonomic velocities, M is the generalized inertia matrix, h
is a vector of generalized forces containing nonlinear terms due to Coriolis
and friction terms and Q denotes generalized forces due to system inputs u
such as external forces and torques.
B.1.1 Constraints
A system such as (B.1.1) can be subjected to kinematic equality constraints
on position level, i.e. geometric constraints, or on velocity level. Note, that
velocity-level constraints exist that do not necessarily have a correspondence
on position-level, i.e. non-holonomic constraints.
B.1.1.1 Geometric Constraints

Geometric, i.e. holonomic, constraints are collected in the vector
g(q) = 0 (B.1.2)
which is in general nonlinear w.r.t. the system coordinates q.

B.1.1.2 Velocity Constraints

A consequence of the position-level requirement (B.1.2) is the corresponding
velocity-level constraint
d d
ġ(q, q̇) = g(q) = g(q) q̇ = 0 (B.1.3)
dt dq

Jg (q)
that is obtained straightforwardly by time-derivation. Therein, Jg is called

the holonomic velocity constraint Jacobian.
In contrast, non-holonomic velocity-level constraints
d
ġ(q, ṡ) = ġ(q, ṡ) ṡ = 0 (B.1.4)
dṡ

Jġ (q)
in general do not have a direct correspondence on position-level. How-

ever, a combination multiple non-holonomic velocity-level constraints may
yield a holonomic velocity-level constraint and thus has a position-level
correspondence.
B.1.1.3 Constrained Equations of Motion

In order to satisfy above constraints, constraint forces λ are introduced
into the equations of motion, resulting in the system of DAE
M(q)s̈ + h(q, ṡ) − A (q)λ = Q(q, u) (B.1.5a)

subjected to q̇ = H(q)ṡ (B.1.5b)
⎡ ⎤
⎣
Jg (q)H(q) ⎦
ṡ = 0. (B.1.5c)
Jġ (q)

A(q)
Therein, the holonomic and non-holonomic velocity constraints are collected

in (B.1.5c) and denoted in Pfaffian form A(q)ṡ = 0. Note that in (B.1.5)
position-level constraints (B.1.2) are not included, i.e. are not required to
be satisfied.
Note that this scheme is not restricted to minimal coordinate formula-

tion. In fact, a choice of non-minimal, i.e. redundant, coordinates can be
implemented together with a set of additional geometric constraints.
Holonomic velocity coordinates are considered a special case with H(q) = I,

i.e. ṡ = q̇. Consequently non-holonomic constraints are also omitted, i.e.
Jġ = O.
B.1.2 Solution of System of DAEs

With the time derivative of (B.1.5c)
d
(A(q)ṡ) = Ȧ(q, q̇)ṡ + A(q)s̈ = 0, (B.1.6)
dt
(B.1.5) can be re-written as
⎡ ⎤⎛ ⎞ ⎛ ⎞
⎣
M(q) −A(q) ⎦⎝ s̈ ⎠
Q(q, u) − h(q, ṡ)
=⎝ ⎠ (B.1.7a)
A(q) O λ −Ȧ(q, q̇)ṡ
subjected to q̇ = H(q)ṡ. (B.1.7b)
The goal of the next step is to solve (B.1.7) for the accelerations s̈. These of
course can be found by simply solving the above (n + m) × (n + m) system
of linear equations.
B.1.3 Divide & Conquer: Constraint Forces and

Accelerations
However, by adding an intermediate step to compute the constraint forces,
the computational effort is changed to the solutions of an m × m system
and two n × n systems. With \ being the operator that indicates solving a
system of linear equations (rather than left-multiplying the inverse of the
matrix), re-writing (B.1.5a) yields
M(q)s̈ + h(q, ṡ) − A (q)λ = Q(q, u)

s̈ − M(q)\A (q)λ = M(q)\(Q(q, u) − h(q, ṡ))
A(q)s̈ − A(q)M(q)\A (q)λ = A(q)M(q)\(Q(q, u) − h(q, ṡ))
and by substituting (B.1.6), the constraint forces λ are found to be

' (
λ = − A(q)M(q)\A (q) \ A(q)M(q)\(Q(q, u) − h(q, ṡ)) + Ȧ(q, q̇)ṡ .
(B.1.8)
Now that the generalized constraint forces λ are known, (B.1.5a) is solved
for the accelerations s̈, i.e.
M(q)s̈ + h(q, ṡ) − A (q)λ = Q(q, u)

s̈ = M(q)\ Q(q, u) − h(q, ṡ) + A (q)λ . (B.1.9)
Note that in (B.1.8) and (B.1.9), three systems of linear equations with the
matrix M = M > 0 need to be solved. Therein, the opportunity to further
reduce the computational burden by applying a re-usable decomposition of
M arises, e.g. Cholesky factorization.
B.1.4 Solution by Time Integration

State-Space Representation
By re-writing the system of second-order DAEs (B.1.5) as system of first-
order DAEs, i.e. state-space representation with the state
⎛ ⎞
⎝
q⎠
x= , (B.1.10)
ṡ
the solution is finally obtained by time integration

⎛ ⎞ ⎛ ⎞
⎝
q̇ ⎠ ⎝
H(q)ṡ ⎠
ẋ = = (B.1.11)
s̈ s̈
with the velocity transformation (B.1.5b).
Numerical integration introduces drift effects due to accumulation of nu-

merical artifacts resulting from round-off errors and accuracy limitations
of the used number format. Thereby, the included position and velocity
constraints are no longer satisfied. The Baumgarte stabilization technique
[9] is a well-known method to mitigate such issues. Therein, the right-hand

side of the second line in (B.1.7) is augmented by additional terms, i.e.
−Ȧ(q, q̇)q̇ → −Ȧ(q, q̇)q̇ + Pg(q) + DA(q)q̇ (B.1.12)
in the holonomic case and
−Ȧ(q, q̇)ṡ → −Ȧ(q, q̇)ṡ + DA(q)ṡ (B.1.13)
in the non-holonomic case. These additional terms act in a similar manner

as a PD controller and drive the position-level and velocity-level constraint
residues g(q) (only in holonomic case) and A(q)ṡ, respectively, towards
zero. Therein, the stabilization matrices P, D > 0 are used.
An overview of other integration stabilization techniques is given in [58].
B.1.5 Conditional Constraints

Models, represented by the DAE formulation (B.1.5), can be operated in
multiple stages in which the active set of constraints may vary. Conditions
under which certain constraints are considered active may depend on the
state x of the system, i.e. position q and velocity ṡ, or external quantities
such as system inputs u.
Constraint Redundancy Resolution

In practice, multiple constraint sets may become active, depending on their
respective activation conditions. In the active set of constraints, constraints
may be redundant, i.e. they are included in more the one currently active
constraint subset. Without further treatment, this would yield a constraint
Jacobian A with linearly dependent rows. This issue is resolved in step
(B.1.8) that solves for the constraint forces λ by
'
means of a singular
(
value
decomposition of the (square) matrix Ã(q) = A(q)M(q)\A (q) wherein
redundant constraints are omitted by thresholding small singular values.
Note, that conditions may also depend on the constraint forces λ, as they
are an algebraic function of the state x and the input u, cf. (B.1.8).
B.1.6 Solution Algorithm

The above solution strategy for solving the equations of motion of con-
strained mechatronic multibody systems is summarized in Algorithm 5.
Therein, a single dynamic system is considered, that is denoted in possibly
redundant position coordinates q and non-holonomic velocity coordinates
ṡ. It is subjected to conditional, possibly redundant, non-holonomic con-
straints. The solution of the EoM is sought to be obtained in simulation, i.e.
using numerical time integration methods on a time grid t0 < t1 < t2 < . . .
that is either fixed (fixed step-size integrator), or variable (variable step-size
integrator). The goal of the provided algorithm is to compute the system’s
state derivative ẋ, cf. (B.1.11), depending on the system state x(tk ) and
the input u(tk ) at time step tk . The result is then used in time integration
in order to obtain the state (B.1.10) at the following time step tk+1 .
B.2 Code Generation Utility

In order to enable efficient numerical simulation of dynamic models, the
Maple package SimCode C has been developed that facilitates automated
code generation for differential-algebraic functions for use in MathWorks
MATLAB/Simulink. Key features of SimCode C are
• support for second-order ordinary differential equations, e.g. equations

of motion of mechatronic systems
– redundant and minimal coordinate formulation
– non-holonomic velocity coordinates
– resolution of conditional, redundant algebraic constraints
– constraint stabilization using Baumgarte’s method
• support for algebraic functions depending on state, accelerations, or
constraint forces
• multiple parameter vectors
• multiple inputs/outputs
B.2 Code Generation Utility 189
Algorithm 5 Computation of state derivative ẋ (and constraint forces λ).

inputs: x, u
q, ṡ ← x
compute M(q), H(q)
L ← Cholesky decomposition of M(q)
A(x): determine currently active set of constraints
compute h(q, ṡ)
g(q) ← ( ): vector of active holonomic position-level constraints
Jg (q) ← [ ]: constraint Jacobian of active holonomic velocity-level con-
straints
J̇g (q,q̇) ← [ ]: time derivative of constraint Jacobian of active holonomic
velocity-level constraints
for all holonomic
⎛ ⎞
constraints i ∈ A(x) do
g
g ← ⎝ ⎠: collect active position-level constraints
gi ⎡ ⎤ ⎡ ⎤
Jg (q) ⎦ J̇g (q,q̇) ⎦
Jg (q) ← ⎣ , J̇g (q,q̇) ← ⎣ : collect active velocity-
Jgi (q) J̇gi (q,q̇)
level constraints
end for
Jġ (q) ← [ ]: constraint Jacobian of non-holonomic velocity-level con-
straints
J̇ġ (q, ṡ) ← [ ]: time derivative of constraint Jacobian of active non-
holonomic velocity-level constraints
for all non-holonomic
⎡ ⎤
constraints⎡i ∈ A(x)⎤do
J (q) J̇ (q, ṡ) ⎦
Jġ (q) ← ⎣ ġ ⎦, J̇ (q, ṡ) ← ⎣ ġ
ġ : collect active velocity-level
Jġi (q) J̇ġi (q, ṡ)
constraints
end for ⎡ ⎤ ⎡ ⎤
Jg (q)⎦ J̇g (q,q̇) ⎦
A(q) ← ⎣ , Ȧ(q) ← ⎣ : assemble constraint Jacobian
Jġ (q) J̇ġ (q, ṡ)
compute constraint forces λ SVD; use Cholesky decomposition L
compute acceleration s̈ ⎛cf. (B.1.9);
⎞ ⎛
use Cholesky
⎞
decomposition L
q̇ H(q)ṡ
update state derivative ẋ ← ⎝ ⎠ = ⎝ ⎠ return ẋ, λ
s̈ s̈
• multiple (independent) dynamic systems/algebraic functions with

common code generation for increased evaluation efficiency
• code generation targets
– C MEX Function for MATLAB
– C MEX S-Function for Simulink
– MATLAB S-Function for Simulink
– MATLAB MEX S-Function for Simulink
– Simulink template model
– initialization file with dummy parameters of correct dimensions
– make file for building above C functions and generating a
Simulink test model as well an initialization file
While this tool does not take the elaborate task of creating a dynamics model
off of the user, it greatly the reduces burden of efficiently implementing it
for use in numeric simulation and control.
The generated C programming language code makes use of linear algebra

functions of the free and open-source GNU Scientific Library (GSL) [49] and
the Basic Linear Algebra Subprograms (BLAS) low-level routines wrapped
therein. Therein, static memory allocation is used in order to enable use
on some real-time systems which disallow memory allocation at run-time.
SimCode C is intended to act as a full replacement of the deprecated code

generation software [76]. A release as open-source software is planned for
2019.
B.2.1 Current Limitations and Open Challenges

Conservation of Momentum
The above solution strategy does not take into account a relevant conserva-
tion quantity of mechanical systems: momentum. In general, at a switching
point between different sets of active constraints, the total generalized
momentum may not be continuous.
B.2 Code Generation Utility 191
By injecting an impulsive generalized force into the system at such a

transition point, this issue is mitigated. Note, that an impulsive force
results in a discontinuity of the system velocities ṡ.
The paper [85] presents a solution strategy the introduces conservation of

momentum into the numerical solution of multi-body systems. As a use
case, a variable topology mechanism is considered whose joints become
locked during motion. However, a problem that has not been treated in
this thesis are possible limit cycles arising from the presence of conditional
constraint and momentum conservation methods. The issue therein is that
the change of the velocities ṡ may modify the active set of constraints that
requires re-computation of the impulsive constraint forces and thus yet
another change of ṡ.
Contact Problems
Contact problems are of great relevance for the mechanical part of many
mechatronic systems, e.g. walking machines, or industrial robots interacting
with their environment. Currently, only a very simple form of contact
problems can be solved, i.e. contact that is toggled by geometric but
not dynamic constraints. Further development will be focused on contact
problems that not only allow conditions depending on accelerations but
also on constraint forces. Part of this is described in [53] wherein the
contact problem is posed as a Linear Complementarity Problem (LCP).
LCPs are most efficiently solved by special solvers, e.g. [130], or by virtue
of re-writing as a QP problem by any QP solver, e.g. [44].

BOOK2

Uploaded by

Copyright:

Available Formats

BOOK2

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

BOOK2

Uploaded by

Copyright:

Available Formats

Alexander

Optimal Path and

Dissertation, Johannes Kepler University Linz, 2019

ISBN 978-3-658-28593-7 ISBN 978-3-658-28594-4 (eBook)

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . XIII

List of Symbols and Abbreviations . . . . . . . . . . . . . XV

3 Kinematics of Serial Robots . . . . . . . . . . . . . . . . 49

3.1 Position and Orientation . . . . . . . . . . . . . . . . . . . 50

4 Path and Trajectory Planning . . . . . . . . . . . . . . 93

5 Optimal Path Tracking . . . . . . . . . . . . . . . . . . . 137

A B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . . 175

B Mechatronic Multibody Systems . . . . . . . . . . . . . . 181

2.1 A Convex Function. . . . . . . . . . . . . . . . . . . . . . . 11

3.1 Position Vector. . . . . . . . . . . . . . . . . . . . . . . . . . 50

4.1 Furuta Pendulum. . . . . . . . . . . . . . . . . . . . . . . 95

4.8 SCARA4 Minimum-Energy Trajctory Planning. . . . . . . . 110

5.1 SCARA4 Minimum-Time Path Tracking. . . . . . . . . . . . 143

B.1 Schematics of Autonomous Walking Robot (AWARO). . . . 182

4.1 SCARA4 Minimum-Time Trajctory Planning: Results. . . . 108

5.1 SCARA4 Minimum-Time Path Tracking: Results. . . . . . . 147

As the theoretical foundation relevant to this thesis, basics of numerical

Optimal control problems require a certain level of complexity of the

are therefore not known beforehand. A sudden parameter change in the

In dieser Arbeit werden – soweit für das Verständnis relevant – zunächst

Optimalsteuerungprobleme benötigen einen gewissen Detailgrad der dar-

sich ändernde Parameter beeinﬂusst werden, womit die Aufgabe zu einem

The present thesis provides contributions to sub-ﬁelds of kinematics and

An important portion of the present thesis is devoted to kinematic redun-

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020

inverse kinematics methods for such kinematic structures are examined.

Chapter 3 treats kinematic properties of serial robots, i.e. the problem

Chapter 5 discusses optimal path tracking. In contrast to path and tra-

In Appendix A, an alternative parametrization of B-spline curves in terms

Example 1.1: SCARA4 (description)

Example 1.2: Stäubli TX90L (description)

Figure 1.3: Stäubli TX90L Industrial Manipulator On Linear Axis (Contour

Example 1.3: Comau Racer R3 (description)

Figure 1.4: Comau Racer R3 Industrial Manipulator (Contour Rendering).

In this thesis a number of numerical examples are discussed including their

Aim & Contributions

• Fast solution approximation using parametric sensitivities. In certain

• Optimal inverse kinematics and path tracking for kinematically re-

• B-spline software package. Another contribution is a library to evalu-

are derived symbolically. The contribution consists of a code genera-

First, a brief overview of optimization theory regarding gradient-based

In the following sections, B-spline curves are discussed. These constitute a

Both topics, numerical optimization and B-spline curves, will then be

© Springer Fachmedien Wiesbaden GmbH, part of Springer Nature 2020

details for understanding the contribution of the presented work in the

2.1 Numerical Optimization

Deﬁnition 2.2: Convex Function

Deﬁnition 2.3: Concave Function

Example 2.1: Convex Function

f ((1 − k)x1 + kx2 ) ≤ (1 − k)f (x1 ) + kf (x2 )

with x1 , x2 ∈ S holds ∀k ∈ [0, 1], f is a convex function.

Figure 2.1: A Convex Function.

2.1.2 Constrained Optimization

which minimizes the objective function J(w) w.r.t. w while satisfying

Ω = {w : gi (w) = 0, i = 1, . . . , Neq ; gi (w) ≤ 0, i = Neq + 1, . . . , Neq + Nineq }.

Deﬁnition 2.5: Convex Programming Problem