# Time-Optimal Control via Heaviside Step-Function Approximation

Kai Pfeiffer, Quang-Cuong Pham The authors are with the Schaeffler Hub for
Advanced Research at NTU and School of Mechanical and Aerospace Engineering,
Nanyang Technological University, SingaporeThis work is partly supported by
the Schaeffler Hub for Advanced Research at NTU, under the ASTAR IAF-ICP
Programme ICP1900093.

###### Abstract

Least-squares programming is a popular tool in robotics due to its simplicity
and availability of open-source solvers. However, certain problems like sparse
programming in the 0- or 1-norm for time-optimal control are not equivalently
solvable. In this work we propose a non-linear hierarchical least-squares
programming (NL-HLSP) for time-optimal control of non-linear discrete dynamic
systems. We use a continuous approximation of the heaviside step function with
an additional term that avoids vanishing gradients. We use a simple
discretization method by keeping states and controls piece-wise constant
between discretization steps. This way we obtain a comparatively easily
implementable NL-HLSP in contrast to direct transcription approaches of
optimal control. We show that the NL-HLSP indeed recovers the discrete time-
optimal control in the limit for resting goal points. We confirm the results
in simulation for linear and non-linear control scenarios.

##  I Introduction

Time-optimal control can be considered a powerful tool when as fast as
possible task fulfillment of a dynamic system is desired. However, optimal
control methods based on direct methods for problem discretiziation are not
easily implementable [1] or their solution relies on proprietary software [2].
In this work we propose a non-linear hierarchical least-squares programming
(NL-HLSP) that can be easily implemented ($\sim$20 lines of code if a non-
linear solver and a task library are available) and that is provably
convergent to the true discrete time-optimal control in the limit and for
resting goal points (that is the system is able to remain at these points; for
example a robot is not able to remain at a Cartesian point if at the same time
the desired velocity is not zero).

Optimal control is the problem of identifying a control $u$ such that a
dynamic system with states $x$ is driven to a desired goal state $f_{d}$ while
minimizing a defined cost on control $u$, the state $x$ or linear / non-linear
functions of it. A desired goal is thereby a state that the system should end
up in while considering constraints on the controls $u$ and / or the states
$x$. This boils down to a constrained optimization problem with Ordinary
Differential Equations (ODE), that describe the dynamic system behavior, and
algebraic equations, that describe physical relations [3].

Analytic solutions to certain specifications of the continuous optimal control
problem have been proposed. A solution to the linear quadratic regulator (LQR)
with linear dynamics can be found in [4]. The authors in [5] propose a
solution with limits on the controls. However, for complicated non-linear
systems, and especially constrained problems, analytical solutions are usually
too difficult or impossible to formulate. Instead, in order to be machine
solvable the original optimal control problem can be discretized and then be
solved as a non-linear programming (NLP), commonly referred to as direct
transcription method [6]. Different methods have been proposed which differ in
the polynomials and collocation points (at which the functions are evaluated)
that are used to approximate the continuous controls and states. The authors
in [7] use Legendre-Gauss-Radau quadrature as it allows easy constraint
formulation and is shown to possess high stability for systems of high order
ODE’s. Legendre-Gauss-Lobatto points however provide the smallest
interpolation error in a least-squares sense [8]. The open-source matlab
implementation GPOPS of a direct transcription method is described in [1].
However, the corresponding C++ implementation [2] is proprietary and
furthermore relies on the proprietary NLP solver SNOPT [9].

A specific form of optimal control is time-optimal control [10]. Here the cost
function specifically aims to minimize the time at which a desired goal state
is reached. The resulting control usually exhibits a ‘bang-bang’ profile, that
is a limit control for given bounds on the control inputs [11].

Time-optimal control is a complex problem, especially when the controls and
states are considered at the same time, for example for whole-body robot
trajectory optimization [12]. One way to reduce its complexity is to reduce
the discrete time-optimal control problem to a simpler one by only considering
the controls while the states are assumed to be known. This method, referred
to as time-optimal path parametrization, has seen significant leaps in the
recent past in terms of accuracy, convergence and computational complexity
[13]. However, how to choose the states is oftentimes not entirely clear but
might be taken for example from kinematic solutions or motion capture.

Aside direct collocation methods, optimal control can also be discretized to
reasonable accuracy for example by the explicit Euler-method [14, 15, 16]. In
this case time-optimal control can be considered a mixed-integer non-linear
programming [17] where the discrete optimal time is represented as an integer
and the corresponding states and controls are continuous. Such problems, even
in the linear case [18] or for example expressed as optimization problems with
$\ell_{0}$-norm cost functions for sparsity enhancing linear regression [19],
are very expensive to solve due to their combinatorial nature. A simplified
approach only considering a robot’s end-effector position in a traveling
salesman scenario has been treated in [20]. Another approach is to turn the
$\ell_{0}$\- into a weighted $\ell_{1}$-norm optimization problem. Thereby,
different weights have been proposed in the literature [19, 21, 22] which aim
to represent the original problem as closely as possible. This approach has
been borrowed for discrete linear dynamic systems for example in [14] with a
globally converging weight series. A similar approach is followed in [23],
confirming that $\ell_{1}$-norm optimization can retrieve an equivalent
solution to discrete time-optimal control. The authors in [24] propose a
similar method based on a sliding window but formulated as an $\ell_{2}$-norm
optimization problem. The window is iteratively shifted until the time-optimal
control is contained within. However, no global convergence guarantee is
given.

Control problem formulation as least-squares problems ($\ell_{2}$-norm) is
oftentimes appropriate and sufficient in robot control and planning [25, 26]
and enjoys great popularity because of its simplicity and availability of
open-source solvers [27]. A special formulation of least-squares programming
is hierarchical least-squares programming [28]. Here constraints and
objectives can be further prioritized within such that a more efficient robot
control can be achieved [29]. Especially the separation of regularization
tasks is oftentimes very helpful as the actual objectives can be fulfilled to
higher accuracy as has been demonstrated in [30]. However, there are problem
settings that are not equivalently solvable in the $\ell_{2}$-norm. An example
would be the above mentioned problem in the $\ell_{1}$-norm for discrete time-
optimal control [14, 23]. In this work we propose a globally converging
approximation of the discrete time-optimal control problem in the
$\ell_{2}$-norm based on the approximate heaviside function [31]. The
heaviside function has been treated in different works. The authors in [32]
use a heaviside step-function approximation in order to indicate mechanical
stress violations in topology optimization. The authors in [33] directly
optimize over the discontinuous Heaviside step-function and propose an
appropriate Newton’s method to do so.

Our contribution is therefore threefold:

  * •

Discrete time-optimal control as an easily implementable NL-HLSP that can be
solved by off the shelf non-linear least-squares solvers.

  * •

Applicability to both linear and non-linear systems.

  * •

Global convergence for discrete time-optimal control formulated as a least-
squares problem.

First, we outline continuous time-optimal control and our discretization of
it, see sec. II. Secondly, we describe our approximated discrete time-optimal
control problem and the weight function based on the heaviside step-function
approximation (sec. III). In sec. IV we show the equivalence to the true
discrete time-optimal control problems in the limit and for resting goal
points. Lastly, the algorithm is evaluated for linear and non-linear discrete
dynamic systems (sec. V).

##  II Problem definition

In this work we consider time-optimal control (TOC) problems of the form

$\displaystyle\operatorname*{min.}_{x,u,T}\qquad$
$\displaystyle\int^{T}_{0}dt$ (TOC) s.t
$\displaystyle\dot{x}(t)=f_{\text{dyn}}(x(t),u(t))$ $\displaystyle
f_{\text{ter}}(x(T))=0$ $\displaystyle x(t),u(t)\in\Omega$

The states $x(t)\in\mathbb{R}^{n_{x}}$ and $u(t)\in\mathbb{R}^{n_{u}}$ are
continuous in time $t$. The goal is to minimize the time $T$ that it takes to
reach the terminal (ter.) state $f_{\text{ter}}(x(T))=0$. We define

$\displaystyle f_{\text{ter}}(x(t))\coloneqq f_{\text{task}}(x(t))-f_{d}(t)$
(1)

where $f_{\text{task}}(x(t))$ is some task function and $f_{d}$ is the
corresponding desired value. The dynamics $f_{\text{dyn}}$ determine the
behavior of the control system by some possibly non-linear relationship.
$\Omega$ is a constraint polytope which both the states and controls are
constrained to.

The problem can be discretized for example by direct transcription methods.
These methods provide an equivalent solution to the original continuous
optimal control problem (TOC) with strong convergence and stability properties
[7]. However, they are not easily implementable or rely on proprietary
software. We therefore discretize our problem by the explicit Euler method by
assuming the piece-wise constant states and controls over each discritization
instance $i=0,\dots,N-1$ [14, 15, 16] (in contrast to higher order polynomials
as in direct transcription methods). We refer to the following optimization
problem as the ‘true discrete time-optimal control’ (DTOC) throughout the
remainder of the paper:

$\displaystyle\operatorname*{min.}_{x,u,N^{*}}\qquad$ $\displaystyle
T=N^{*}\Delta t$ (DTOC) s.t $\displaystyle
f_{\text{dyn}}(x(0),u(0),x(1),...,u(N-1),x(N))=0$ $\displaystyle
f_{\text{ter}}(x(N^{*}+1),x)=0$ $\displaystyle x(0)=x_{0}$ $\displaystyle
x(i+1),u(i)\in\Omega,\quad i=0,\dots,N-1$

$\Delta t$ is the discretization time step. $N$ is the number of collocation
points of the discrete problem. $N^{*}$ is the last time step at which
$f_{\text{ter}}(x(N^{*}),x)\neq 0$ while at the consequent step we have
$f_{\text{ter}}(x(N^{*}+1),x)=0$. The dependence $f_{\text{ter}}(x(i),x)$
indicates that $f_{\text{ter}}$ is necessarily dependent of $x(i)$ but
possibly also from $x$ (for example in case of finite differences or Euler
integration schemes). The discrete states $x$ and controls $u$ are defined as

$\displaystyle
x\coloneqq\begin{bmatrix}x(1)^{T}&\cdots&x(N)^{T}\end{bmatrix}^{T}$
$\displaystyle
u\coloneqq\begin{bmatrix}u(0)^{T}&\cdots&u(N-1)^{T}\end{bmatrix}^{T}$

The initial state is given by $x(0)=x_{0}\in\mathbb{R}^{n_{x}}$.

DTOC can be solved by means of $\ell_{0}$ optimization. However, due to its
combinatorial nature an approximation based on the $\ell_{1}$-norm is usually
solved instead. Different weight functions have been proposed which aim to
approximate the original $\ell_{0}$-problem as close as possible. However,
such $\ell_{1}$-problems can not be equivalently solved by least-squares
programming. The reason is that the Hessian of the Taylor approximation of the
problem is always positive-definite and not zero as required by $\ell_{1}$
programming ($\ell_{1}$ problems can be for example be solved by quadratic
programming, or specialized linear programming solvers). Least-squares
programming is a very popular tool due to its simplicity and availability of
open-source solvers. Furthermore, it allows for hierarchical optimization, a
tool that has gained considerable attention in the recent past, especially in
the context of robot control. In this work we therefore propose a NL-HLSP for
time-optimal control which is applicable both to linear and non-linear dynamic
systems and task functions.

##  III Approximate discrete time-optimal control via heaviside step-function
approximation

In this section we recast DTOC into an NL-HLSP of the following form:

$\displaystyle\min_{{z},{v}_{l}}\quad$
$\displaystyle\frac{1}{2}\left\|{v}_{l}\right\|^{2}\qquad\quad\hskip
8.0ptl=1,\dots,p$ (NL-HLSP) s.t. $\displaystyle{f}_{l}({z})\hskip
3.0pt\leqq\hskip 3.0ptv_{l}$ $\displaystyle\underline{f}_{l-1}({z})\hskip
3.0pt\leqq\hskip 3.0pt{\underline{v}}_{l-1}^{*}$

Each of the $p$ priority level contains constraints of the form $f_{l}(z)\leqq
v_{l}$. The symbol $\leqq$ indicates both equality and inequality constraints.
$v_{l}$ is a slack variable of level $l$ that is minimized in a least-squares
sense while being subject to the constraints of the levels $1$ to $l$ while
not increasing the already identified optimal slacks
$\underline{v}_{l-1}^{*}=\begin{bmatrix}v_{1}^{*T}&\cdots&v_{l-1}^{*T}\end{bmatrix}^{T}$
of the previous levels $1$ to $l-1$ [34, 30]. This problem can be solved by
sequential hierarchical least-squares programming (S-HLSP) as proposed in [29,
35].

Concretely, the approximate discrete time-optimal control (ADTOC) as an NL-
HLSP is given in tab. I. The variable vector is chosen as
$z=\begin{bmatrix}x^{T}&u^{T}&N^{*}\end{bmatrix}$.

$l$ | $f_{l}(x,u,N^{*})\leqq v_{l}$
---|---
| with $t=0,\dots,N-1$
1 | $\begin{bmatrix}u_{t}-u_{\max}\\\ u_{\min}-u_{t}\\\ \end{bmatrix}\leq v_{1,t+1}$
1 | $f_{\text{dyn}}(x_{0},u_{0},...,u_{N-1},x_{N})=v_{1}$
2 | $\begin{bmatrix}N^{*}\Delta t\\\ w(0,N^{*})f_{\text{ter}}(x(0),x)\\\ \vdots\\\ w(N-1,N^{*})f_{\text{ter}}(x(N-1),x)\end{bmatrix}=v_{2}$
3 | Regularization of $x$, $u$, $N^{*}$
TABLE I: ADTOC

For better readability we abbreviate variables $b$ at time $t$ as
$b_{t}\coloneqq b(t)$. The ADTOC (tab. I) has three levels $p=3$. On the first
priority level we define control constraints with minimum and maximum control
values $u_{\text{min}}$ and $u_{\text{max}}$, respectively. Furthermore, the
dynamics constraints $f_{\text{dyn}}$ are defined.

The second level contains the approximation of the time-optimal control.
First, it contains the time minimization term $N^{*}\Delta t$. Furthermore,
for each discretization step $i=1,\dots,N$ we introduce the terminal
constraint $f_{\text{ter}}(x(i))$ weighted by the function

$w(i,N^{*})=h(i,N^{*})(i-N^{*}+1)^{k}$ (2)

The term $(i-N^{*}+1)^{k}$ is outlined further down this section. $h(i,N^{*})$
is a differentiable approximation of the heaviside step function [31]

$h(i,N^{*})=(0.5+0.5\tanh(k(i-N^{*})))$ (3)

The parameter $k\in\mathbb{Z}\geq 1$ regulates the steepness of the step
approximation. Specfically, for $k\rightarrow\infty$ we have
$h(i<N^{*},N^{*})=0$, $h(N^{*},N^{*})=0.5$ and $h(i>N^{*},N^{*})=1$ for some
$i\in\mathbb{R}$ and $N^{*}\in\mathbb{R}$. The same holds for the function
$w$. Instead of using the infinite limit we define a more numerical sensible
threshold $k_{\epsilon}$ as follows.

######  Definition III.1 (Limit $k_{\epsilon}$)

The limit $k_{\epsilon}$ fulfills the bracket condition
$h(i-1,i-0.5)=\epsilon$ and $h(i,i-0.5)=1-\epsilon$ for any
$i\in\mathbb{Z}_{0}$. $k_{\epsilon}$ is then given by

$k_{\epsilon}=2\text{arctanh}\left(\frac{0.5-\epsilon}{0.5}\right)$ (4)

$\epsilon$ signals a numerical threshold. Notably, $k_{\epsilon}$ is
independent of $\Delta t$.

The above ensures that unless $N^{*}$ is exactly $i+0.5$ for some
$i\in\mathbb{Z}_{\geq 0}$, $w<\epsilon$ or $w>1-\epsilon$ for $i>i-0.5$ and
$i<i-0.5$, respectively. Note that while $k_{\epsilon}$ is independent of
$\Delta t$ we have $k\rightarrow\infty$ for $\epsilon\rightarrow 0$. The
parameter $\epsilon$ therefore needs to be chosen to a reasonable value
compromising between the desired accuracy of the approximated time-optimal
control problem and numerical stability.

Let us now further explain the term $(i-N^{*}+1)^{k}$ in the weight function
$w$ by looking at the derivative of $w(i,N^{*})$ with respect to $\hat{N}^{*}$

$\displaystyle\nabla_{N^{*}}w(i,N^{*})\coloneqq\frac{\delta w}{\delta
N^{*}}(i,N^{*})$ (5) $\displaystyle=$
$\displaystyle\nabla_{N^{*}}h(i,N^{*})(i-N^{*}+1)^{k}-h(i,N^{*})(i-N^{*}+1)^{k-1}$

The derivative of the heaviside approximation is given by

$\displaystyle\nabla_{N^{*}}h(i,N^{*})=$
$\displaystyle-0.5k(1-\tanh(k(i-N^{*}))^{2})$ (6)

It can be observed that for $k=k_{\epsilon}$ the derivative
$\nabla_{N^{*}}h(i,N^{*})$ vanishes for any $i\in\mathbb{Z}\setminus N^{*}$.
In contrast, $\nabla_{N^{*}}w(i,N^{*})\neq 0$ for any
$i\in\mathbb{Z}_{>N^{*}}$ thanks to the second term of (5). This has numerical
advantages as we further explain in sec. IV-A.

Figure 1: Function and gradient values of $h(i,N^{*})$, $w(i,N^{*})$ and
$h(i,N^{*})(t-N^{*})$ for $N^{*}=5$ and $k=1$ and $k=4$.

A depiction of $w$ and $h$ and its derivatives is given in fig. 1.

It can be observed that both the time minimization term and the terminal
constraints are not separated as constraint and objective as for DTOC. This
means that we have turned the original time-optimal control problem into a
weighted optimization problem with weights $w$ on the terminal constraints
$f_{\text{ter}}$. However, we show in sec. IV that with our choice of weights
we can indeed recover DTOC in the limit $\Delta t\rightarrow 0$,
$k=k_{\epsilon}$ and for resting goal points.

The hierarchical problem formulation allows us to define the regularization
task on a separate level $l=3$ from the actual objective of finding the time-
optimal control on level 2. The above problem can also be defined as a
conventional constrained least-squares programming ($p=2$) by defining the
(often desirable) regularization task on the second level with a small weight.
However, this would negatively influence the task performance of the second
level [30].

##  IV Convergence of the approximate discrete time-optimal control problem

In this section we investigate the convergence behavior of ADTOC (tab. I).
Similarly to [3] we proceed from a numerical standpoint. We show that the true
time-optimal control $\hat{u}$ and $\hat{x}$ poses a global KKT point to tab.
I. For this we look at following simplified optimization problem by assuming
$x,u\in\Omega$ where $\Omega$ is the feasible constraint polytope with respect
to the control, state and dynamics constraints:

$\displaystyle\operatorname*{min.}_{x,u\in\\{\Omega\\},N^{*}}\qquad$
$\displaystyle\frac{1}{2}\left\|\begin{bmatrix}N^{*}\Delta t\\\
w(0,N^{*})f_{\text{ter}}(x)\\\ \vdots\\\
w(N-1,N^{*})f_{\text{ter}}(x)\end{bmatrix}\right\|^{2}$ (7)

We assume that $N>\hat{N}^{*}$ is reasonably chosen such that a feasible
solution to the time-optimal control problem exists. Therefore, in the
following we only consider the unconstrained version of (7) (we could argue
that for example controls at the limits are simply removed from the
optimization problem in the sense of null-space methods [35]). The
corresponding first order optimality conditions
$K_{x,u,N^{*}}\coloneqq\nabla_{x,u,N^{*}}\mathcal{L}=0$ derived from the
Lagrangian $\mathcal{L}\coloneqq\frac{1}{2}\|\begin{bmatrix}N^{*}\Delta
t&\cdots&w(N-1,N^{*})f_{\text{ter}}(N-1)^{T}\end{bmatrix}^{T}\|^{2}$ are

$\displaystyle\begin{bmatrix}K_{x}\\\ K_{u}\\\
K_{N^{*}}\end{bmatrix}\coloneqq\begin{bmatrix}\sum_{i=0}^{N-1}w(i,N^{*})^{2}f_{\text{ter}}(x(i))^{T}\nabla_{x}f_{\text{ter}}(x(i))\\\
\sum_{i=0}^{N-1}w(i,N^{*})^{2}f_{\text{ter}}(x(i))^{T}\nabla_{u}f_{\text{ter}}(x(i))\\\
N^{*}\Delta t^{2}+\Sigma(x,N^{*})\end{bmatrix}=0$

$\Sigma(x,N^{*})$ is defined as

$\Sigma(x,N^{*})\coloneqq\sum_{i=0}^{N-1}w(i,N^{*})\nabla_{N^{*}}w(i,N^{*})\|f_{\text{ter}}(x(i))\|^{2}$
(8)

We now proceed as follows. We first show that the solution $N^{*}$ is
contained within the horizon $0\leq N^{*}\leq N$, see sec. IV-A. This is
important for algorithm coherence since a negative $N^{*}$ would be an
irrational solution. Similarly, $N^{*}>N$ would mean that the terminal
constraint $f_{\text{ter}}$ vanishes from the optimization problem. We then
proceed by showing the convergence of ADTOC (tab. I) to DTOC in the limit and
for resting goal points, see sec. IV-B.

###  IV-A Coherent solution $0\leq N^{*}\leq N$

We can make following statement about $\Sigma(x,N^{*})$:

###### Theorem 1

The sum $\Sigma(x,N^{*})$ is negative for all $i$ for given $N^{*}$ and
$k=k_{\epsilon}$.

###### Proof:

We look at the expression

$\displaystyle\sum_{i=0}^{N-1}w(i,N^{*})\nabla_{N^{*}}w(i,N^{*})=$ (9)
$\displaystyle\sum_{i=0}^{N-1}\left(\nabla_{N^{*}}h(i,N^{*})-\frac{h(i,N^{*})^{2}}{i-N^{*}+1}\right)(i-N^{*}+1)^{2k}<0$

The first term is negative since $\nabla_{N^{*}}h(i,N^{*})<0$ for
$k=k_{\epsilon}$. Secondly, the expression $(i-N^{*}+1)^{2k-1}$ is negative
for all $i<N^{*}-1$. Since $h(i<N^{*}-1,N^{*})<\epsilon$ vanishes for
$i<N^{*}-1$ with $k_{\epsilon}$ the second expression is negative as well. ∎

We show in the evaluation section V that we achieve good convergence even with
rather large $\epsilon$ (and therefore not strict negativity of
$\Sigma(x,N^{*})$)

Importantly, due to the additional term $(i-N^{*}+1)$ in $w$ any
$f_{\text{ter}}(x(i))\neq 0$ with $i>N^{*}$ contributes to the sum
$\Sigma(x,N^{*})$. This would not be the case for the pure heaviside function
with $k=k_{\epsilon}$. Here any Newton step $K_{N^{*}}(u+\Delta u)$,
$K_{N^{*}}(x+\Delta x)$ or $K_{N^{*}}(N^{*}+\Delta N^{*})$ would not lead to
reduction of the error $w(i,N^{*})f_{\text{ter}}(x(i))\neq 0$ with $i>N^{*}$.

The reason why we do not make the expression $(i-N^{*}+1)^{k}$ fully positive
over the range $0\leq N^{*}\leq N$ (for example by $(i-N^{*}+N)$) is that we
would create a function without a crossing at $(N^{*},0.5)$ (see fig. 1 for
$h(t-N^{*})$). With our chosen value 1 we still approximate the original
desired function $h(t-N^{*})$ while not neglecting the value
$h(N^{*},N^{*})(N^{*}-N^{*}+1)=0.5$ at $i=N^{*}$.

We now make a statement about the range of the obtained solution $N^{*}$.

###### Theorem 2

The obtained $N^{*}$ is within the range $0\leq N^{*}\leq N$ for
$k=k_{\epsilon}$.

###### Proof:

The Newton step $\Delta N^{*}$ with respect to $N^{*}$ is

$\displaystyle K_{N^{*}}(N^{*}+\Delta N^{*})\approx K_{N^{*}}(N^{*})+\Delta
N^{*}\nabla_{N^{*}}K_{N^{*}}(N^{*})=$ $\displaystyle N^{*}\Delta
t^{2}+\Sigma(x,N^{*})+\Delta N^{*}(\Delta t^{2}+$ (10)
$\displaystyle\sum_{i=0}^{N}\left(\nabla_{N^{*}}w(i,N^{*})^{2}+w(i,N^{*})\nabla_{N^{*}}^{2}w(i,N^{*})\right)\|f_{\text{ter}}(x(i))\|^{2})$

For $k=k_{\epsilon}$ and $N^{*}<0$ the above sum is positive for all $i$ and
given $N^{*}$. More specifically, we have for the second derivative
$\nabla^{2}_{N^{*}}w(i,N^{*})=0.5k(1-\tanh(k(i-N^{*}))^{2})>0$ since with (5)
and $k=k_{\epsilon}$ any (higher order) derivative of $h$ is zero for $i\neq
N^{*}$. The Newton step with respect to $N^{*}$ then becomes

$\Delta N^{*}=\frac{-N^{*}\Delta t^{2}-\sum(x,N^{*})}{\sum_{i=0}^{N}\Delta
t^{2}+\nabla_{N^{*}}^{2}w(i,N^{*})}>0$ (11)

with both positive numerator (using theorem 1 for $\sum(x,N^{*})<0$) and
denominator. This means that for any negative $N^{*}$ we get a positive Newton
step $\Delta N^{*}$ until $N^{*}+\Delta N^{*}\geq 0$. For $N^{*}>N$ the sum in
(10) vanishes entirely. The resulting Newton step $\Delta N^{*}<0$ is negative
such that $N^{*}+\Delta N^{*}=N$. ∎

The latter ensures that the constraint $f_{\text{ter}}=0$ never entirely
vanishes from the optimization problem.

###  IV-B Convergence to the true discrete time-optimal control

We first make a statement about the possible set of KKT points within the
given control horizon.

###### Theorem 3

Any $0\leq N^{*}\leq N$ poses a KKT point to ADTOC (tab. I) if any
$\|f_{\text{ter}}(x(i))\|^{2}>0$, with $i\geq N^{*}$ and some $x$, poses a
feasible point to the constraint polytope $\Omega$.

###### Proof:

Since $N^{*}\Delta t^{2}>0$ and $\Sigma(x,N^{*})<0$ we get $K_{N^{*}}=0$ for
any $N^{*}$ if we can find a corresponding $x$, $u\in\Omega$ (and therefore
$\|f_{\text{ter}}(x)\|$). ∎

With this foundation let’s look at the overall convergence behavior of ADTOC
(tab. I).

###### Theorem 4

ADTOC (tab. I) represents DTOC if $\Delta t\rightarrow 0$, $k=k_{\epsilon}$
and the desired point $f_{d}$ is a resting point such that
$\|f_{\text{ter}}(i>\hat{N}^{*})\|^{2}=0$.

###### Proof:

Assume the time-optimal control $\hat{u}$ and state $\hat{x}$ such that
$\|f_{\text{ter}}(i>\hat{N}^{*})\|^{2}=0$ (which implies a resting goal point
$f_{d}$).

First, we consider the case $N^{*}\geq\hat{N}^{*}+1$. Then the sum
$\Sigma_{k_{\epsilon}}=0$ (8) vanishes for all $N^{*}\geq\hat{N}^{*}+1$ since
$\|f_{\text{ter}}(i>\hat{N}^{*})\|^{2}=0$. The resulting Newton step (10) is
negative with $\Delta N^{*}=-\Delta N^{*}<0$ and drives
$N^{*}\rightarrow\hat{N}^{*}+1$.

Secondly, we consider the case $N^{*}\leq\hat{N}^{*}$. The corresponding
Lagrangian becomes $\mathcal{L}({N}^{*})=0.5({N}^{*}\Delta
t)^{2}+\sum_{i=0}^{N^{*}}w(i,N^{*})^{2}\|f_{\text{ter}}(x(i))\|^{2}$. We now
consider the limit case $\Delta t\rightarrow 0$. We then have
$\mathcal{L}({N}^{*})=\sum_{i=0}^{N^{*}}w(i,N^{*})^{2}\|f_{\text{ter}}(x(i))\|^{2}>0$.
Furthermore, for $N^{*}=\hat{N}^{*}$ we have
$\mathcal{L}(\hat{N}^{*})=(\hat{N}^{*}\Delta t)^{2}$ and limit
$\mathcal{L}(\hat{N}^{*})\rightarrow 0$. Since $\mathcal{L}(N^{*})\geq 0$ for
all $N^{*}\in\mathbb{Z}_{\geq 0}$ this is a global minimum
$\min{\mathcal{L}}$. This means that a global KKT point is within the interval
$N^{*}\in]\hat{N}^{*},\hat{N}^{*}+1[$ since $\min{\mathcal{L}}$. Due to
theorem 3 we therefore find an optimal point $\hat{x},\hat{u}\in\Omega$ if it
exists.

Now assume a sub-optimal (s) $\hat{u}_{s}$ and $\hat{x}_{s}$ that is optimal
to $\hat{N}^{*}_{s}>\hat{N}^{*}$ with
$\|f_{\text{ter}}(i>\hat{N}^{*}_{s})\|^{2}=0$ but sub-optimal to
$\hat{N}^{*}$. We then clearly have for the sub-optimal Lagrangian
$\mathcal{L}_{s}(\hat{N}^{*}_{s})=\hat{N}^{*}_{s}\Delta
t^{2}>\mathcal{L}(\hat{N}^{*})=\hat{N}^{*}\Delta t^{2}$. This means that the
true time-optimal $\hat{N}^{*}$ poses a global optimum to ADTOC (tab. I).

∎

Note that in the above we do not make any assumptions about the structure of
$f_{\text{dyn}}$ or $f_{\text{ter}}$. Our algorithm is therefore applicable to
both linear and non-linear systems as we demonstrate in the evaluation sec. V.

##  V Evaluation

We apply our method to both a linear point-mass and a non-linear robot
manipulator control example. In order to solve the NL-HLSP’s we use the
globally converging sequential hierarchical least-squares programming solver
S-HLSP with trust-region and hierarchical step-filter proposed in [35]. It is
based on the sparse HLSP solver s-$\mathcal{N}$IPM-HLSP [36, 35]. All
gradients and Hessians of the dynamics and task functions are computed
analytically, for example according to [37, 38]. We choose $\Delta t=0.1$ s,
$N=25$ (or $\Delta t=0.01$ s with $N=100$ and $N=80$) and $k=4$ with
$\epsilon=3.4\cdot 10^{-4}$. While $\epsilon$ is relatively large (meaning
that $\Sigma(x,N^{*})$ can become positive, in contrast to theorem 1) we did
not experience any negative influence on the algorithm convergence.

$l$ | $f_{l}(x,u)\leqq v_{l}$
---|---
| with $t=0,\dots,N-1$
1 | $\begin{bmatrix}u_{t}-u_{\max}\\\ u_{\min}-u_{t}\\\ \end{bmatrix}\leq v_{1,t+1}$
1 | $f_{\text{dyn}}(x_{0},u_{0},...,u_{N-1},x_{N})=v_{1}$
2 | $f_{\text{ter}}(x(N^{*}+1),x)=v_{2}$
2 | $\begin{bmatrix}f_{\text{ter}}(x(N^{*}+1),x)\\\ \vdots\\\ f_{\text{ter}}(x(N),x)\end{bmatrix}=v_{2}$
3 | Regularization of $x$, $u$
TABLE II: DTOC (blue) and ‘padded’ DTOC (red); $N^{*}$ is fixed.

In order to confirm our results we compare our algorithm to the two
optimization problems given in tab. II. Both problems’ decision variables do
not include $N^{*}$ which is rather fixed at a chosen value. The first problem
therefore corresponds to DTOC with the terminal constraint at $N^{*}+1$. The
other optimization problem is a ‘padded’ version of DTOC with terminal
constraints on the collocation points $N^{*}+1$ to $N$. This reproduces the
behavior of ADTOC (tab. I) which can solve the original problem (DTOC)
equivalently only if the desired state $f_{d}$ is a resting goal point (see
theorem 4). Furthermore, we report the results of GPOPS [1] which implements a
direct transcription method for optimal control problems of the form (TOC).
The resulting NLP is solved by SNOPT [9].

In order to preserve sparsity we set $w$ to zero when it is close to 0 up to a
numerical threshold

$w(t,N^{*})\leftarrow\begin{cases}0&\text{if $w(t,N^{*})<10^{-20}$}\\\
w(t,N^{*})&\text{otherwise}\end{cases}$ (12)

Note that we do not cut values that are close to 1 since this would
reintroduce the discontinuity that we are trying to circumvent from the begin
with.

We display the variable $T^{*}=N^{*}\Delta t$ which indicates the optimal time
at which still $f_{\text{ter}}\neq 0$ (the time ($N^{*}+1)\Delta t$ is the
time where we first have $f_{\text{ter}}=0$).

###  V-A Point mass

Figure 2: Control $u$, states $x$ and norm of task error $\|f_{\text{ter}}\|$
for a linear point mass moving on a plane for ADTOC (tab. I)
($T^{*}_{\text{ADTOC}}$) and problem tab. II ($T^{*}_{tab.2}$).

First we apply our method on a simple point mass of $m=1$ kg moving along a
horizontal line with single-input force $u$ and state
$x=\begin{bmatrix}q&\dot{q}\end{bmatrix}$. The state $q$ is the position of
the point-mass. The state $\dot{q}$ is the point-mass velocity. The initial
state is chosen as $x_{0}=\begin{bmatrix}1&0\end{bmatrix}$. The point-mass
dynamics are linear in the state $x$ and control $u$ (using explicit Euler
integration $x_{i+1}=x_{i}+\Delta t\dot{x}_{i}$ and $m\ddot{q}_{i}=u_{i}$ with
$i=0,\dots,N-1$)

$\displaystyle f_{\text{dyn}}(i)$
$\displaystyle=x_{i+1}+\begin{bmatrix}-1&-\Delta t\\\
0&-1\end{bmatrix}x_{i}-\begin{bmatrix}0\\\ \Delta/m\end{bmatrix}u_{i}$ (13)

The control limit is chosen to $10$ N. We set our task function to
$f_{\text{ter}}=x$ with $f_{d}=0$.

The results of ADTOC (tab. I) are given in fig. 2. $N^{*}$ is identified as
$N^{*}=5.1$ for $\Delta t=0.1$ s and $N^{*}=6.2$ for $\Delta t=0.01$ s. As can
be seen from the top graph, the identified control trajectories
($T^{*}_{\text{ADTOC},\Delta t=0.1}=0.51$ and $T^{*}_{\text{ADTOC},\Delta
t=0.01}=0.62$) follow a bang-bang profile which indicates time-optimality. For
$\Delta=0.01$ s (computation time 4.5 s with 75 solver iterations, $N=100$)
the control is at the limit $-10$ N until control time 0.3 s and then at the
limit $10$ N from time 0.33 s. The control is zero from time 0.65 s onwards. A
similar but less pronounced profile is observed for $\Delta t=0.1$ s
(computation time 0.7 s with 64 solver iterations, $N=25$), seemingly
confirming our convergence theorem 4 of equivalence of ADTOC (tab. I) with
DTOC for $\Delta t\rightarrow 0$ (note that $\Delta t\rightarrow 0$ is
prohibitive due to memory limitations since $N\rightarrow\infty$).

For comparison, we give the results of GPOPS (100 transcription nodes). The
corresponding control profile is very similar to that of our method with
$\Delta t=0.01$ s but with a slightly sharper switching point from control on
the lower bound $-10$ N to the upper bound $10$ N. This leads to a sharp drop
of $\|f_{\text{ter}}\|$ at $t=0.63$ s. On the other hand, the task error for
our method $T^{*}_{\text{ADTOC},\Delta t=0.01}=0.62$ stays elevated until 0.62
s but then declines sharply to $\|f_{\text{ter}}\|\approx 10^{-11}$. For
$\Delta t\rightarrow 0$ (and therefore DTOC according to theorem 4) we would
expect a behavior similar to GPOPS with a finite jump of $\|f_{\text{ter}}\|$
to zero between the corresponding control iterations $\hat{N}^{*}$ and
$\hat{N}^{*}+1$.

Furthermore, we give the results of the problem tab. II with fixed $N^{*}=4$
and $N^{*}=5$. For $N^{*}=4$ ($T^{*}_{tab.2,\Delta t=0.1}=0.4$) the control
profile is sharper than for $T^{*}_{\text{ADTOC},\Delta t=0.1}=0.51$. The
control then drops to zero at time 0.7 s. However, from the bottom graph it
can be observed that the task error is only reduced to
$\|f_{\text{ter}}\|=0.1$. This is in contrast to the results for
$T^{*}_{tab.2,\Delta t=0.1}=0.5$ where the task error is reduced to
$\|f_{\text{ter}}\|=10^{-13}$ at time 0.6 s. However, the corresponding
control profile is very conservative and does not follow a bang-bang profile.
A good middle ground between time optimality and error reduction is identified
for ADTOC (tab. I) $T^{*}_{\text{ADTOC},\Delta t=0.1}=0.51$ (while not being
as computationally expensive as $T^{*}_{\text{ADTOC},\Delta t=0.01}=0.62$).
The task error at time 0.6 s is approximately of order
$\|f_{\text{ter}}\|=5\cdot 10^{-4}$ which can be considered sufficiently
accurate for this control application.

The task error for $T^{*}_{tab.2,\Delta t=0.1}=0.5$ declines the fastest with
$\|f_{\text{ter}}\|\approx 10^{-13}$ at time 0.6 s. However, while this is
closest to the desired solution of DTOC it does not fulfill higher order
constraints like zero acceleration or jerk at convergence. These are
implicitly fulfilled for ADTOC (tab. I) and padded DTOC with fixed $N^{*}$ in
tab. II, depending on how much bigger $N>N^{*}$. Note that this is a matter of
defining a more embracing task function $f_{\text{task}}$ including
acceleration and jerk regularization. On the other hand, due to theorem 4
requiring a resting goal point $f_{d}$, ADTOC (tab. I) is not able to achieve
the same results as DTOC with fixed $N^{*}$ (tab. II).

###  V-B Planar manipulator with two joints

Figure 3: Controls $u$, states $x$ and norm of task error $\|f_{\text{ter}}\|$
for a non-linear 2D manipulator with two joints for ADTOC (tab. I), DTOC and
padded DTOC (tab. II).

In this section we want to identify the time-optimal control for moving a 2D
planar manipulator with fixed base and two joints from its initial task space
position $f_{\text{task}}(x)=\begin{bmatrix}2&0\end{bmatrix}^{T}$ m to
$f_{d}=\begin{bmatrix}1&1\end{bmatrix}^{T}$ m. The robot’s link lengths are
given by $L_{1}=1.25$ m and $L_{2}=0.75$ m. The robot state is given by
$x=\begin{bmatrix}q_{0,0}&q_{1,0}&\dot{q}_{0,0}&\dot{q}_{1,0}&\cdots&\dot{q}_{0,T-1}&\dot{q}_{1,T-1}\end{bmatrix}$
where $q_{0}$ and $q_{1}$ are the joint angles of joint one and two,
respectively. The initial state is given by
$x_{0}=\begin{bmatrix}0&0&0&0\end{bmatrix}$ rad. Both joints are actuated by
the torques $u=\begin{bmatrix}\tau_{1}&\tau_{2}\end{bmatrix}^{T}$. The joint
torques are limited to $5$ Nm. We use the dynamics as described in [39] with
link masses $m_{1}=m_{2}=1$ kg for the Euler explicit integration scheme
$x_{i+1}=x_{i}+\Delta t\dot{x}_{i}$. Due to the more generic form of the
dynamics of DTOC compared to the continuous one (TOC) we do not rely on matrix
inversion in order to obtain an explicit formulation of the joint
accelerations $\ddot{q}_{1}$ and $\ddot{q}_{2}$ (instead we use
$f_{\text{dyn}}=M(\dot{q}_{i+1}-\dot{q}_{i})+\Delta t\\{M\ddot{q}_{i}\\}$
where $q=\begin{bmatrix}q_{1}&q_{2}\end{bmatrix}^{T}$, $M$ is the mass matrix
for example computed by the Newton-Euler equations together with the vector
$\\{M\ddot{q}_{i}\\}$ representing the forces acting on the robot).

The results are given in fig. 3. Our algorithm ADTOC (tab. I) with $\Delta
t=0.01$ s ($T^{*}_{\text{ADTOC},\Delta t=0.01}=0.66$) recovers a sharp bang-
bang control profile for both $\tau_{1}$ and $\tau_{2}$ (computation time 22 s
with 121 solver iterations, $N=80$). The control vanishes at approximately
0.72 s with $\|f_{\text{ter}}\|\approx 10^{-15}$. This is in contrast to
$\Delta t=0.1$ s ($T^{*}_{\text{ADTOC},\Delta t=0.1}=0.57$, computation time
1.5 s with 88 solver iterations, $N=25$) with the control and task error
vanishing only at approximately 0.8 s.

Next, we report the results for GPOPS with 50 collocation points. We have
$T^{*}=1.04$ s (the last time with non-zero control and task error). As can be
seen from the corresponding joint trajectory, a different inverse kinematics
solution to the boundary condition $f_{\text{ter}}=0$ is found which is less
time-optimal than the one found for our method (note that there is an infinite
number of solutions due to robot redundancy [40]). This is due to the non-
linear optimizer not finding a better (global) solution to the problem.
Nonetheless, the bang-bang control profile is time-optimal with respect to the
identified joint trajectory.

Furthermore, the results of the problem tab. II with fixed $N^{*}=5$ and
$N^{*}=6$ are given. For $N^{*}=5$ ($T^{*}_{tab.2,\Delta t=0.1}=0.5$) we have
a limit control at the upper bound 5 Nm. However, the task error at 0.6 s is
only decreased to $7\cdot 10^{-3}$. This is in contrast to the control profile
of $T^{*}_{tab.2,\Delta t=0.1}=0.6$ with the more conservatively chosen
terminal point $N^{*}=6$. There is enough time to reduce the error to $2\cdot
10^{-7}$. However, from the conservative control profile it can be concluded
that $N^{*}=6$ is not time-optimal. Unlike ADTOC (tab. I) and padded DTOC with
known $N^{*}$ (tab. II) acceleration and jerk constraints are not implicitly
fulfilled for DTOC with known $N^{*}$ (tab. II) and therefore enables quicker
task convergence. This means that the point $f_{d}$ is reached only
instantaneously at times 0.6 s and 0.7 s with divergence afterwards, as can be
seen from the non-stationary state trajectories in the second graph from the
bottom.

The results of the padded DTOC (tab. II) with fixed $N^{*}=6$ and $N^{*}=7$
follow closely the ones of our method ADTOC (tab. I) with $\Delta t=0.1$ s in
terms of controls, states and task error reduction. Thereby, the same pattern
as with the results for tab. II is observed: for too low $N^{*}=6$ a bang-bang
control profile is obtained but the task error is reduced insufficiently
($\|f_{\text{ter}}\|=5\cdot 10^{-3}$); for too high $N^{*}=7$ the task error
is reduced significantly ($\|f_{\text{ter}}\|=8\cdot 10^{-9}$) but the control
profile is not time-optimal / not a limit one.

##  VI Conclusion

In this work we have formulated, implemented and evaluated an easily
implementable NL-HLSP for approximate discrete time-optimal control. We showed
that the method corresponds to the true discrete time-optimal control problem
in the limits and for resting goal points. This behavior was confirmed in
simulations with linear and non-linear systems, recovering the bang-bang
control profiles typically seen in time-optimal control.

In future work we would like to investigate whether the current limitation to
resting goal points can be relaxed for a broader applicability of our method.
Furthermore, while we demonstrated the applicability of our method to whole-
body motion control it is limited by its computational complexity. We
therefore would like to investigate methods to reduce the computational
burden, especially with respect to large robotic systems like humanoid robots.

##  VII Acknowledgments

We would like to thank Dr. Bing Song for her insightful comments on our work.

## References

  * [1] A. V. Rao, D. A. Benson, C. Darby, M. A. Patterson, C. Francolin, I. Sanders, and G. T. Huntington, “Algorithm 902: Gpops, a matlab software for solving multiple-phase optimal control problems using the gauss pseudospectral method,” _ACM Trans. Math. Softw._ , vol. 37, no. 2, apr 2010. [Online]. Available: https://doi.org/10.1145/1731022.1731032
  * [2] M. A. Patterson and A. V. Rao, “Gpops-ii: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming,” _ACM Trans. Math. Softw._ , vol. 41, no. 1, oct 2014. [Online]. Available: https://doi.org/10.1145/2558904
  * [3] L. T. Biegler, A. M. Cervantes, and A. Wächter, “Advances in simultaneous strategies for dynamic process optimization,” _Chemical Engineering Science_ , vol. 57, no. 4, pp. 575–593, 2002. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0009250901003761
  * [4] D. E. Kirk, “Optimal control theory : an introduction,” 1970.
  * [5] R. Goebel and M. Subbotin, “Continuous time constrained linear quadratic regulator - convex duality approach,” in _Proceedings of the 2005, American Control Conference, 2005._ , 2005, pp. 1401–1406 vol. 2.
  * [6] F. Biral, E. Bertolazzi, and P. Bosetti, “Notes on numerical methods for solving optimal control problems,” _IEEJ Journal of Industry Applications_ , vol. 5, pp. 154–166, 03 2016.
  * [7] K. Shivakumar and L. T. Biegler, “Convergence rates for direct transcription of optimal control problems using collocation at radau points,” _Computational Optimization and Applications_ , vol. 41, no. 1, pp. 81–126, 09 2008.
  * [8] D. Benson, G. Huntington, T. Thorvaldsen, and A. Rao, “Direct trajectory optimization and costate estimation via an orthogonal collocation method,” _Journal of Guidance Control and Dynamics - J GUID CONTROL DYNAM_ , vol. 29, pp. 1435–1440, 08 2006.
  * [9] P. E. Gill, W. Murray, and M. A. Saunders, “Snopt: An sqp algorithm for large-scale constrained optimization,” _SIAM Journal on Optimization_ , vol. 12, no. 4, pp. 979–1006, 2002. [Online]. Available: https://doi.org/10.1137/S1052623499350013
  * [10] L. Pontryagin, _Mathematical Theory of Optimal Processes_ , ser. Classics of Soviet Mathematics. Taylor & Francis, 1987. [Online]. Available: https://books.google.com.sg/books?id=kwzq0F4cBVAC
  * [11] R. Bellman, I. Glicksberg, and O. Gross, “On the ”bang-bang” control problem,” _Quarterly of Applied Mathematics_ , vol. 14, no. 1, pp. 11–18, 1956. [Online]. Available: http://www.jstor.org/stable/43634288
  * [12] O. Von Stryk and M. Schlemmer, _Optimal Control of the Industrial Robot Manutec r3_ , 04 1998, vol. 115.
  * [13] H. Pham and Q.-C. Pham, “Time-optimal path tracking via reachability analysis,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_ , 2018, pp. 3007–3012.
  * [14] D. Chen, L. Bako, and S. Lecoeuche, “The minimum-time problem for discrete-time linear systems: A non-smooth optimization approach,” in _2012 IEEE International Conference on Control Applications_ , 2012, pp. 196–201.
  * [15] K. Flaßkamp and T. D. Murphey, “Structure-preserving local optimal control of mechanical systems,” _Optimal Control Applications and Methods_ , vol. 40, no. 2, pp. 310–329, 2019. [Online]. Available: https://onlinelibrary.wiley.com/doi/abs/10.1002/oca.2479
  * [16] A. Meduri, P. Shah, J. Viereck, M. Khadiv, I. Havoutis, and L. Righetti, “Biconmp: A nonlinear model predictive control framework for whole body motion planning,” _ArXiv_ , vol. abs/2201.07601, 2022.
  * [17] P. Belotti, C. Kirches, S. Leyffer, J. Linderoth, J. Luedtke, and A. Mahajan, “Mixed-integer nonlinear optimization,” _Acta Numerica_ , vol. 22, p. 1–131, 2013.
  * [18] T. Marcucci and R. Tedrake, “Warm start of mixed-integer programs for model predictive control of hybrid systems,” _IEEE Transactions on Automatic Control_ , vol. PP, pp. 1–1, 07 2020.
  * [19] E. J. Candes, M. B. Wakin, and S. P. Boyd, “Enhancing sparsity by reweighted l1 minimization,” 2007. [Online]. Available: https://arxiv.org/abs/0711.1612
  * [20] I. Gentilini, F. Margot, and K. Shimada, “The travelling salesman problem with neighbourhoods: Minlp solution,” _Optimization Methods and Software_ , vol. 28, no. 2, pp. 364–378, 2013. [Online]. Available: https://doi.org/10.1080/10556788.2011.648932
  * [21] T. T. Cai and A. Zhang, “Sparse representation of a polytope and recovery of sparse signals and low-rank matrices,” _IEEE Transactions on Information Theory_ , vol. 60, no. 1, pp. 122–132, 2014.
  * [22] S. Zhou, N. Xiu, Y. Wang, L. Kong, and H. Qi, “A null-space-based weighted l 1 minimization approach to compressed sensing,” _Information and Inference A Journal of the IMA_ , vol. 5, pp. 76–102, 02 2016.
  * [23] A. Dötlinger and R. M. Kennel, “Near time-optimal model predictive control using an l1-norm based cost functional,” in _2014 IEEE Energy Conversion Congress and Exposition (ECCE)_ , 2014, pp. 3504–3511.
  * [24] Y. Yang and A. M. Williamson, “Discrete minimum time control via iterative optimization and sliding window algorithm,” in _2020 59th IEEE Conference on Decision and Control (CDC)_ , 2020, pp. 1732–1737.
  * [25] J. Vaillant, A. Kheddar, H. Audren, F. Keith, S. Brossette, A. Escande, K. Bouyarmane, K. Kaneko, M. Morisawa, P. Gergondet, E. Yoshida, S. Kajita, and F. Kanehiro, “Multi-contact vertical ladder climbing with an HRP-2 humanoid,” _Autonomous Robots_ , vol. 40, no. 3, pp. 561–580, 2016.
  * [26] K. Pfeiffer, A. Escande, and A. Kheddar, “Nut fastening with a humanoid robot,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , Sep. 2017, pp. 6142–6148.
  * [27] S. Agarwal, K. Mierle, and T. C. S. Team, “Ceres Solver,” 3 2022. [Online]. Available: https://github.com/ceres-solver/ceres-solver
  * [28] A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical quadratic programming: Fast online humanoid-robot motion generation,” _The International Journal of Robotics Research_ , vol. 33, no. 7, pp. 1006–1028, 2014\.
  * [29] K. Pfeiffer, A. Escande, P. Gergondet, and A. Kheddar, “The hierarchical newton’s method for numerically stable prioritized dynamic control,” _IEEE Transactions on Control Systems Technology_ , pp. 1–14, 2023.
  * [30] K. Pfeiffer, A. Escande, and A. Kheddar, “Singularity resolution in equality and inequality constrained hierarchical task-space control by adaptive nonlinear least squares,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3630–3637, Oct 2018.
  * [31] E. Berg, _Heaviside’s Operational Calculus as Applied to Engineering and Physics_ , ser. Electrical engineering texts. McGraw-Hill Book Company, 1929. [Online]. Available: https://books.google.com.sg/books?id=SbRCAAAAIAAJ
  * [32] C. Wang and X. Qian, “Heaviside projection based aggregation in stress constrained topology optimization,” _International Journal for Numerical Methods in Engineering_ , vol. 115, 06 2017.
  * [33] S. Zhou, L. Pan, and N. Xiu, “Heaviside set constrained optimization: Optimality and newton method,” 08 2020.
  * [34] A. Escande, N. Mansard, and P.-B. Wieber, “Hierarchical Quadratic Programming: Companion report,” Tech. Rep., Nov. 2013, 10p. [Online]. Available: https://hal.archives-ouvertes.fr/hal-00970816
  * [35] K. Pfeiffer and A. Kheddar, “Sequential hierarchical least-squares programming for prioritized non-linear optimal control,” 2023. [Online]. Available: https://arxiv.org/abs/2302.11891
  * [36] K. Pfeiffer, A. Escande, and L. Righetti, “$\mathcal{N}$ipm-hlsp: An efficient interior-point method for hierarchical least-squares programs,” 2021. [Online]. Available: https://arxiv.org/abs/2106.13602
  * [37] J. Carpentier and N. Mansard, “Analytical derivatives of rigid body dynamics algorithms,” 06 2018.
  * [38] J. de Jong, A. Müller, and J. Herder, “Higher-order derivatives of rigid body dynamics with application to the dynamic balance of spatial linkages,” _Mechanism and Machine Theory_ , vol. 155, p. 104059, 2021. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S0094114X20302779
  * [39] M. Baccouch and D. Stephen, “A two-link robot manipulator: Simulation and control design,” _International Journal of Robotic Engineering_ , vol. 5, 12 2020.
  * [40] B. Siciliano and J.-J. E. Slotine, “The general framework for managing multiple tasks in high redundant robotic systems,” in _International Conference on Advanced Robotics_ , 1991, pp. 1211 – 1216 vol.2.