ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.

3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 1

A Dynamic Programming Framework for Optimal

Planning of Redundant Robots Along Prescribed
Paths With Kineto-Dynamic Constraints
Enrico Ferrentino, Member, IEEE, Heitor J. Savino, Member, IEEE, Antonio Franchi, Fellow, IEEE,
and Pasquale Chiacchio, Senior Member, IEEE
arXiv:2207.05622v2 [cs.RO] 12 Dec 2023

Abstract—Offline optimal planning of trajectories for re- we go through the whole process of planning and executing
dundant robots along prescribed task space paths is usually a time-optimal trajectory on a real robot, and discuss some
broken down into two consecutive processes: first, the task practical details, such as trajectory smoothness and actuator
space path is inverted to obtain a joint space path, then, the saturation, aiding the practitioners in deploying our algorithm
latter is parametrized with a time law. If the two processes effectively. Currently, the algorithm’s applicability is limited
are separated, they cannot optimize the same objective func- to those cases where hours are available for planning, hence
tion, ultimately providing sub-optimal results. In this paper, it is not well-suited for those cases where the robot activity
a unified approach is presented where dynamic programming has to change frequently. By replacing the underlying dynamic
is the underlying optimization technique. Its flexibility allows programming engine with a different methodology, such as
accommodating arbitrary constraints and objective functions, randomized algorithms, the planning time could be controlled
thus providing a generic framework for optimal planning of real to be upper-bounded, thus returning the most efficient solution
systems. To demonstrate its applicability to a real world scenario, that can be achieved in the time available for reconfiguring the
the framework is instantiated for time-optimality on Franka production. Other applications of interest include optimal ground
Emika’s Panda robot. The well-known issues associated with control of space robotic assets and performance benchmarking
the execution of non-smooth trajectories on a real controller are of online planning algorithms.
partially addressed at planning level, through the enforcement of
constraints, and partially through post-processing of the optimal Index Terms—Robot programming, manipulator motion-
solution. The experiments show that the proposed framework planning, time optimal control, optimization methods.
is able to effectively exploit kinematic redundancy to optimize
the performance index defined at planning level and generate
feasible trajectories that can be executed on real hardware with
I. I NTRODUCTION
satisfactory results. A. Scope and Objective
Note to Practitioners—The common planning algorithms which
consolidated over the years for generating trajectories for non-
redundant robots are not adequate to fully exploit the more
K INEMATICALLY redundant manipulators possess more
degrees of freedom than those strictly required to ex-
ecute a given task. Such a characteristic gives the system a
advanced capabilities offered by redundant robots. This is
especially true in performance-demanding tasks, as for robots higher degree of dexterity and mobility that can be exploited
employed on assembly lines in manufacturing industries, re- to optimize performance indices of interest, besides fulfilling
peatedly performing the same activity. Once the assembly line the main task. Redundancy can be structural, when the robot
engineer has defined the tool path in the task space, our planning is designed to be redundant for generic tasks, or functional,
algorithm unifies inverse kinematics and time parametrization when the robot is operated so as to be redundant. The concept
so as to bring the manipulator at its physical limits to achieve
specific efficiency goals, being execution time the most typical applies to anthropomorphic serial chains, that are by far the
one. The algorithm is configurable in terms of constraints to most widespread robotic systems in manufacturing industries,
consider and objective functions to optimize, therefore it can be but it is general enough to be extended to more complex
easily adapted to optimize other custom-defined efficiency indices, systems, including humanoids, mobile manipulators (terres-
to better respond to the needs of the automation plant. Being trial, aerial), parallel robots and systems of cooperating robots.
based on discrete dynamic programming, the global optimum
is guaranteed for a given resolution of the problem. This can The optimization problem that looks into the exploitation
be configured by the operator to achieve the desired trade- of redundancy to optimize a given performance index is
off between efficiency and planning time. In our experiments, commonly referred to as redundancy resolution.
Many robotic tasks require to follow a specific path, such
E. Ferrentino and P. Chiacchio are with the Department of as in welding, cutting, gluing and in some assembly and
Computer and Electrical Engineering and Applied Mathematics
(DIEM), University of Salerno, Fisciano, SA, 84084, Italy, e-mail: disassembly tasks. Planning a collision-free task space path is
{eferrentino,pchiacchio}@unisa.it. also a convenient solution when the environment is cluttered.
H. J. Savino is with Ambev Robotics Lab, Anheuser-Busch InBev, Jacareı́, This is opposed to point-to-point (PTP) motion, where the
SP, 12334-480, Brazil, e-mail: [email protected].
A. Franchi is with the Robotics and Mechatronics Laboratory, Faculty robot links are more loosely constrained during motion. The
of Electrical Engineering, Mathematics & Computer Science, University definition of the speed by which the path is tracked is also
of Twente, Enschede 7522NH, The Netherlands, and also with the De- a decision variable that, in most applications, is necessary
partment of Computer, Control and Management Engineering, Sapienza
University of Rome, 00185 Rome, Italy; e-mails: [email protected] and to optimize. The typical objective is time minimization, but
[email protected] any performance index involving derivatives can be designed
0000–0000/00$00.00 © 2023 IEEE
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 2

for a specific purpose. The optimization problem that looks solution for both optimization problems, as in Fig. 2, so
into the definition of velocity along a given path is commonly that kinematic redundancy is effectively exploited for the
referred to as optimal trajectory planning or optimal time- minimization/maximization of the objective function defined
parametrization. at trajectory planning level.
Both the optimization problems above can be addressed
(and solved) at planning level, sometimes offline, before the
robot moves, or at control level, while the task is being x(λ) q(t)
Redundancy resolution
executed and the system has to react to unforeseen events. & trajectory planning
Although, nowadays, most applications require the robots
to live and operate in unstructured, unknown and highly-
dynamical environments, there still are many situations in Fig. 2. Redundancy resolution and trajectory planning as one unique process;
which tasks are better planned offline. One of the most x(λ) is the task space path and q(t) the joint space trajectory.
frequent employments of anthropomorphic arms in manufac-
turing industries concerns the execution of repetitive tasks in
structured environments, where the task is planned once and
executed cyclically. In aerospace, offline planning instances B. Related works
exist in the mission design phase for feasibility and budget Optimal trajectory planning along prescribed paths deals
assessments. Also, some sequences for in-orbit manipulation with the optimization of an integral cost function, subject to
can be pre-planned in an optimal way. In missions that are the kinematic constraint of the end-effector’s path and dynamic
characterized by windowed communications, a long planning capabilities of the actuators (mainly torque and torque rate
time is usually available on ground to deliver more efficient limits). The foundations of the problem were laid for time
commands to the spaceborne asset. On the contrary, when minimization and non-redundant robots [4]–[7], where the
the domain requires highly-reactive behaviors, offline planning unique joint space path is parametrized with respect to the
can provide benchmark solutions to evaluate the performance curvilinear coordinate of the path λ. The robot kinematic
of online control algorithms. The scenarios just described are and dynamic constraints are encoded on limits on its first-
the domain of this paper. and second-order time-derivatives λ̇ and λ̈, called pseudo-
Whether they are executed on-line or offline, redundancy velocity and pseudo-acceleration, respectively. In this new
resolution and trajectory planning have been mostly treated domain, λ and λ̇ represent the state variables and λ̈ the input
as two independent, consecutive optimization processes [1]– of a parametrized dynamic system. The optimal solution is
[3], as depicted in Fig. 1. A task space path is given, that bang-bang in λ̈ [8]. If, for a redundant manipulator, the joint
is mapped, through inverse kinematics, in the joint space space path is already assigned, the theory of non-redundant
(possibly optimizing a geometric quality index), then, the manipulators holds without modifications.
joint space path is time-parametrized to yield a trajectory The first extension to kinematically redundant systems, that
(respecting constraints and possibly optimizing a different includes the more generic case of a path assigned in the task
quality index), that can be executed through motion control space, was developed in [9], [10]. Therein, a non-minimal ex-
at joint level. The optimization performed at the former stage tended state is considered, made of λ, λ̇, the joint positions and
can be functional to the optimization performed at the latter. their derivatives. The input corresponds to actuation torques.
For instance, if the objective of trajectory planning is to define The time-optimal problem is solved numerically by making
a minimum-time motion, redundancy resolution may optimize, use of the Extended Pontryagin’s Maximum Principle (EPMP)
according to a heuristic approach, the acceleration/deceleration and considering an approximation of the torque constraints.
capabilities of the manipulator along the path. Overall, the two This formulation is inflexible with respect to the introduction
processes “cooperate” to achieve a common objective. of additional constraints and objective functions.
Unfortunately, the resolution of two independent optimal By adopting a suitable parametrization of redundancy [11],
problems does not guarantee the achievement of the overall e.g. through joint space decomposition (JSD) [12], a minimal
global optimum. Indeed, if the objective is to optimize a state of the parametrized dynamic system can be found,
generic dynamic index, e.g., traveling time, but the time law composed of λ, λ̇, the redundancy parameter, and its first-
is only defined at the level of trajectory planning, there is order derivative. This time, the equations can be arranged to
no possibility to formulate a redundancy resolution problem let the input be the vector made of λ̈ and the acceleration of
which explicitly considers it. the redundancy parameter, but they cannot be constrained ex-
plicitly. As a consequence, a linear programming (LP) problem
has to be defined for each possible state in order to determine
x(λ) q(λ) q(t)
Redundancy Trajectory the optimal input, that is extremized according to the bang-
resolution (IK) planning bang paradigm. In [13], in order to provide an initial state, the
initial joint positions are fixed and the resolution is achieved
through a try-and-error approach. In addition, in order to
Fig. 1. Redundancy resolution and trajectory planning as two independent make the algorithm more efficient, the number of switching
processes; x(λ) is the task space path, q(λ) the joint space path and q(t)
the joint space trajectory. points is possibly reduced with respect to the real time-optimal
trajectory. Objective functions other than tracking time cannot
Hence, the objective of this paper is to provide a unified be considered, as the bang-bang hypothesis is made. Lastly,
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 3

the algorithm cannot enforce zero joint velocity at the end of described above, it naturally arises as the unifying approach
the motion, which is undesirable in most applications. (see, for instance, [32]–[36] for redundancy resolution, and
A different approach consists in addressing the optimization [37]–[40] for trajectory planning). DDP is extremely flexible
problem directly [14]–[17], without any prior manipulation of in the accommodation of arbitrary constraints and objective
the quantities in play. The time axis is divided into a fixed functions and, compatibly with the available planning time,
number of intervals of the same variable length, whose sum can achieve the global optimum. When applied to real systems,
is tf , that is the result of the optimization process. When tf its formulation can well describe the constraints at hand and
is given, the duration of each time interval in the time domain no conservative hypotheses are necessary. The computational
is also determined. The problem is to find, for each of the complexity of DDP is square with the cardinality of the
intervals, the value of λ, the joint position vector, and their discrete sets defining the feasible search space [36]. However,
derivatives. The path constraint is imposed through second- the more constraints are defined in the problem formulation
order kinematics, either with JSD or null-space projection. (which is the typical case of real systems), the more states are
For both techniques, since the path constraint is enforced excluded from the search. Therefore, differently from several
through numerical integration of the second-order solution, other numerical optimization methods, e.g., those based on
an additional positional kinematics constraint needs to be the Newton’s method, where more constraints make the search
included to prevent drift. The problem can be solved with of the optimum cumbersome, in DDP they are beneficial to
direct multiple shooting using the interior-point method, after reducing the computation time.
an initial guess on the optimizing variables is made. Since DDP is based on the discretization of the state space which,
the problem is non-convex, globally-optimal solutions cannot in principle, complicates the generation of smooth references
be guaranteed and, in general, the quality of the resulting for motion control. However, the possibility to include higher-
solution strictly depends on the initial guess. On the other order constraints mitigates this issue.
hand, the formulation is flexible enough to include constraints Our contribution is a unified generic framework for redun-
on the joint positions and their derivatives, as well as other dancy resolution and trajectory planning based on DDP. With
application-specific constraints. For instance, in [17], the path respect to previous works proposing a unified solution, i.e.
constraint is further derived to account for the limits on joint [12], [14]–[17], our methodology does not need the user to
positions, velocities, accelerations, jerks and snaps (i.e. the provide a seed state, i.e., an initial state from which the search
fourth-order derivative of the position) through which contin- starts, and which closely influences the final solution, but can
uous and quasi-differentiable motor torques can be obtained. retrieve the globally-optimal solution of the discretized state
By acting directly on λ̈, this is also the objective in [18]. This space. Possibly, this solution can be used as a valuable seed
result is of utmost importance since the insufficient continuity state for a different optimization problem to further improve
of globally-optimal minimum time trajectories is the main other features of the solution, such as its smoothness. We
shortcoming when it comes to the control of real robots. further elaborate on this aspect in Section IV-E.
For the sake of completeness, we note that a multitude
of different optimization approaches exist in more recent D. Overview
literature on trajectory planning. They include dynamic op- In Section II, the problem is presented and formalized.
timization in the form of convex quadratic [19], [20] or First, parameters and equations that describe the problem are
cone [21] programming, evolutionary algorithms [22]–[25], introduced in Section II-A and discretized so as to identify
dynamic programming [26], heuristic algorithms [27], and a suitable state that provides a minimal description. Then, in
machine learning [28], with several approaches to describe Section II-B, the constraints are introduced as maps that enable
the solution, in the form of, e.g., polynomials [27], [29] and transitions towards certain states, in a generic framework. Last,
B-splines [22]–[25] to control the joint space smoothness or in Section II-C, the optimization problem is introduced in a
achieve multi-objective optimization [20], [22]–[25], [30]. The generic way and specialized for time-optimal planning. The
mentioned techniques, however, are only applied to solve sub- algorithmic formulation is provided in Section III, together
problems of the unified optimal redundancy resolution and with some important considerations of practical interest. The
time parametrization problem addressed here. They consider, results of the experiments are presented in Section IV. First,
for instance, PTP instead of path-constrained tasks [23], [25], the experimental setup is described in Section IV-A, then,
or only the first [19], [31] or second [21], [29], [30] stage in results of offline planning and execution on a real robot
Fig. 1. are given in Section IV-B and Section IV-F respectively. In
Sections IV-C and IV-D, our approach is compared with
C. Methodology and contribution two algorithms from [1] and [14]–[17], using the decoupled
In all the offline planning applications that motivate this approach of Fig. 1 and another unified approach as in Fig. 2,
work, although the environment where the robot operates is respectively. The work is concluded in Section V, where some
structured, static, known or partially known, not all the formu- lines of development are also outlined for future research.
lations of the optimization problem can capture the complexity
II. O PTIMAL PLANNING OF REDUNDANT ROBOTS WITH
of the real system and some show greater flexibility than oth-
DYNAMIC PROGRAMMING
ers. This paper adopts discrete dynamic programming (DDP)
as the main underlying methodology and central idea to cope A. State definition
with the complexity of reality. Also, because of its employ- Let us consider a geometrical path for the end-effector
ment, in previous works, for both the optimization problems x(λ) : [0, L] → Rm , such that a given manipulator with n
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 4

degrees of freedom is redundant, i.e. n > m. It follows that redundancy parameters are selected as a subset of the joint
r = n−m, the redundancy degree, is strictly greater than zero. positions or they are a more generic function of them. Under
λ is the arc length, L is the path length. The joint positions this hypothesis, each of the joint positions in Sq (i) in (4) can
along the path q(λ) ∈ Rn are unknown, as well as the time law be computed from non-redundant inverse kinematics (IK) as
λ(t) : R → [0, L] associated with them. The time derivative
q(i) = k−1 x(i), v(i), g(i) ,


of the arc length λ̇ = dλ (8)
dt encodes the time information, and,
when expressed as λ̇(λ), for variable λ, it is often referred to where v ∈ Rr is the vector of redundancy parameters. Since
as phase plane trajectory (PPT) [4], [5]. Intuitively, the PPT more than one configuration may satisfy the generic non-
corresponds to the norm of the tangential velocity with respect redundant IK, g is a selector of the specific IK solution to be
to the arc length. considered at stage i. For example, if the square inverse kine-
Let us discretize λ in space with a given step ∆λ and Ni ∈ matics function k−1 (x, v) admits 4 solutions, g ∈ {0, 1, 2, 3}.
N, i.e., This way defined, q(i) is unique. Since x is a function of λ,
L we may rewrite the equation above as
λ(i) = i∆λ , with i = 0, 1, 2, . . . , Ni and ∆λ = (1)
Ni
q(i) = k−1 λ(i), v(i), g(i) .



(9)
such that x λ(i) = x(i) can be viewed as the (i + 1)-th
waypoint. The variable i is termed stage index. Now, we may fold (6) and (9), for each i, into (4) and (5),
The robot dynamic model is given by yielding


q̇(i) ∼ fq̇ Sλ (i), Sλ̇ (i), Sv (i), Sg (i) , (10)



τ (i) = H q(i) q̈(i) + f q(i), q̇(i) , (2)

q̈(i) ∼ fq̈ Sλ (i), Sλ̇ (i), Sv (i), Sg (i) . (11)
where, by omitting the dependence on i for simplicity, H(q)
is the n × n inertia matrix and By substituting both equations above into (2), we find
T that, in general, the input is a function of the four identified
f (q, q̇) = q̇ C(q)q̇ + d(q̇) + g(q). (3)
sequences, i.e.
C(q) is the n × n × n matrix of Christoffel symbols, d(q̇) is

the n × 1 vector of Coulomb and viscous friction terms, and τ (i) ∼ fτ Sλ (i), Sλ̇ (i), Sv (i), Sg (i) . (12)
g(q) is the n × 1 vector of gravitational torques. This means that, given the current state at stage i, selecting
Let S(·) (i) be the discrete-time function (or sequence of the input τ (i) that drives the discrete-time system to another
samples) of the continuous-time function (·)(λ) up until the state is equivalent to selecting the parameters λ̇, v and g at
i-th sample. For instance, Sλ̇ (i) = {λ̇(0), . . . , λ̇(i)} is the the next stage. This is coherent with the logic of discretizing
discrete-time phase plane trajectory up until stage i. the state space instead of the input space that is common to
The joint velocities and accelerations at a given stage i, all the previous works using DDP for redundancy resolution
q̇(i) and q̈(i), respectively, in a discrete-time formulation, can and time-optimal planning. Notoriously, this is also a way to
be approximated, through fixed-step integration schemes, as control the number of involved variables due to the addition of
functions of the sequences Sq (i), Sq̇ (i) and St (i), i.e., extra dimensions, i.e. the curse of dimensionality [41], that is


q̇(i) ∼ fq̇ Sq (i), St (i) , (4) a well-known issue in dynamic programming. The parameters

λ̇, v and g also constitute a minimum representation of the
q̈(i) ∼ fq̈ Sq̇ (i), St (i) . (5) state for each stage, as opposed to [9] and [14] that, rather, use
The exact form of the functions above depends on the specific redundant representations. In a formulation based on calculus
integration scheme. We note here, and further clarify in Section of variations, this result is related to the minimum number of
III, that derivatives are assumed to be computed with past differential equations required to fully represent the system at
samples, in a backward integration scheme, because future hand [11].
samples, in a forward optimization scheme, are unknown, as Thus, let us proceed by discretizing the variables λ̇ and v.
they depend on optimal choices that yet have to be made. The former is
In the same way, each timestamp t(i) in St (i) can be derived λ̇M
from the phase plane trajectory up until stage i, through some λ̇l = l∆λ̇ , with l = 0, 1, 2, . . . , Nl and ∆λ̇ = , (13)
Nl
discrete approximation, e.g. the trapezoidal approximation
used in [38], that generically is where λ̇M is the maximum pseudo-velocity value that the

phase plane trajectory can reach. The latter is
t(i) ∼ ft Sλ (i), Sλ̇ (i) . (6)
v = j ◦ ∆v + vmin , (14)
Let us remark, once again, that Sλ̇ (i) is unknown and, as a
consequence, t(i) is unknown, too. where ‘◦’ denotes the element-wise Hadamard product, ∆v =
Let us define the forward kinematics function k(q) : Rn → [∆v,1 , . . . , ∆v,r ] is the vector of the sampling intervals, j ∈ Nr
m
R . The path constraint at the given waypoints is given by the vector of the redundancy parameter indices, and vmin


x(i) = k q(i) . (7) is the vector of lower bounds of the redundancy parame-
ters domains. The elements of j take the maximum values
In order to parametrize the redundancy, we may adopt a joint Nj,1 , . . . , Nj,r so that [Nj,1 , . . . , Nj,r ] ◦ ∆v + vmin equals the
selection (or JSD) or a joint combination scheme [11], i.e. the upper bound of the redundant joint domains.
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 5

The discretized domains of λ, λ̇, v and g (which is naturally Joint velocity, acceleration and jerk limits can be directly
discrete) makes up a grid of r + 3 dimensions. We essen- encoded in equivalent stage-dependent, as well as state-
tially adopt the same approach as other DDP formulations dependent sets Bi1 Bi2 and Bi3 , so that:
for redundancy resolution and time-optimal planning, but we
q̇(i) ∈ Bi1 s(i) ,


(18)
increase the number of dimensions to take into account: (1) 2


the resolution of both problems together; (2) the presence of q̈(i) ∈ Bi s(i) , (19)
...
q (i) ∈ Bi3 s(i) .


multiple inverse kinematics solutions. (20)
Let us recall that, for non-redundant manipulators, the
Equivalently, joint torque and torque rate limits can be encoded
curvilinear coordinate parametrization of dynamics is obtained
in similar sets Ci1 and Ci2 :
through the following equations:
τ (i) ∈ Ci1 s(i) ,


(21)
q̇ =J−1 x′ λ̇, 2


τ̇ (i) ∈ Ci s(i) . (22)
h i (15)
q̈ =J−1 x′ λ̈ + x′′ λ̇2 − (J−1 x′ )T B(J−1 x′ )λ̇2 , More commonly, all the quantities above are given within
fixed domains that do not change along the trajectory, are not
′
where J = ∂k dx
∂q is the manipulator’s Jacobian matrix, x = dλ , configuration dependent, and not velocity-dependent, but the
2
x′′ = ddλx2 , and B is the Hessian of the vector function k [7]. generality of the framework allows for the accommodation
Equations (15) are substituted in (2) to yield the parametrized of more complex constraints. For example, interacting torque
dynamics. In the redundant case considered here, J is no constraints expressed as
longer invertible, compromising the possibility of a unique τ = [τ1 , τ2 , . . . , τn ]T ∈ E(q, q̇), (23)
parametrization. As a consequence, the pseudo-acceleration
limits cannot be pre-computed: a state space grid may still that cannot be parametrized [37], can be easily handled with
be computed on the basis of the discrete values of the state, DDP.
but the content of each single node should be reconsidered All the sets above can be combined together to define the
in light of this observation. Each node in the grid will then set Di of reachable states for a generic stage i, that is
contain a state given by
(
...
Di = Ai ∩ s(i) : q̇(i) ∈ Bi1 , q̈(i) ∈ Bi2 , q (i) ∈ Bi3 ,
sljg (i) = [λ̇l (i), qjg (i)], (16)
τ (i) ∈ Ci1 , τ̇ (i) ∈ Ci2 (24)
where qjg is the vector of joint positions obtained from (9) )
using the redundancy parameters vj and selecting the g-th IK
with s(i − 1) ∈ Ai−1 , . . . , s(0) ∈ A0 .
solution.
In what follows, for the sake of simplicity, whenever we
do not need to refer to the specific position that a given state C. Discrete dynamic programming problem
occupies in the grid, we drop the state’s subscripts and refer Since all the states are available in the grid, the DDP prob-
to the generic state at stage i as s(i). lem is transformed into a graph search problem. This allows
to adopt either a forward or a backward optimization scheme
[36]. The objective function to optimize can be generally
B. Constraints defined as
In offline robotic planning, common constraints are limits on
X Ni


joint positions and their derivatives, actuation constraints, ob- I(Ni ) = ψ s(0) + ϕ s(k − 1), s(k) , (25)
stacles in the workspace, velocity and acceleration limits in the k=1
Cartesian space, initial/final joint configurations, cyclicity (for


where ψ s(0) is a cost associated to the initial state and ϕ
closed task space paths), power limits, maximum/minimum is the cost computed locally between two adjacent states.
forces exchanged with the environment in an interaction If, for instance, the objective function to minimize is
the
scenario. Many other constraints can be designed depending time, there is no cost associated to the initial state, i.e. ψ s0 =
on the specific application at hand. In the experiments that 0 and I = t. Assuming a backward Euler approximation, (25)
will follow in Section IV, we consider minimum/maximum becomes
joint positions, maximum joint velocities, accelerations, jerks, Ni
X λ(k) − λ(k − 1)
torques and torque rates [42]. t(Ni ) = (26)
k=1
λ̇(k)
Since the robot dynamics cannot be parametrized with
respect to λ and its derivatives, the same holds for the A different approximation can be adopted at the beginning and
constraints. Therefore, for redundant robots, they are directly at the end of the trajectory to cope with the case λ̇ = 0 [38].
verified on the joint variables. Hence, let us define the stage- To complete our formulation, we rewrite (26) in a recursive
dependent set Ai , containing all nodes returning joint positions form and use the set Di in (24), by minimizing over the
that respect the joint domains, the path constraint and, possi- admissible states:
bly, imposed initial/final configurations. Such constraints are topt (0) = 0,
formalized as 
λ(i) − λ(i − 1)

(27)
s(i) ∈ Ai . (17) topt (i) = min topt (i − 1) + ,
s(i)∈Di λ̇(i)
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 6

where topt (i) at a generic stage i is the optimal return function be used for limiting the manipulator’s motion to specific sub-
(or value function) and topt (Ni ) represents the optimal cost. spaces of the configuration space.
At step 2, the nodes in the grid are enabled/disabled
according to geometrical constraints including, for example,
III. A LGORITHMIC IMPLEMENTATION
configurations that bring the robot to collide with the sur-
Based on the problem formulation above, we can define rounding environment. Also, if IK solutions are homogeneous,
the time-optimal trajectory planning algorithm for redundant entire grids can be excluded to make the DDP algorithm
robots (TOTP-R) with DDP as in Algorithm 1. find a trajectory in specific extended aspects. The sets Ai
can also be used to impose that the robot must start and
Algorithm 1 DDP TOTP-R algorithm. finish its motion at rest, or even stop along the path if this
1: Initialize state space grid through inverse kinematics and dis- is required by the specific task. In time-optimal planning of
cretization of λ̇, according to (9) and (16) non-redundant robots, the same sets Ai can be used to impose
2: Initialize Ai , ∀i = 0..Ni the maximum velocity curve (MVC) constraint [4], [5]. In this
3: Initialize Bi1 , Bi2 , Bi3 , Ci1 , Ci2 ∀i = 0..(Ni − 1) with state- case, since we do not perform any parametrization of dynamics
independent information
4: Initialize Di = ∅, ∀i = 0..Ni and constraints, this is not possible, and the MVC constraint
5: Initialize cost map ti,l,j,g = +∞ ∀i, l, j, g is implicitly checked at the time of verifying (18)-(22) at step
6: t0,l,j,g ← 0 ∀l, j, g 12.
7: D0 ← A0 In most practical cases, constraints on joint velocities, ac-
8: for i ← 0 to Ni − 1 do celerations, jerks, torques, and torque rates are given in terms
9: for each sljg ∈ Di do
10: for each smkh ∈ A... i+1 do
of state-independent connected sets, e.g. q̇ ∈ [q̇min , q̇max ].
11: Compute q̇, q̈, q , τ , τ̇ In this case, the sets in (18)-(22) are completely defined
12: if Constraints in (18) - (22) are satisfied then beforehand, as done at step 2. Conversely, if they were
13: Di+1 ← Di+1 ∪ {smkh } state-dependent, they could be re-computed at the time the
14: Compute instantaneous cost function ϕ constraints are checked, where the current velocity and accel-
15: if ti,l,j,g + ϕ < ti+1,m,k,h then
16: ti+1,m,k,h ← ti,l,j,g + ϕ eration of the system are known.
17: Let sljg at stage i be the predecessor of smkh At a given stage i, for each pair of nodes (steps 9 and 10)
at stage i + 1 the discrete-time functions (or sequences) of parameters Sλ (i),
18: topt (Ni ) ← minj,g [tNi ,0,j,g ] Sλ̇ (i), Sv (i), Sg (i) are available through back-pointers to
19: Build functions λ̇(i) and q(i) of optimal pseudo-velocities and compute joint velocities, accelerations and torques as in (10)-
joint positions by screening the predecessors map backward (12) respectively, as well as joint jerks and torque rates with
equivalent discrete approximations. If constraints are satisfied,
The algorithm assumes that a task space path is given we say that the node smkh (i + 1) can be reached by the node
as a discrete set of points and that they are associated to sljg (i). The former is then added to the set of nodes Di+1 that
discrete values of the curvilinear coordinate, from λ = 0 can be visited at the next stage (step 13). Among all the nodes
up to the length of the path λ = L. If this is not the case, at stage i that can reach smkh (i + 1), at step 17, we save the
but some analytic curve is given, it can be sampled at the pointer to the one that provides the lowest cumulative cost (or
desired rate. Although (1) is a uniform sampling, this is not a optimal return function), according to (27).
necessary condition for TOTP-R. At step 1, the redundancy After the process above has been repeated for all the task
parameters vector is discretized according to (14), which space points, the one with minimum cumulative traversing
allows to compute IK solutions as in (9). The pseudo-velocity time is picked (step 18) and, from it, the information about the
is also discretized according to (13), so that the grid nodes time law, i.e. λ̇(i), and exploitation of the null-space, i.e. q̇(i),
can be defined as in (16). are retrieved by following the map of predecessors backwards
In agreement with [36], [43], let us define the extended (step 19). The timestamps can be computed through (6) and
aspect as each connected set of joint configurations q, such applied to the joint space path to obtain the optimal joint space
that the determinant of the augmented (with the redundancy trajectory. Likewise, optimal joint velocities, accelerations, and
parameters) kinematics’ Jacobian |Ja (q)| never nullifies. On torques can be computed from (10)-(12).
the basis of this definition, every combination of signs of A few considerations are worthwhile. The enforcement of
the factors of |Ja (q)| identifies a different extended aspect higher-order derivative constraints, like (20) and (22), would
[44], achieving a partition of the configuration space. If these in principle require the introduction of an additional axis
combinations are known to the IK solver in (9), IK solutions corresponding to the pseudo-acceleration λ̈ [37], [45]. The
belonging to different extended aspects can be mapped to employment of jerk and/or torque rate constraints becomes
different grids. In this case, g is also an extended aspect particularly important when the trajectories have to be exe-
selector and, if desired, can be fixed in (9) to avoid traversing cuted on the real hardware, however the introduction of an
singularities pertaining to the augmented Jacobian. However, additional dimension might make the problem intractable from
in general, finding a proper factorization of |Ja (q)| is not the practical standpoint. This is the issue addressed in [39],
trivial, in which case the aforementioned mapping cannot be where higher-order constraints are also enforced in a planning
established and the state space grids obtained through (9) are scenario based on dynamic programming, without redundancy.
non-homogeneous [36], i.e. each grid possibly mixes solutions Therein, continuous and differentiable profiles are generated in
from different extended aspects. As a consequence, g cannot the phase plane through interpolation between two consecutive
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 7

TABLE I TABLE II
PANDA LIMITS FROM JOINT 1 ( TOP ) TO JOINT 7 ( BOTTOM ) C UBIC SPLINE CONTROL POINTS FOR THE ELLIPSE - LIKE PATH
... Coordinate
qmin qmax q̇max q̈max q max τmax τ̇max
[rad] [rad] [rad/s] [rad/s2 ] [rad/s3 ] [Nm] [Nm/s] in meters [m] CP1 CP2 CP3 CP4 CP5
−2.8973 2.8973 2.1750 15 7500 87 1000 x 0.5 0.5 0.5 0.5 0.5
−1.7628 1.7628 2.1750 7.5 3750 87 1000 y 0.0 −0.3 0.0 0.3 0.0
−2.8973 2.8973 2.1750 10 5000 87 1000 z 0.8 0.6 0.5 0.6 0.8
−3.0718 −0.0698 2.1750 12.5 6250 87 1000
−2.8973 2.8973 2.6100 15 7500 12 1000
−0.0175 3.7525 2.6100 20 10000 12 1000
−2.8973 2.8973 2.6100 20 10000 12 1000
and, on the opposite, the computational complexity of
the DDP algorithm;
• redundancy parameter resolution, determining, on one

stages. The constraints of interest, including jerk and torque side, the capability of the redundant manipulator to
rate, are verified at selected check points along such profiles. exploit its null-space to optimize the performance index
This way, the search space does not need to be augmented at hand and, on the opposite, such as before, the compu-
and the complexity can be controlled without exiting the phase tational complexity of the algorithm.
plane. In this paper, we essentially adopt the same solution, but The redundancy parameter is selected as the joint position
the type of interpolation is left as a user choice as an additional q4 (middle of the chain), because it simplifies the analytical
mean to control the complexity. In both cases, when higher- inverse kinematics in (8) [48]. As far as the discrete approx-
order constraints are checked without augmenting the search imations for derivatives are concerned, in our experiments,
space, the global optimality might be compromised [39]. that are based on a single-threaded implementation, we select
Furthermore, with respect to pure redundancy resolution a simple backward Euler approximation so as to speed up
at kinematic level and pure time-optimal planning for non- the computation and consume less memory. However, with
redundant robots, the TOTP-R search space is of an increased a high-performance implementation, more complex discrete
dimension. Practically, this rules out the usage of an arbitrarily approximations could be used and more accurate results should
fine discretization and more trade-offs should be considered. be expected.
Therefore, our solutions might be, in some cases, far from the
true global optimum, and we should rely on a more relaxed B. Planning
condition of resolution optimality. A proof of convergence of
Two paths are designed in the task space. One is a straight
approximate dynamic programming, for the more generic case
line path having x = 0.5 m, z = 0.4 m and y spanning for 0.5
of multi-objective optimization, is provided in [46].
m from ys = 0.25 m to ye = −0.25 m. The other is an ellipse-
like line with L = 1.45, obtained through cubic splining of the
IV. E XPERIMENTAL RESULTS control points (CP) in Table II. The end-effector orientation
A. Experimental setup can be easily understood from the accompanying video3 .
Both tasks constrain six dimensions, leaving one degree of
The TOTP-R algorithm is validated on the Panda 7-DOF freedom for redundancy resolution. In addition, the robot has
manipulator by Franka Emika1 , whose joint limits are reported to track the path as fast as possible and is free to select the IK
in Table I2 . solutions allowing for the shortest tracking time: no extended
We rely on the dynamic model identified in [47] and related aspect is excluded beforehand and the robot can possibly visit
libraries. They allow access to the inertia matrix H(q), the more than one of them. Initial and final joint positions are
Coriolis vector q̇T C(q)q̇, the friction vector d(q̇), and the not constrained and, therefore, they are part of the solution.
gravity vector g(q) in (2) and (3). The friction model includes The parameters of the algorithm are selected as a result of a
Coulomb terms and adopts a sigmoidal formulation to avoid trade-off between computation time and accuracy.
discontinuities for low joint velocities. The TOTP-R algorithm is executed on the straight line path
The execution of the TOTP-R algorithm requires a set of and the results for different sets of parameters are reported in
parameters to be defined, that are crucial for the performance Table III. PV stands for pseudo-velocity, RP for redundancy
of the algorithm, both in terms of quality of the joint space parameter. A necessary condition for global optimality is that
solution and CPU planning time. Indeed, this is common to at least one actuator saturates for each waypoint. In our DDP
other techniques (see, for example, [3]). They are: algorithm, saturation is never exact because of discretization,
• number of waypoints in the assigned task space path; thus an actuator is considered in saturation if its velocity,
• pseudo-velocity limit to be used in the grid computation acceleration, jerk, torque or torque rate is above 95% of its
algorithm, which in principle should be large enough capacity, in agreement with Table I. All solutions of Table III
to contain the maximum pseudo-velocity that the phase present a bang-bang behavior according to this approximation.
plane trajectory can reach, that is not known beforehand; With reference to Table III, we note that the optimal cost
• pseudo-velocity resolution, determining, on one side, the consistently is in a neighborhood of 0.6 s, with a variability
accuracy in the definition of the phase plane trajectory of a few milliseconds. By increasing the number of waypoints
only (see plans 4-6), the cost may increase or decrease. We
1 https://www.franka.de/panda
2 https://frankaemika.github.io/docs/ 3 https://t.ly/ZKelW
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 8

TABLE III
R ESULTS OF THE TOTP-R ALGORITHM ON THE STRAIGHT LINE PATH

Plan Number of PV RP res. PV Cost

ID waypoints limit (deg) res. (s)
1 10 1.4 0.500 0.02 0.655
2 10 1.4 0.250 0.02 0.594
3 10 1.4 0.250 0.01 0.592
4 10 1.4 0.125 0.02 0.574
5 15 1.4 0.125 0.02 0.645
6 20 1.4 0.125 0.02 0.592

should remark that more waypoints correspond to a larger

number of constraints in the task and less freedom in deviating
from the true straight line. On the other hand, more waypoints
allow to reduce the error related to the linear approximation
that we make at each step by using a simple Euler integration
scheme. Regardless of the number of waypoints selected by
the user, we note that, since (9) is a positional IK scheme,
the task space path output by the planner does not differ
from the input one. Unlike other planning techniques, task
deviations due to, e.g. integration of velocity or acceleration
equations, are not possible. As expected, with a fixed number
of waypoints, the cost decreases for finer resolutions of the
redundancy parameter and the pseudo-velocity (see plans 1-4),
although the improvement due to the latter, in our experiments,
is negligible (see plans 2 and 3).
The actuation limits of the Panda robot (see Table I), in
our experiments, are always such that constraints in (18)-(19)
activate before constraints in (20)-(22). This is also due to the
fact that planning is performed by considering zero load at the
end-effector, while jerk limits are very large. The resolution-
optimal joint space solution for plan 6 is reported in Fig. 3, its
kinematic derivatives in Fig. 4, torques and their derivatives in Fig. 3. Optimal joint positions for plan 6 of the straight line path.
Fig. 5. The computation time needed to find such a solution
is 219 minutes on a 64-bit Ubuntu 18.04 LTS OS running on
an Intel® CoreTM i7-2600 CPU @ 3.40GHz ×8. The TOTP-R the kinematic solver IKFast [48] in a way that grids are not
algorithm in Fig. 1 has been implemented in C++ over ROS4 homogeneous.
and MoveIt!5 . No multi-core execution model has been used The phase plane trajectory, that in the case of TOTP-R
in the experiments. is a phase space trajectory (PST), is reported in Fig. 7. As
We note that, at first, joint 1’s acceleration saturates to reach noted above, for redundant manipulators, the PST is a function
the maximum velocity, which persists for a large portion of the of two independent variables that are λ (the progress along
trajectory, between t = 0.16 s and t = 0.37 s. Here, there is the path) and v, the redundancy parameter(s). The 3D view
a rather simultaneous saturation of joint 4’s acceleration and provides an indication of how the DDP algorithm exploits
joint 7’s velocity, up until t = 0.44 s. In the last segment, the redundancy parameter to increase the pseudo-velocity and
joint 1’s acceleration saturates again, followed by joint 2’s consequently compute a better solution with respect to a pre-
acceleration to perform the final braking. For some segments assigned joint space path.
of the trajectory, two actuators are in saturation at the same The best parameters for the straight line path are directly
time, which is expected for a redundant manipulator, as argued re-used for the ellipse-like path, to assess their consistency
in [9]. across different paths. Since the path is longer (L = 1.45
A representation of the null-space for the straight line path
m), a higher number of waypoints should be used to keep
with 20 waypoints (plan 6) and joint position q1 is reported
a similar spatial resolution. The parameters used for this use
in Fig. 6 with feasibility maps [36], [43]. Because of the
case are reported in Table IV, together with the optimal cost.
task-manipulator geometry and joint limits, only 4 of the 8
Graphs are omitted for the sake of brevity, but similar results
grids expected for the Panda have feasible configurations,
are obtained. Even in this case, saturation only happens for
corresponding to the colored cells. The redundancy parameter,
velocities and accelerations.
i.e. q4 , is represented along the y-axis in its physical domain
In order to show that the algorithm is effectively impos-
(see Table I). The inverse kinematics solutions are returned by
ing dynamic constraints, e.g. actuation torques, we plan the
4 https://www.ros.org/ straight line path again by setting stricter torque limits, i.e.
T
5 https://moveit.ros.org/ τ max = [15, 30, 87, 10, 12, 12, 12] , and reusing the configu-
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 9

1 1

joint 1
joint 1

0 0

-1 -1
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
Time [s] Time [s]
1
1

joint 2
joint 2

0
0
-1
-1 0 0.1 0.2 0.3 0.4 0.5
0 0.1 0.2 0.3 0.4 0.5 Time [s]
Time [s] 1

joint 3
1
joint 3

0
0
-1
0 0.1 0.2 0.3 0.4 0.5
-1
0 0.1 0.2 0.3 0.4 0.5 Time [s]
Time [s] 1

joint 4
1
0
joint 4

0
-1
0 0.1 0.2 0.3 0.4 0.5
-1 Time [s]
0 0.1 0.2 0.3 0.4 0.5
Time [s] 1

joint 5
1 0
joint 5

0 -1
0 0.1 0.2 0.3 0.4 0.5
-1 Time [s]
0 0.1 0.2 0.3 0.4 0.5 1
Time [s]
joint 6

1 0
joint 6

0 -1
0 0.1 0.2 0.3 0.4 0.5
Time [s]
-1
0 0.1 0.2 0.3 0.4 0.5 1
joint 7

Time [s]
0
1
joint 7

-1
0 0 0.1 0.2 0.3 0.4 0.5
Time [s]
-1
0 0.1 0.2 0.3 0.4 0.5
Fig. 5. Optimal joint torques (blue) and torque rates (red) for plan 6 of the
Time [s]
straight line path, normalized in [−1, 1].
Fig. 4. Optimal joint velocities (blue), accelerations (red), and jerks (yellow)
for plan 6 of the straight line path, normalized in [−1, 1].
approach from the literature, in order to assess the gain in
TABLE IV performance given by the unified approach proposed in this
R ESULTS OF THE TOTP-R ALGORITHM ON THE ELLIPSE - LIKE PATH WITH
SAME PARAMETERS AS PLAN 6 OF TABLE III.
paper. The same straight line and ellipse-like paths are inverted
and time-parametrized with the algorithm in [1]. Each task
Length Number of PV RP res. PV Cost space waypoint is inverted through the local Jacobian-based
(m) waypoints limit (deg) res. (s) iterative optimization scheme
1.45 60 1.4 0.125 0.02 1.856

qk+1 (i) = qk (i) + βJ†k ek − α(I − J†k Jk )∇q ck (28)

ration parameters of plan 6 in Table III. Because of the stricter
limits, the cost of the solution increases to 0.644 s. Fig. 8 where k is the iteration index, J† is the n × m full-rank
shows the planned joint space trajectory, while Fig. 9 shows Moore-Penrose pseudo-inverse of the manipulator’s geometric
the saturating variables. In particular, joint 1’s torque saturates Jacobian, ek is the m × 1 pseudo-twist Cartesian error vector,
first, until reaching the maximum velocity, which persists until I is the identity matrix of appropriate dimension, ∇q ck is the
t = 0.43 s. Here, joint 7’s velocity saturates for a short period gradient of the cost function c selected for redundancy resolu-
of time. In the last segment, we observe a rather simultaneous tion, α = 10−4 and β = 10−1 are gains on the null-space
saturation of joint 1’s and joint 2’s torques to perform the final and Cartesian space correction terms, chosen after several
braking. Accelerations, jerks and torque rates never saturate. trials. The cost is the projection of the dynamic manipulability
ellipsoid onto the path’s tangent t, that is associated to the
C. Comparison with a decoupled approach acceleration/deceleration capabilities of the arm along the path
[1]:
Since we do not have the true minimum time trajectory
available, we compare our DP algorithm with a decoupled c(q) = tT (HJ† )T HJ† t. (29)
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 10

parameter (rad) -1.8

redundancy

/2
-2
0
-2.2 - /2
-
5 10 15 20
waypoints
parameter (rad)

-1.8
redundancy

/2
-2
0
-2.2 - /2
-
5 10 15 20
waypoints
parameter (rad)

-1.8
redundancy

/2
-2
0
-2.2 - /2
-
5 10 15 20
waypoints
parameter (rad)

-1.8
redundancy

/2
-2
0
-2.2 - /2
-
5 10 15 20
waypoints

Fig. 6. Panda grids for g = 1 (top) to g = 4 (bottom), representing q1 for

the straight line path; q1 color scale is in radians.

Fig. 8. Optimal joint positions for the straight line path, planned with the
same parameters as plan 6, with stricter torque limits.

joint displacement, velocity manipulability ellipsoid projection

and multi-objective functions considering two or more per-
formance indices together [49]. Furthermore, the initial joint
positions must be assigned and cannot be retrieved as part of
the solution. Lastly, the redundancy resolution procedure is
not guarantee to converge in case of, e.g., strict joint limits,
crossing of singularities. In our solution returning the cost
of 1.280 s (more than twice the average time in Table III),
redundancy resolution drives some joints close to their limits,
Fig. 7. Phase space trajectory (blue) with projections (black) on planes λ-λ̇ which forces the robot to slow down its progression on the
and λ-v. path, reconfigure, and accelerate again. This is the main cause
behind the huge deterioration of performance with respect to
the unified approach.
After the joint-space path has been obtained, it is time-
parametrized with the algorithm from [37], by using the same
pseudo-velocity limit and a pseudo-velocity resolution of 0.01. D. Comparison with a multiple shooting approach
The trajectory tracking times for the straight line and the
ellipse-like paths are 1.280 s and 2.557 s, about 113% and In Section I-B, we identified the technique proposed in [14]–
38% higher than the unified approach. The CPU time for [17] as the state of the art in terms of time-optimal planning for
the whole planning process is about 1-2 minutes. Besides the redundant robots with a unified approach. We discussed some
mere assessment of cost and performance, we remark that of its advantages, including the possibility of easily adding
the decoupled approach is very sensitive to the parameters α generic, possibly nonlinear constraints on the joint positions
and β, as well as the specific objective function c selected and their derivatives and generating smooth solutions. Here,
for redundancy resolution. Other typical choices for c are we compare it with our approach on the straight line path. The
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 11

1 where N is the number of multiple shooting intervals, T k =

joint 1

0 1, . . . , N , y = y0T , uT0 , . . . , yN
T
−1 , u T
N −1 , y T
, tf is the
h iTN
T T
-1
0 0.1 0.2 0.3 0.4 0.5 0.6
decision vector, with yk = λk , λ̇k , qk , q̇k , i.e. the state
Time [s]
h iT
1 vector at the k-th interval, uk = λ̈k , γk,1 , . . . , γk,r , i.e. the
input vector at the k-th interval, wk ∈ Rm is the end-effector
joint 2

0
twist in the base frame, ak,i are the r vectors in the null space
-1
0 0.1 0.2 0.3 0.4 0.5 0.6
basis of Jk , γk,i are their gains, and gRK4 is the 4-th order
Time [s] explicit Runge-Kutta integration scheme, employed to impose
1 continuity between multiple shooting intervals.
joint 3

0
We select N = 20, yielding 359 decision variables in y,
initialize yk and λ̈k , ∀k, to match the joint space trajectory
-1 obtained from Section IV-C, and we set γk = γk,1 = 0, ∀k, as
0 0.1 0.2 0.3 0.4 0.5 0.6
Time [s] done in [14], [15]. Since the algorithms in [14]–[17] are not
1 publicly available, we implement problem (30) in MATLAB
joint 4

0 and solve it with fmincon using the interior-point method.

The trajectory tracking time is 1.165 s (about 94% higher than
-1
0 0.1 0.2 0.3 0.4 0.5 0.6 our approach), the CPU time is 192 minutes. The involved
Time [s] quantities only partially saturate, which confirms the locally-
1 optimal nature of the solution. Indeed, as noted in [14]–[17],
joint 5

0 global optimality is not guaranteed as problem (30) is non-

convex, and neither is convergence for some choices of the
-1
0 0.1 0.2 0.3 0.4 0.5 0.6 parameters and initial guess.
Time [s]
1
joint 6

E. Additional comparison trials

0
In order to confirm the correctness of the comparison, we
-1
0 0.1 0.2 0.3 0.4 0.5 0.6 ran additional trials assuming availability of a-priori informa-
Time [s] tion on the optimal solution of the straight line use case. In
1
particular, by initializing problem (28) with the initial positions
joint 7

0 retrieved by our DDP algorithm (Fig. 3), setting c to joint

-1
displacement [49], and re-executing the steps of Section IV-C
0 0.1 0.2 0.3 0.4 0.5 0.6 again, a trajectory tracking time of 0.628 s is achieved. If we
Time [s]
use the latter solution as the initial guess for y in (30), the
Fig. 9. Optimal joint velocities (blue) and torques (red), planned with the interior-point algorithm is able to consistently reach trajectory
same parameters as plan 6, with stricter torque limits, normalized in [−1, 1]. tracking times of ∼ 0.600 s, for different configurations of the
parameters N, γk . This confirms the strong dependency of the
approaches in Sections IV-C and IV-D on the initial guess, as
problem formulation is: well as their locally-optimal nature. In our approach, no initial
information on the solution needs to be available, as a global
min tf exploration of the search space is performed.
y
s.t. 0 ≤ λk ≤ 1
λ̇k ≥ 0 F. Execution
qmin ≤ qk ≤ qmax In this section, we assess the feasibility of the generated
q̇min ≤ q̇k ≤ q̇max plan. We want to confirm that, despite the discrete approxima-
tions embedded in the DDP algorithm, the plan is actually ex-
q̈min ≤ q̈k ≤ q̈max ecutable through a standard interpolation and control scheme.
τ min ≤ τ k ≤ τ max We experimentally show that no additional requirements are
r (30) imposed to the control system, besides controlling the plant in
q̈k = J†k (ẇk − J̇k q̇k ) +
X
γk,i ak,i proximity of actuation limits.
i=1 The execution of the TOTP-R algorithm on either trajectory
τ k = H(qk )q̈k + f (qk , q̇k ) generates several control signals, as seen in Fig. 4 and Fig. 5.
yk+1 − gRK4 (yk , uk ) = 0 Before they can be used as commands for the Panda robot,
xk = k(qk ) it is necessary to interpolate them at the frequency of the
controller, i.e. 1 kHz. We choose to control joint positions,
λ0 = 0, λN = 1 while command torques are generated with an internal joint
λ̇0 = λ̇N = 0 impedance controller, configured with maximum stiffness.
q̇0 = q̇N = 0 Position control has already been used in other works, e.g.
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 12

[3], dealing as well with execution of time-optimal joint space trajectory tracking error. Moreover, the latter is comparable
solutions, with satisfactory results. Furthermore, it is a typical to other experiments of time-optimal trajectories execution
choice as many robots do not allow for torque commands for [3]. In comparing with previous works, we should remark
practicality and safety reasons. that our plans are calculated with the real robot capacities,
A high-level description of the control setup is shown as reported in Table I, while conservative values might be
in Fig. 10. As mentioned, Algorithm 1 is implemented in employed elsewhere, e.g. a given percentage of the actual
ROS/MoveIt! and deployed as a node. Task space paths x(λ) limit.
are provided in bag files and optimal joint space solutions, i.e. The same planned trajectory is scaled in time to be 10%
curves q(t), q̇(t) and q̈(t) are also stored in bag files. The faster and is sent to the robot again to estimate its performance.
controller manager of ros_control6 is configured to work In this case, the motion is completed in 1.70 s, 0.67% slower
with the Panda robot hardware interface (provided through than planned, with a maximum error of 1.86 deg, associated to
franka_ros7 ) and to load a robot-agnostic joint trajectory joint 1 at t = 0.11 s. Joint position tracking errors are reported
controller (JTC). Internally, the JTC performs quintic interpo- in Fig. 12. As expected, the robot cannot be controlled beyond
lation on the input signals to deliver smoother signals to the its capacities and the actual motion results in a remarkable
robot driver. degradation of tracking performances. We also note here that
the controller aborts for trajectories that are more than 10%
faster than the optimal plan.

Joint position errors [rad]

0.01

-0.01 Joint 1 Joint 3 Joint 5 Joint 7

Joint 2 Joint 4 Joint 6

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8

Time [s]

Fig. 10. Block diagram showing the control setup. Fig. 11. Joint position errors for the ellipse-like trajectory.

In order to guarantee that the interpolation of the planned

Joint position errors [rad]

0.04
trajectories still generates curves that are within bounds, we
exploit some basic signal processing functions that are already 0.02
provided by the manufacturer, i.e. a low-pass filter and a
0
rate limiter. In particular, both of them are active to deliver
less oscillating commands and ensure compliance to actuator -0.02 Joint 1 Joint 3 Joint 5 Joint 7
Joint 2 Joint 4 Joint 6
limits. The employment of a filter to smooth the trajectory
-0.04
is also beneficial to eliminate the high frequency content that 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6
excites the unmodeled joint elasticity that characterizes joint Time [s]

transmissions in time-optimal control [50]. In fact, we note Fig. 12. Joint position errors for a trajectory 10% faster than the optimum.
that controlling the motors in the proximity of their limits,
so that they saturate, reserves no margin for the controller to
compensate for unmodeled dynamics and/or uncertainties in V. C ONCLUSION
the modeled dynamics. Some techniques explictly addressing
In this paper, a DDP framework has been presented to
this control issues can be found in [51]–[60].
plan trajectories for redundant robots along prescribed paths.
The joint position tracking errors for the ellipse-like trajec-
The framework unifies redundancy resolution and trajectory
tory are reported in Fig. 11. The robot completes the motion
planning so as to solve them together to effectively utilize
in 1.859 s, 0.13% slower than planned with a maximum
kinematic redundancy to optimize the objective function de-
error in joint space of 0.8 deg. The delay introduced by the
fined at trajectory planning level. The extreme flexibility of
low-pass filter has been removed in the plots to provide a
DDP has been exploited to accommodate generic constraints
faithful comparison with the planned references. A video of
that characterize real applications. DP ensures resolution-
the execution8 is provided as supplemental material. While
optimality, i.e. the asymptotic achievement of the globally-
the actual tracking performance is dependant on the specific
optimal solution as the dicretization step tends to zero.
robot controller, we show that the trajectories planned with our
Although it is perfectly suited to solve one or the other prob-
methodology are feasible on a general-purpose architecture,
lem separately, and it is applicable to many offline planning
i.e. the controller does not abort because of, e.g., out-of-
scenarios, e.g. planning of cyclic operations, DDP still requires
bound or jerky references, poor smoothness or excessive
hours to days to solve a medium-complexity planning problem
6 http://wiki.ros.org/ros control for redundant robots (one or two orders of magnitude higher
7 https://frankaemika.github.io/docs/franka ros.html than other approaches) and, most importantly, its scalability
8 https://t.ly/ZKelW for systems characterized by a higher degree of redundancy,
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 13

i.e. r > 1, is questionable. Also, in order to impose exact [15] A. Reiter, H. Gattringer, and A. Müller, “Redundancy resolution in
constraints on higher-order derivatives, the phase space should minimum-time path tracking of robotic manipulators,” in Proceedings of
the 13th International Conference on Informatics in Control, Automation
be further augmented with additional dimensions. and Robotics. SCITEPRESS - Science and Technology Publications,
Nevertheless, the proposed state space formalization can 2016, pp. 61–68.
be employed with optimization techniques other than DDP, [16] A. Reiter, H. Gattringer, and A. Müller, “On redundancy resolution
in minimum-time trajectory planning of robotic manipulators along
such as randomized algorithms, for those scenario where predefined end-effector paths,” in Informatics in Control, Automation
planning time is a constraint. Additional future developments and Robotics. Springer International Publishing, nov 2017, pp. 190–
might regard a deeper study into decoupled techniques to 206.
evaluate whether some specific objective functions can be [17] A. Reiter, A. Müller, and H. Gattringer, “On higher order inverse kine-
matics methods in time-optimal trajectory planning for kinematically
used for redundancy resolution (e.g., acceleration capability redundant manipulators,” IEEE Transactions on Industrial Informatics,
of the manipulator) that allow optimizing trajectory planning vol. 14, no. 4, pp. 1681–1690, April 2018.
at a later stage. Also, it should be investigated whether the [18] X. Wang, Y. Gan, and X. Dai, “Time-optimal and smooth trajectory
planning based on continuous pseudo-acceleration,” in 2023 Interna-
optimization of integral performance indices can be beneficial tional Conference on Advanced Robotics and Mechatronics (ICARM).
to this respect, in place of the local optimization methods IEEE, jul 2023.
discussed in [1]–[3]. [19] U. Wolinski and M. Wojtyra, “A novel QP-based kinematic redundancy
resolution method with joint constraints satisfaction,” IEEE Access,
vol. 10, pp. 41 023–41 037, 2022.
ACKNOWLEDGMENTS [20] Y. Zhang and Y. Jia, “Motion planning of redundant dual-arm robots
with multicriterion optimization,” IEEE Systems Journal, vol. 17, no. 3,
The authors would like to thank Simon Lacroix for kindly pp. 4189–4199, sep 2023.
[21] T. Marauli, H. Gattringer, and A. Müller, “Time-optimal path fol-
supporting the setup of the experiments, and Federica Storiale lowing for non-redundant serial manipulators using an adaptive path-
and Giuseppe Santoriello for contributing to some of the discretization,” Robotica, vol. 41, no. 6, pp. 1856–1871, mar 2023.
software modules employed in the experiments. [22] J. Ogbemhe, K. Mpofu, and N. Tlale, “Optimal trajectory scheme for
robotic welding along complex joints using a hybrid multi-objective
genetic algorithm,” IEEE Access, vol. 7, pp. 158 753–158 769, 2019.
R EFERENCES [23] Z. Wang, Y. Li, K. Shuai, W. Zhu, B. Chen, and K. Chen, “Multi-
objective trajectory planning method based on the improved elitist non-
[1] P. Chiacchio, “Exploiting redundancy in minimum-time path following dominated sorting genetic algorithm,” Chinese Journal of Mechanical
robot control,” in 1990 American Control Conference. IEEE, May 1990, Engineering, vol. 35, no. 1, feb 2022.
pp. 2313–2318. [24] X. Zhang and G. Shi, “Multi-objective optimal trajectory planning for
[2] F. Basile and P. Chiacchio, “A contribution to minimum-time task-space manipulators in the presence of obstacles,” Robotica, vol. 40, no. 4, pp.
path-following problem for redundant manipulators,” Robotica, vol. 21, 888–906, jul 2021.
no. 2, pp. 137–142, February 2003. [25] Z. Wang, Y. Li, P. Sun, Y. Luo, B. Chen, and W. Zhu, “A multi-objective
[3] K. Al Khudir and A. De Luca, “Faster motion on cartesian paths approach for the trajectory planning of a 7-DOF serial-parallel hybrid
exploiting robot redundancy at the acceleration level,” IEEE Robotics humanoid arm,” Mechanism and Machine Theory, vol. 165, p. 104423,
and Automation Letters, vol. 3, no. 4, pp. 3553–3560, October 2018. nov 2021.
[4] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, “Time-optimal control of [26] M. Schappler, “Pose optimization of task-redundant robots in second-
robotic manipulators along specified paths,” The International Journal order rest-to-rest motion with cascaded dynamic programming
of Robotics Research, vol. 4, no. 3, pp. 3–17, September 1985. and nullspace projection,” in Informatics in Control, Automation and
[5] K. Shin and N. McKay, “Minimum-time control of robotic manipulators Robotics. Springer International Publishing, 2023, pp. 106–131.
with geometric path constraints,” IEEE Transactions on Automatic [27] G. Singh and V. K. Banga, “Combinations of novel hybrid optimization
Control, vol. 30, no. 6, pp. 531–541, June 1985. algorithms-based trajectory planning analysis for an industrial robotic
[6] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectory manipulators,” Journal of Field Robotics, vol. 39, no. 5, pp. 650–674,
planning,” IEEE Journal on Robotics and Automation, vol. 3, no. 2, mar 2022.
pp. 115–123, April 1987. [28] J. Ma, Z. Cheng, X. Zhang, Z. Lin, F. L. Lewis, and T. H. Lee, “Local
[7] J.-J. E. Slotine and H. S. Yang, “Improving the efficiency of time- learning enabled iterative linear quadratic regulator for constrained tra-
optimal path-following algorithms,” IEEE Transactions on Robotics and jectory planning,” IEEE Transactions on Neural Networks and Learning
Automation, vol. 5, no. 1, pp. 118–124, 1989. Systems, vol. 34, no. 9, pp. 5354–5365, sep 2023.
[8] Y. Chen, S. Y.-P. Chien, and A. A. Desrochers, “General structure of
[29] G. Chen, M. Ma, S. Guo, B. Hou, and J. Wang, “An easy-implemented
time-optimal control of robotic manipulators moving along prescribed
optimization method of trajectory planning based on polynomial inter-
paths,” International Journal of Control, vol. 56, no. 4, pp. 767–782,
polation,” in 2021 40th Chinese Control Conference (CCC). IEEE, jul
October 1992.
2021.
[9] M. Galicki and I. Pajak, “Optimal motions of redundant manipulators
with state equality constraints,” in Proceedings - IEEE International [30] H. Wu, J. Yang, S. Huang, X. Ning, and Z. Zhang, “Multi-objective
Symposium on Assembly and Task Planning (ISATP’99). IEEE, July adaptive trajectory optimization for industrial robot based on accelera-
1999, pp. 181–185. tion continuity constraint,” Robotics and Computer-Integrated Manufac-
[10] M. Galicki, “Time-optimal controls of kinematically redundant manipu- turing, vol. 84, p. 102597, dec 2023.
lators with geometric constraints,” IEEE Transactions on Robotics and [31] L. Zlajpah and A. Muller, “A task space decomposition algorithm for
Automation, vol. 16, no. 1, pp. 89–93, 2000. the inverse kinematics of functionally redundant manipulators,” in 2021
[11] E. Ferrentino and P. Chiacchio, “Redundancy parametrization in 20th International Conference on Advanced Robotics (ICAR). IEEE,
globally-optimal inverse kinematics,” in Advances in Robot Kinematics dec 2021.
2018. Springer International Publishing, June 2018, pp. 47–55. [32] A. P. Pashkevich, A. B. Dolgui, and O. A. Chumakov, “Multiobjective
[12] A. Reiter, K. Springer, H. Gattringer, and A. Müller, “An explicit ap- optimization of robot motion for laser cutting applications,” Interna-
proach for time-optimal trajectory planning for kinematically redundant tional Journal of Computer Integrated Manufacturing, vol. 17, no. 2,
serial robots,” PAMM, vol. 15, no. 1, pp. 67–68, oct 2015. pp. 171–183, March 2004.
[13] S. Ma and M. Watanabe, “Time optimal path-tracking control of kine- [33] A. Dolgui and A. Pashkevich, “Manipulator motion planning for high-
matically redundant manipulators,” JSME International Journal Series speed robotic laser cutting,” International Journal of Production Re-
C, vol. 47, no. 2, pp. 582–590, 2004. search, vol. 47, no. 20, pp. 5691–5715, July 2009.
[14] A. Reiter, A. Müller, and H. Gattringer, “Inverse kinematics in [34] A. Guigue, M. Ahmadi, R. Langlois, and M. J. D. Hayes, “Pareto
minimum-time trajectory planning for kinematically redundant manipu- optimality and multiobjective trajectory planning for a 7-dof redundant
lators,” in IECON 2016 - 42nd Annual Conference of the IEEE Industrial manipulator,” IEEE Transactions on Robotics, vol. 26, no. 6, pp. 1094–
Electronics Society. IEEE, October 2016, pp. 6873–6878. 1099, December 2010.
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 14

[35] J. Gao, A. Pashkevich, and S. Caro, “Optimization of the robot and [59] W. Niu and M. Tomizuka, “A new approach of coordinated motion
positioner motion in a redundant fiber placement workcell,” Mechanism control subjected to actuator saturation,” Journal of Dynamic Systems,
and Machine Theory, vol. 114, pp. 170–189, August 2017. Measurement, and Control, vol. 123, no. 3, pp. 496–504, jul 1999.
[36] E. Ferrentino and P. Chiacchio, “On the optimal resolution of inverse [60] H. Tam, “Minimum time closed-loop tracking of a specified path by
kinematics for redundant manipulators using a topological analysis,” robot,” in 29th IEEE Conference on Decision and Control. IEEE,
Journal of Mechanisms and Robotics, vol. 12, no. 3, June 2020. December 1990, pp. 3132–3137.
[37] K. Shin and N. McKay, “A dynamic programming approach to trajectory
planning of robotic manipulators,” IEEE Transactions on Automatic
Control, vol. 31, no. 6, pp. 491–500, June 1986.
[38] S. Singh and M. C. Leu, “Optimal trajectory generation for robotic ma-
nipulators using dynamic programming,” Journal of Dynamic Systems,
Measurement, and Control, vol. 109, no. 2, pp. 88–96, 1987.
[39] D. Kaserer, H. Gattringer, and A. Müller, “Nearly optimal path following Enrico Ferrentino (M’21-S’20) received the M.S.
with jerk and torque rate limits using dynamic programming,” IEEE degree in computer and automation engineering from
Transactions on Robotics, vol. 35, no. 2, pp. 521–528, April 2019. the Polytechnic University of Turin, Turin, Italy and
[40] ——, “Time optimal motion planning and admittance control for coop- the Ph.D. degree in robotics from the University of
erative grasping,” IEEE Robotics and Automation Letters, vol. 5, no. 2, Salerno, Fisciano, Italy. Between 2013 and 2014, he
pp. 2216–2223, April 2020. was Visiting Scholar at the NASA Jet Propulsion
[41] R. Bellman, Ed., Dynamic Programming. Princeton University Press, Laboratory in Pasadena, USA, where he joined the
1957. Axel tethered robot project, in the Robotic Mobility
[42] V. Petrone, E. Ferrentino, and P. Chiacchio, “Time-optimal trajectory group led by Dr. I. Nesnas Between 2014 and
planning with interaction with the environment,” IEEE Robotics and 2017 he was Ground Segment Engineer at ALTEC
Automation Letters, vol. 7, no. 4, pp. 10 399–10 405, oct 2022. S.p.A., Turin, Italy, where he joined the ESA robotic
mission ExoMars. In 2019, he was Visiting Scholar at LAAS-CNRS in
[43] J. A. Pámanes, P. Wenger, and J. L. Zapata, “Motion planning of
Toulouse, France, where he joined the H2020 PRO-ACT project, in the team
redundant manipulators for specified trajectory tasks,” in Advances in
led by Prof. A. Franchi. Since 2020, he is Lecturer of Robotics and Medical
Robot Kinematics. Springer Netherlands, 2002, pp. 203–212.
Robotics with the Department of Information and Electric Engineering and
[44] P. Wenger, P. Chedmail, and F. Reynier, “A global analysis of following Applied Mathematics at the University of Salerno and, since 2021, Researcher
trajectories by redundant manipulators in the presence of obstacles,” of the Automatic Control Group in the same institution. His research interests
in [1993] Proceedings IEEE International Conference on Robotics and include optimal planning of redundant and cooperating robots, interaction
Automation. IEEE Comput. Soc. Press, 1993. control, human-robot collaboration. His applications of interest fall in the
[45] H. Pham and Q.-C. Pham, “On the structure of the time-optimal path fields of industrial, aerospace and medical robotics.
parameterization problem with third-order constraints,” in 2017 IEEE
International Conference on Robotics and Automation (ICRA). IEEE,
May 2017.
[46] A. Guigue, M. Ahmadi, M. J. D. Hayes, and R. G. Langlois, “A discrete
dynamic programming approximation to the multiobjective deterministic
finite horizon optimal control problem,” SIAM Journal on Control and
Optimization, vol. 48, no. 4, pp. 2581–2599, January 2009. Heitor J. Savino has a B.S. degree in Mechatronics
[47] C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, and A. De Luca, Engineering from Amazon State University (UEA),
“Dynamic identification of the Franka Emika Panda robot with retrieval a M.Eng. from Amazon Federal University (UFAM),
of feasible parameters using penalty-based optimization,” IEEE Robotics and a Doctoral degree in Electrical Engineering
and Automation Letters, vol. 4, no. 4, pp. 4147–4154, October 2019. from Federal University of Minas Gerais (UFMG),
[48] R. Diankov, “Automated construction of robotic manipulation pro- 2016.
grams,” Ph.D. dissertation, The Robotics Institute, Carnegie Mellon Visiting graduate student at Massachusetts Insti-
University, 2010. tute of Technology (MIT), as member of Interactive
[49] F. Storiale, E. Ferrentino, and P. Chiacchio, “Two-stage time-optimal Robotics Group (IRG), 2015-2016. Conducted post-
planning of robots along pre-scribed paths with integral optimization of doctoral research at UFMG from 2016-2017, and
redundancy,” in 9th International Conference on Control, Decision and at LAAS-CNRS, 2019-2020, as part of the H2020
Information Technologies (CoDIT), 2023. PRO-ACT project. Assistant Professor at Federal University of Alagoas
[50] J. Kim and E. A. Croft, “Online near time-optimal trajectory planning (UFAL) 2017-2021, at Institute of Computing (IC). Since 2022, is the founder
for industrial robots,” Robotics and Computer-Integrated Manufacturing, and leading researcher at Ambev Robotics Laboratory, subsidiary of Anheuser-
vol. 58, pp. 158–171, August 2019. Busch InBev, to conduct research on robotics for the beverage industry.
[51] H. Arai, K. Tanie, and S. Tachi, “Path tracking control of a manip-
ulator considering torque saturation,” IEEE Transactions on Industrial
Electronics, vol. 41, no. 1, pp. 25–31, 1994.
[52] O. Dahl and L. Nielsen, “Torque-limited path following by online
trajectory time scaling,” IEEE Transactions on Robotics and Automation,
vol. 6, no. 5, pp. 554–561, October 1990.
Antonio Franchi (F’23–SM’16–M’07) received
[53] Z. Shiller and H. Chang, “Trajectory preshaping for high-speed articu-
the M.Sc. degree in electrical engineering and
lated systems,” Journal of Dynamic Systems, Measurement, and Control,
the Ph.D. degree in system engineering from the
vol. 117, no. 3, pp. 304–310, sep 1995.
Sapienza University of Rome, Rome, Italy, in 2005
[54] Z. Shiller, H. Chang, and V. Wong, “The practical implementation of and 2010, respectively, and the HDR degree in
time-optimal control for robotic manipulators,” Robotics and Computer- Science, from the National Polytechnic Institute of
Integrated Manufacturing, vol. 12, no. 1, pp. 29–39, mar 1996. Toulouse in 2016. From 2010 to 2013 he was a
[55] J. Kieffer, A. Cahill, and M. James, “Robust and accurate time-optimal research scientist at the Max Planck Institute for
path-tracking control for robot manipulators,” IEEE Transactions on Biological Cybernetics, Tübingen, Germany. From
Robotics and Automation, vol. 13, no. 6, pp. 880–890, 1997. 2014 to 2019 he was a tenured CNRS researcher
[56] O. Dahl, “Path-constrained robot control with limited torques- at LAAS-CNRS, Toulouse, France. From 2019 to
experimental evaluation,” IEEE Transactions on Robotics and Automa- 2021 he was an associate professor at the University of Twente. Enschede,
tion, vol. 10, no. 5, pp. 658–669, 1994. The Netherlands. Since 2022 he is a full professor in aerial robotics control
[57] J. Moreno-Valenzuela, “Tracking control of on-line time-scaled trajec- at the University of Twente. Enschede, The Netherlands. Since 2023 he is
tories for robot manipulators under constrained torques,” in Proceedings also a full professor at the department of Computer, Control and Management
- 2006 IEEE International Conference on Robotics and Automation Engineering, Sapienza University of Rome, Rome, Italy. He co-authored more
(ICRA). IEEE, 2006, pp. 19–24. than 160 peer-reviewed international publications on design and control of
[58] K. S. Eom, I. H. Suh, and W. K. Chung, “Disturbance observer robotic systems applied to multi-robot systems and aerial robots.
based path tracking control of robot manipulator considering torque
saturation,” Mechatronics, vol. 11, no. 3, pp. 325–343, apr 2001.
ACCEPTED VERSION, PUBLISHED AT HTTPS://DOI.ORG/10.1109/TASE.2023.3330371 – IEEE TRANSACTIONS ON AUTOMATION SCIENCE AND ENGINEERING 15

Pasquale Chiacchio is Professor of Automatic Con-

trol and Robotics in the Department of Information
and Electric Engineering and Applied Mathematics,
University of Salerno. His main research interests
include robotics and modeling, and control of dis-
crete event systems. In the robotics field, he has
been working on robot control and identification, in-
verse kinematic problem, interaction control, control
of redundant manipulators, control of cooperative
manipulators. In the discrete event systems field, he
has been working on supervisory control based on
monitors, optimal supervisory control, and formal specification for supervisory
systems. The results have been published in the main journals of the sector
and have been accompanied by an intense experimental activity. He has been
the coordinator of two Research Projects of National Interest (PRIN) and has
been involved in research projects funded by the European Union (ECHORD,
AIRobots, EuRoC, LOCOMACHS, LABOR). He is Senior member of the
Institute of Electrical and Electronic Engineers (IEEE) and member of the
Italian Society of Control Researchers. In December 2011, he was nominated
Knight, then in July 2014 promoted to Officer of the Order of Merit of
the Italian Republic. From 2016 he is the Director of the Ph.D. program
in Information Engineering.