# Projection-based first-order constrained optimization solver for robotics

Hakan Girgin, Tobias Löw, Teng Xue and Sylvain Calinon The authors are with
the Idiap Research Institute, Martigny, Switzerland and with EPFL, Lausanne,
Switzerland.This work was supported by the State Secretariat for Education,
Research and Innovation in Switzerland for participation in the European
Commission Horizon Europe Program through the INTELLIMAN project
(https://intelliman-project.eu/, HORIZON-CL4-Digital-Emerging Grant 101070136)
and the SESTOSENSO project (http://sestosenso.eu/, HORIZON-CL4-Digital-
Emerging Grant 101070310).

###### Abstract

Robot programming tools ranging from inverse kinematics (IK) to model
predictive control (MPC) are most often described as constrained optimization
problems. Even though there are currently many commercially-available second-
order solvers, robotics literature recently focused on efficient
implementations and improvements over these solvers for real-time robotic
applications. However, most often, these implementations stay problem-specific
and are not easy to access or implement, or do not exploit the geometric
aspect of the robotics problems. In this work, we propose to solve these
problems using a fast, easy-to-implement first-order method that fully
exploits the geometric constraints via Euclidean projections, called Augmented
Lagrangian Spectral Projected Gradient Descent (ALSPG). We show that 1. using
projections instead of full constraints and gradients improves the performance
of the solver and 2. ALSPG stays competitive to the standard second-order
methods such as iLQR in the unconstrained case. We showcase these results with
IK and motion planning problems on simulated examples and with an MPC problem
on a 7-axis manipulator experiment.

##  I Introduction

Many tasks in robotics can be framed as constrained optimization problems. The
inverse kinematics (IK) problem finds a configuration of the robot that
corresponds to a desired pose in the task space while satisfying constraints
such as joint limits or center-of-mass stability. Motion planning and optimal
control determine a trajectory of configurations and/or control commands
achieving the task subject to the dynamics and the constraints of the task and
the environment over a certain time horizon. Model predictive control (MPC)
recasts the optimal control problems with shorter horizons to solve simpler
constrained optimization problems in real-time. In this work, we present a
projection-based first-order optimization method that can be implemented and
used for all these aforementioned problems.

There are many commercially available second-order solvers to address general
constrained optimization problems such as SNOPT [1], SLSQP [2], LANCELOT [3]
and IPOPT [4]. In robotics, the literature on optimization mainly focuses on
developing solvers that effectively solve each robotic problem separately. For
example, in the motion planning literature, one can find many constrained
variants of differential dynamic programming (DDP) [5] or iterative linear
quadratic regulator (iLQR) [6], TrajOpt [7], CHOMP [8] and ALTRO [9].
Furthermore, some of these solvers are not open-source and difficult to
implement, which hinders benchmarking and potential improvements. As powerful
as these solvers are, their applications for finding real-time feedback
mechanisms such as closed-loop inverse kinematics and MPC requires tuning and
adaptations of the solver. In this chapter, we address these challenges by
proposing a very simple, yet powerful solver that can be easily implemented
without having large memory requirements.

(a)

(b)

(c)

(d)

Figure 1: Projection view of inverse kinematics problem. (a) Reaching a point
(standard IK problem):
$\mathcal{C}_{\bm{p}}=\\{\bm{p}\>|\>\bm{p}=\bm{p}_{d}\\}$. (b) Reaching
under/above/on a plane (in the halfspace):
$\mathcal{C}_{\bm{p}}=\\{\bm{p}\>|\>\bm{a}^{\scriptscriptstyle\top}\bm{p}{\leq}\bm{b}\\}$.
(c) Reaching inside/outside/on a circle:
$\mathcal{C}_{\bm{p}}=\\{\bm{p}\>|\>\lVert\bm{p}-\bm{p}_{d}\rVert_{2}^{2}{\leq}r^{2}\\}$.
(d) Reaching inside/outside/on a rectangle:
$\mathcal{C}_{\bm{p}}=\\{\bm{p}\>|\>\lVert\bm{A}(\bm{p}-\bm{p}_{d})\rVert_{\infty}{\leq}L/2\\}$.
These problems can be tested online with closed-loop
controllers333https://hgirgin.github.io/IKSPG.html, created as extensions of
the _Robotics Codes from Scratch_ toolbox444https://robotics-codes-from-
scratch.github.io/.

The constraints in many of these problems are described as geometric set
primitives or their combinations (see Table I). Examples include joint angle
or velocity limits or center-of-mass stability as bounded domain sets,
avoiding/reaching geometric shapes such as spheres and convex polytopes as
hyperplane and quadric sets, friction cone constraints as second-order cone
sets. These constraints have in common that they can be formulated as
projections rather than constraints. We argue that exploiting the projection
capability of these sets instead of treating them as generic constraints in
the solvers can significantly improve the performance.

Projected gradient descent is the simplest algorithm that takes into account
these projections. Its idea is to project the gradient to have a next iterate
inside the constraint set. In the optimization literature, a first-order
projection-based solver called spectral projected gradient descent (SPG) has
emerged as an alternative [10]. SPG has been studied and applied to many
fields because of its great practical performance even compared to second-
order constrained optimization solvers [11]. Its extension to additional
arbitrary constraints has been proposed as within augmented Lagrangian methods
[12, 13, 14]. However, as the application of this idea to popular second-order
methods in robotics is not trivial [15], the usefulness of projections in the
field has been overlooked.

In this letter, we integrate the recent work of [14] into robotics
optimization problems ranging from IK to MPC by providing the most common
Euclidean projections with an additional rectangular projection. We propose an
extension with multiple projections and additional nonlinear constraints. In
particular, we provide an efficient direct-shooting optimal control
formulation of this solver to address motion planning and MPC problems.

##  II Related work

Euclidean projections and the analytical expressions to many projections can
be found in [16]. It also gives a general theory on how to project onto a
level set of an arbitrary function using KKT conditions. Extensive studies and
theoretical background on the projections and their properties can be found in
[17]. In [18], Usmanova _et al._ propose an efficient algorithm for the
projection onto arbitrary convex constraint sets and show that exploiting
projections in the optimization significantly increases the performance. In
[19], Bauschke and Koch discuss and benchmark algorithms for finding the
projection onto the intersection of convex sets. One of the main algorithms
for this is Dykstra’s alternating projection algorithm [20].

The simplest algorithm that exploits projections is the projected gradient
descent. Spectral projected gradient descent improves over this by exploiting
the curvature information via its spectral stepsizes. A detailed review on
spectral projected gradient methods is given in [13].

In [21], Torrisi _et al._ propose to use a projected gradient descent
algorithm to solve the subproblems of sequential quadratic programming (SQP).
They show that their method can solve MPC of an inverted pendulum faster than
SNOPT. Our work is closest to theirs with the differences that we use SPG
instead of a vanilla projected gradient descent to solve the subproblems of
augmented Lagrangian instead of SQP. Instead, we propose a direct way of
handling multiple projections and inequality constraints, which is not trivial
in [21].

In [22], Giftthaler and Buchli propose a projection of the update direction of
the control input onto the nullspace of the linearized constraints in iLQR.
This approach can only handle simple equality constraints (for example,
velocity-level constraints of second-order systems) and cannot treat position-
level constraints for such systems, which is a very common and practical class
of constraints in real world applications.

##  III Background

TABLE I: Projections onto bounded domain, affine hyperplane, quadric and second-order cone  | Bounds | Affine hyperplane | Quadric | Second-order cone
---|---|---|---|---
$\mathcal{C}$ | $l{\leq}x{\leq}u$ | $l{\leq}\bm{a}^{\scriptscriptstyle\top}\bm{x}{\leq}u$ | $l{\leq}\frac{1}{2}\bm{x}^{\scriptscriptstyle\top}\bm{x}{\leq}u$ | $\lVert\bm{x}\rVert{\leq}t$
$\Pi_{\mathcal{C}}$ | $\begin{cases}x&\\!\\!\text{if }l{\leq}x{\leq}u\\\ u&\\!\\!\text{if }x{>}u\\\ l&\\!\\!\text{if }x{<}l\end{cases}$ | $\begin{cases}\bm{x}&\\!\\!\text{if }l{\leq}\bm{a}^{\scriptscriptstyle\top}\bm{x}{\leq}u\\\ \bm{x}-\frac{\bm{a}(\bm{a}^{{\scriptscriptstyle\top}}\bm{x}-u)}{\lVert\bm{a}\rVert^{2}_{2}}&\\!\\!\text{if }\bm{a}^{\scriptscriptstyle\top}\bm{x}{>}u\\\ \bm{x}-\frac{\bm{a}(\bm{a}^{\scriptscriptstyle\top}\bm{x}-l)}{\lVert\bm{a}\rVert^{2}_{2}}&\\!\\!\text{if }\bm{a}^{\scriptscriptstyle\top}\bm{x}{<}l\end{cases}$ | $\begin{cases}\bm{x}&\\!\\!\text{if }l{\leq}\frac{1}{2}\bm{x}^{\scriptscriptstyle\top}\bm{x}{\leq}u\\\ \frac{\bm{x}\sqrt{2u}}{\lVert\bm{x}\rVert}&\\!\\!\text{if }\frac{1}{2}\bm{x}^{\scriptscriptstyle\top}\bm{x}{>}u\\\ \frac{\bm{x}\sqrt{2l}}{\lVert\bm{x}\rVert}&\\!\\!\text{if }l{>}\frac{1}{2}\bm{x}^{\scriptscriptstyle\top}\bm{x}\end{cases}$ | $\begin{cases}(\bm{x},t),&\\!\\!\text{if }\lVert\bm{x}\rVert{\leq}t\\\ (\bm{0},0),&\\!\\!\text{if }\lVert\bm{x}\rVert{\leq}{-}t\\\ \frac{\lVert\bm{x}\rVert+t}{2}(\frac{\bm{x}}{\lVert\bm{x}\rVert},1)&\\!\\!\text{otherwise}\end{cases}$

In this section, we motivate the use of projections in standard robotic tasks
such as hierarchical inverse kinematics and obstacle avoidance.

###  III-A Euclidean projections onto sets

The solution $\bm{x}^{*}$ to the following constrained optimization problem

$\displaystyle\min_{\bm{x}}\lVert\bm{x}-\bm{x}_{0}\rVert_{2}^{2}\quad\mbox{s.t.}\quad\bm{x}\in\mathcal{C}$
(1)

is called an Euclidean projection of the point $\bm{x}_{0}$ onto the set
$\mathcal{C}$ and is denoted as $\bm{x}^{*}{=}\Pi_{\mathcal{C}}(\bm{x}_{0})$.
This operation determines the point $\bm{x}\in\mathcal{C}$ that is closest to
$\bm{x}_{0}$ in Euclidean sense. For many sets $\mathcal{C}$,
$\Pi_{\mathcal{C}}(\cdot)$ admits analytical expressions that are given in
Table I. Even though, usually these sets are convex (e.g. bounded domains),
some nonconvex sets also admit analytical solution(s) that are easy to compute
(e.g. being outside of a sphere). Note that many of these sets are frequently
used in robotics, from joint/torque limits and avoiding spherical/square
obstacles to satisfying virtual fixtures defined in the task space of the
robot.

###  III-B Projection view of inverse kinematics

The inverse kinematics (IK) problem in robotics corresponds to finding a joint
configuration $\bm{q}^{*}$ of the robot that corresponds to a given desired
end-effector pose $\bm{p}_{d}$. Iterative procedures are developed to robustly
solve this problem considering singularities at the Jacobian level. The
success and the convergence speed of these algorithms depend on the
initialization of the problem, which is often selected as the current joint
configuration of the robot $\bm{q}_{0}$. With this view in mind, we can
express IK as a projection problem of the initial joint angles $\bm{q}_{0}$
onto a set $\mathcal{C}_{\bm{q}}$ and of the initial end-effector position
$\bm{p}_{0}$ onto a set $\mathcal{C}_{\bm{p}}$. These two sets are assumed to
be nonempty and closed sets that admit tractable and efficient projections. A
common example for $\mathcal{C}_{\bm{q}}$ is the box constraints for the joint
limits. Footnote 4 shows examples for the set $\mathcal{C}_{\bm{p}}$ with Fig.
1(a) showing an equality constraint to a desired point, Fig. 1(b) shows an
affine hyperplane constraint for virtually limiting the robot to be under/on a
plane, Fig. 1(c) and Fig. 1(d) show quadric constraints for the end-effector
to stay inside/outside or on the boundary of a circle/square. In this work, we
exploit these easy projections in a first-order optimization solver with the
claim of finding solutions faster than standard constrained optimization
problems.

##  IV Augmented Lagrangian Spectral Projected Gradient Descent for Robotics

This section gives the spectral projected gradient descent (SPG) algorithm
along with the nonmonotone line search procedure. These algorithms are easy to
implement without big memory requirements and yet result in powerful solvers.
Next, we give the augmented Lagrangian spectral projected gradient descent
(ALSPG) algorithm with extensions to general inequality constraints and
multiple projections.

###  IV-A Spectral projected gradient descent

Spectral projected gradient descent (SPG) is an improved version of a vanilla
projected gradient descent using spectral stepsizes. Its excellent numerical
results even in comparison to second-order methods have been a point of
attraction in the optimization literature [11]. SPG tackles constrained
optimization problems in the form of

$\displaystyle\min_{\bm{x}}f(\bm{x})\quad\mbox{s.t.}\quad\bm{x}\in\mathcal{C},$
(2)

by constructing a local quadratic model of the objective function

$\displaystyle f(\bm{x})$ $\displaystyle\approxeq f(\bm{x}_{k})+{\nabla
f(\bm{x}_{k})}^{\scriptscriptstyle\top}(\bm{x}-\bm{x}_{k})+\frac{1}{2\gamma_{k}}\lVert\bm{x}-\bm{x}_{k}\rVert_{2}^{2},$
$\displaystyle=\frac{1}{2\gamma_{k}}\lVert\bm{x}-(\bm{x}_{k}-\gamma_{k}\nabla
f(\bm{x}_{k}))\rVert_{2}^{2}+\text{const.},$

and by minimizing it subject to the constraints as

$\displaystyle\min_{\bm{x}}\frac{1}{2\gamma_{k}}\lVert\bm{x}-(\bm{x}_{k}-\gamma_{k}\nabla
f(\bm{x}_{k}))\rVert_{2}^{2}\quad\mbox{s.t.}\quad\bm{x}\in\mathcal{C},$ (3)

whose solution is an Euclidean projection as described in Section III-A and
given by $\Pi_{\mathcal{C}}(\bm{x}_{k}-\gamma_{k}\nabla f(\bm{x}_{k}))$. The
local search direction $\bm{d}_{k}$ for SPG is then given by

$\bm{d}_{k}=\Pi_{\mathcal{C}}(\bm{x}_{k}-\gamma_{k}\nabla
f(\bm{x}_{k}))-\bm{x}_{k},$ (4)

which is used in a nonmonotone line search (Algorithm 1) with
$\bm{x}_{k+1}=\bm{x}_{k}+\alpha_{k}\bm{d}_{k}$, to find $\alpha_{k}$
satisfying $f(\bm{x}_{k+1})\leq f_{\text{max}}+\alpha_{k}\gamma_{k}{\nabla
f(\bm{x}_{k})}^{\scriptscriptstyle\top}\bm{d}_{k}$, where
$f_{\text{max}}=\max\\{f(\bm{x}_{k-j})\>|\>0\leq j\leq\min\\{k,M-1\\}\\}$.
Nonmonotone line search allows for increasing objective values for some
iterations $M$ preventing getting stuck at bad local minima.

The choice of $\gamma_{k}$ affects the convergence properties significantly
since it introduces curvature information to the solver. Note that when
choosing $\gamma_{k}=1$, SPG is equivalent to the widely known projected
gradient descent. SPG uses spectral stepsizes obtained by a least-square
approximation of the Hessian matrix by $\gamma_{k}\bm{I}$. These spectral
stepsizes are computed by proposals

$\displaystyle\gamma_{k}^{(1)}=\frac{\bm{s}_{k}^{\scriptscriptstyle\top}\bm{s}_{k}}{\bm{s}_{k}^{\scriptscriptstyle\top}\bm{y}_{k}}\quad\text{and}\quad\gamma_{k}^{(2)}=\frac{\bm{s}_{k}^{\scriptscriptstyle\top}\bm{y}_{k}}{\bm{y}_{k}^{\scriptscriptstyle\top}\bm{y}_{k}},$
(5)

where $\bm{s}_{k}=\bm{x}_{k}-\bm{x}_{k-1}$ and $\bm{y}_{k}=\nabla
f(\bm{x}_{k})-\nabla f(\bm{x}_{k-1})$ [11]. In the case of quadratic objective
function in the form of $\bm{x}^{\scriptscriptstyle\top}\bm{Q}\bm{x}$, these
two values correspond to the maximum and minimum eigenvalues of the matrix
$\bm{Q}$. Recent developments in SPG have shown that an alternating use of
these spectral stepsizes lead to better performance. The initial spectral
stepsize can be computed by setting
$\bm{\bar{x}}_{0}=\bm{x}_{0}-\gamma_{\text{small}}\nabla f(\bm{x}_{0})$, and
computing $\bm{\bar{s}}_{0}=\bm{\bar{x}}_{0}-\bm{x}_{0}$ and
$\bm{\bar{y}}_{0}=\nabla f(\bm{\bar{x}}_{0})-\nabla f(\bm{x}_{0})$. Note that
this heuristic operation costs one more gradient computation. The final
algorithm is given by Algorithm 2.

1 Set $\beta=10^{-4}$, $\alpha=1$, $M=10$, $c={\nabla
f(\bm{x}_{k})}^{\scriptscriptstyle\top}\bm{d}_{k}$,

2 $f_{\text{max}}=\max\\{f(\bm{x}_{k-j})|0\leq j\leq\min\\{k,M-1\\}\\}$

3 while  _$f(\bm{x}_{k}+\alpha\bm{d}_{k}) >f_{\text{max}}+\alpha\beta c$_ do

4

5
$\bar{\alpha}=-0.5\alpha^{2}c\Big{(}f(\bm{x}_{k}+\alpha\bm{d}_{k})-f(\bm{x}_{k})-\alpha
c\Big{)}^{-1}$

6 if  _$0.1\leq\bar{\alpha}\leq 0.9$_ then

7 $\alpha=\bar{\alpha}$

8 else

9 $\alpha=\alpha/2$

10 end if

11

12 end while

Algorithm 1 Non-monotone line search

1 Initialize $\bm{x}_{k}$, $\gamma_{k}$ $\epsilon{=}10^{-5}$, $k{=}0$;

2 while  _$\lVert\Pi_{\mathcal{C}}(\bm{x}_{k}-\nabla
f(\bm{x}_{k}))-\bm{x}_{k}\rVert_{\infty} >\epsilon$_ do

3 Find a search direction by
$\bm{d}_{k}=\Pi_{\mathcal{C}}(\bm{x}_{k}-\gamma_{k}\nabla
f(\bm{x}_{k}))-\bm{x}_{k}$

4 Do non-monotone line search using Algorithm 1 to find $\bm{x}_{k+1}$

5

6 Update the spectral stepsize

7 $\bm{s}_{k+1}=\bm{x}_{k+1}-\bm{x}_{k}$ and $\bm{y}_{k+1}=\nabla
f(\bm{x}_{k+1})-\nabla f(\bm{x}_{k})$

8
$\gamma^{(1)}=\frac{\bm{s}_{k+1}^{\scriptscriptstyle\top}\bm{s}_{k+1}}{\bm{s}_{k+1}^{\scriptscriptstyle\top}\bm{y}_{k+1}}$
and
$\gamma^{(2)}=\frac{\bm{s}_{k+1}^{\scriptscriptstyle\top}\bm{y}_{k+1}}{\bm{y}_{k+1}^{\scriptscriptstyle\top}\bm{y}_{k+1}}$

9

10 if  _$\gamma^{(1)} <2\gamma^{(2)}$_ then

11 $\gamma_{k+1}=\gamma^{(2)}$

12 else

13 $\gamma_{k+1}=\gamma^{(1)}-\frac{1}{2}\gamma^{(2)}$

14 end if

15

16 $k=k+1$

17 end while

Algorithm 2 Spectral Projected Gradient Descent (SPG)

###  IV-B Augmented Lagrangian spectral projected gradient descent (ALSPG)

The SPG algorithm has been shown to be a powerful competition to second-order
solvers in many ways. Each iteration can be significantly cheaper than a
second-order method if a computationally efficient projection is used and
provides better directions than other first-order methods. However, SPG alone
is usually not sufficient to solve problems in robotics with complicated
nonlinear constraints. In [14], Jia _et al._ provides an augmented Lagrangian
framework to solve problems with constraints $\bm{g}(\bm{x})\in\mathcal{C}$
and $\bm{x}\in\mathcal{D}$, where $\bm{g}(\cdot)$ is a convex function,
$\mathcal{C}$ is a convex set, and $\mathcal{D}$ is a closed nonempty set,
both equipped with easy projections.

In this section, we build on the work in [14] with the extension of multiple
projections and additional general equality and inequality constraints. The
general optimization problem that we are tackling here is

$\displaystyle\min_{\bm{x}\in\mathcal{D}}f(\bm{x})\quad\mbox{s.t.}\quad\bm{g}_{i}(\bm{x})\in\mathcal{C}_{i},\quad\forall
i\in\\{1,\ldots,p\\}$ (6)

where $\bm{g}_{i}(\cdot)$ are assumed to be arbitrary nonlinear functions.
Note that even though the convergence results in [14] apply to the case when
these are convex functions and convex sets, we found in practice that the
algorithm is powerful enough to extend to more general cases. For simplicity,
we redefine the additional equality constraints as an additional set to be
projected onto with $\mathcal{C}_{y}=\\{\bm{y}\>|\>\bm{y}=\bm{0}\\}$ with
$\Pi_{\mathcal{C}_{g}}(\bm{h}(\cdot))=\bm{0}$. Also, we transform inequality
constraints to equality constraints using the proposed method in the following
Section IV-C.

We use the following augmented Lagrangian function

$\displaystyle\mathcal{L}(\bm{x},\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p})$
$\displaystyle=f(\bm{x})+$
$\displaystyle\sum_{i=1}^{p}\frac{\rho^{\mathcal{C}_{i}}}{2}\left\lVert\bm{g}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}-\Pi_{\mathcal{C}_{i}}\Big{(}\bm{g}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}\Big{)}\right\rVert_{2}^{2}$

whose derivative wrt $\bm{x}$ is given by

$\displaystyle\nabla\mathcal{L}(\bm{x},\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p})$
$\displaystyle=\nabla f(\bm{x})+$
$\displaystyle\sum_{i=1}^{p}\frac{\rho^{\mathcal{C}_{i}}}{2}\nabla\bm{g}_{i}^{\scriptscriptstyle\top}(\bm{x})\Big{(}\bm{g}_{i}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}-\Pi_{\mathcal{C}_{i}}\Big{(}\bm{g}_{i}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}\Big{)}\Big{)},$

using the property of convex Euclidean projections derivative
$\nabla\lVert\bm{g}(\bm{x})-\Pi(\bm{g}(\bm{x}))\rVert_{2}^{2}=\nabla\bm{g}(\bm{x})^{\scriptscriptstyle\top}\big{(}\bm{g}(\bm{x})-\Pi(\bm{g}(\bm{x})\big{)}$,
see [17] for details. This way, we obtain a formulation which does not need
the gradient of the projection function $\Pi_{\mathcal{C}_{i}}(\cdot)$. One
iteration of ALSPG optimizes the subproblem $\mathop{\sf
arg\,min}_{\bm{x}\in\mathcal{D}}\mathcal{L}(\bm{x},\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p})$
given $\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p}$,
and then updates these according to the next iterate. Defining the auxiliary
function
$V(\bm{x},\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}){=}\left\lVert\bm{g}(\bm{x})-\Pi_{\mathcal{C}_{i}}\Big{(}\bm{g}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}\Big{)}\right\rVert$,
the algorithm is summarized in Algorithm 3. Note that one can define and tune
many heuristics around augmented Lagrangian methods with possible extensions
to primal-dual methods. Here, we give only one possible way of implementing
ALSPG.

1 Set $\bm{\lambda}_{0}^{\mathcal{C}_{i}}{=}\bm{0}$,
$\rho_{0}^{\mathcal{C}_{i}}{=}0.1$, $k{=}0$, $\epsilon>0$

2 while
_$\lVert\Delta\mathcal{L}(\bm{x},\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p})\rVert
>\epsilon$_ do

3 $\bm{x}_{k+1}=\mathop{\sf
arg\,min}_{\bm{x}\in\mathcal{D}}\mathcal{L}(\bm{x},\\{\bm{\lambda}^{\mathcal{C}_{i}},\rho^{\mathcal{C}_{i}}\\}_{i=1}^{p})$
with SPG in Algorithm 2

4

5 foreach  _$\mathcal{C}_{i}$_ do

6
$\lambda_{k+1}^{\mathcal{C}_{i}}=\rho^{\mathcal{C}_{i}}\Big{(}\bm{g}_{i}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}-\Pi_{\mathcal{C}_{i}}\Big{(}\bm{g}_{i}(\bm{x})+\frac{\bm{\lambda}^{\mathcal{C}_{i}}}{\rho^{\mathcal{C}_{i}}}\Big{)}\Big{)}$

7

8 if
_$V(\bm{x}_{k+1},\bm{\lambda}_{k+1}^{\mathcal{C}_{i}},\rho_{k}^{\mathcal{C}_{i}})\leq
V(\bm{x}_{k},\bm{\lambda}_{k}^{\mathcal{C}_{i}},\rho_{k}^{\mathcal{C}_{i}})$_
then

9 $\rho_{k+1}^{\mathcal{C}_{i}}{=}\rho_{k}^{\mathcal{C}_{i}}$

10

11 else

12 $\rho_{k+1}^{\mathcal{C}_{i}}{=}10\rho_{k}^{\mathcal{C}_{i}}$

13

14 end if

15

16 end foreach

17

18 end while

Algorithm 3 ALSPG

###  IV-C Handling of inequality constraints

In robotics, one frequent and intuitive way of incorporating the constraints
into the optimization problem is to use soft constraints and tune the weights
until a satisfactory result is obtained. However, this approach breaks the
hierarchy of the task without any real guarantee of constraint satisfaction.
Soft constraints are obtained by transforming the hard constraint function
into a positive cost function using auxiliary functions such as the barrier
function. In this section, we propose to exploit such soft constraint
functions as hard constraints to reduce each inequality constraint to an
equality constraint, eliminating the need of using slack variables. Note that
this procedure is in line with the construction of a standard augmented
Lagrangian for inequality constraints.

Let $g_{i}(\bm{x})\leq 0$ be the $i^{\text{th}}$ inequality constraint with
$i{=}1,\dots,M$ and $g(\cdot):\mathbb{R}^{n}\to\mathbb{R}$. We define
$h_{i}(\bm{x})=\max(0,g_{i}(\bm{x})$, where
$h(\cdot):\mathbb{R}^{n}\to\mathbb{R}^{+}$. Then, the statement
$g_{i}(\bm{x}))\leq 0$ is equivalent to $h_{i}(\bm{x})=0$. Moreover, we can
generalize this statement to obtain one single equality constraint from any
number of inequality constraints in order to increase the computational speed.
This generalization is given by the theorem below.

###### Theorem 1

The statement $g_{i}(\bm{x})\leq 0,\forall i{=}1,\dots,M$ is equivalent to
$h(\bm{x})=\sum_{i=1}^{M}h_{i}(\bm{x})=0$, where
$h_{i}(\bm{x})=\max(0,g_{i}(\bm{x}))$.

###### Proof:

  1. 1.

If $g_{i}(\bm{x})\leq 0,\forall i{=}1,\dots,M$, then it is by definition that
$\sum_{i=1}^{M}h_{i}(\bm{x})=0$.

  2. 2.

Assume $\sum_{i=1}^{M}h_{i}(\bm{x})=0$ and $\exists j\quad\mbox{s.t.}\quad
g_{j}(\bm{x})>0,\quad\forall j{=}1,\dots,N<M$. Then,
$\sum_{i=1}^{M}h_{i}(\bm{x})=\sum_{j=1}^{N}h_{j}(\bm{x})=\sum_{j=1}^{N}g_{j}(\bm{x})>0$,
which contradicts the assumption.

∎

Although it seems to simplify the problem in terms of dimensions, using
Theorem 1 to compactly reduce all inequality constraints into one single
constraint would result in the loss of some information about the gradients
from each constraint in one iteration of any solver. In practice, this
presents itself as a trade-off between the number of iterations and the
computational complexity of each iteration to solve the optimization problem.

##  V Optimal Control with ALSPG

We consider the following generic constrained optimization problem

$\displaystyle\min_{\bm{x}\in\mathcal{C}_{\bm{x}},\bm{u}\in\mathcal{C}_{\bm{u}}}c(\bm{x},\bm{u})\quad\mbox{s.t.}\quad\begin{array}[]{l}\bm{x}=\bm{F}(\bm{x}_{0},\bm{u}),\\\
\bm{h}(\bm{x},\bm{u})=\bm{0},\end{array}$ (7)

where the state trajectory
$\bm{x}{=}\begin{bmatrix}\bm{x}_{1}^{\scriptscriptstyle\top},\bm{x}_{2}^{\scriptscriptstyle\top},\ldots,\bm{x}_{t}^{\scriptscriptstyle\top},\ldots,\bm{x}_{T}^{\scriptscriptstyle\top}\end{bmatrix}^{\scriptscriptstyle\top}$,
the control trajectory
$\bm{u}=\begin{bmatrix}\bm{u}_{0}^{\scriptscriptstyle\top},\bm{u}_{1}^{\scriptscriptstyle\top},\ldots,\bm{u}_{t}^{\scriptscriptstyle\top},\ldots,\bm{u}_{T-1}^{\scriptscriptstyle\top}\end{bmatrix}^{\scriptscriptstyle\top}$
and the function $\bm{F}(\cdot,\cdot)$ correspond to the forward rollout of
the states using a dynamics model
$\bm{x}_{t+1}{=}\bm{f}(\bm{x}_{t},\bm{u}_{t})$. We use a direct shooting
approach and transform Eq. 7 into a problem in $\bm{u}$ only by considering

$\displaystyle\min_{\bm{u}\in\mathcal{C}_{\bm{u}}}c(\bm{F}(\bm{x}_{0},\bm{u}),\bm{u})\quad\mbox{s.t.}\quad\begin{array}[]{l}\bm{F}(\bm{x}_{0},\bm{u})\in\mathcal{C}_{\bm{x}},\\\
\bm{h}(\bm{F}(\bm{x}_{0},\bm{u}),\bm{u})=\bm{0},\end{array}$ (8)

which is exactly in the form of Eq. 6, if
$\bm{g}_{1}(\bm{u})=\bm{F}(\bm{x}_{0},\bm{u})$ and
$\bm{g}_{2}(\bm{u})=\bm{h}(\bm{F}(\bm{x}_{0},\bm{u}),\bm{u})$. The
unconstrained version of this problem can be solved with least-square
approaches. However, assuming $\bm{x}_{t}\in\mathbb{R}^{m}$,
$\bm{u}_{t}\in\mathbb{R}^{n}$, this requires the inversion of a matrix of size
$Tn\times Tn$, whereas here we only work with the gradients of the objective
function and the functions $\bm{g}_{i}(\cdot)$. The component that requires a
special attention is $\nabla\bm{F}(\bm{x}_{0},\bm{u})$ and in particular, its
transpose product with a vector. It turns out that this product can be
efficiently computed with a recursive formula (as also described in [21]),
resulting in fast SPG iterations. Denoting
$\bm{A}_{t}=\nabla_{\bm{x}_{t}}\bm{f}(\bm{x}_{t},\bm{u}_{t})$,
$\bm{B}_{t}=\nabla_{\bm{u}_{t}}\bm{f}(\bm{x}_{t},\bm{u}_{t})$, and
$\nabla_{\bm{u}}\bm{F}(\bm{x}_{0},\bm{u})^{\scriptscriptstyle\top}\bm{y}=\bm{z}$
with
$\bm{y}=\begin{bmatrix}\bm{y}_{0},\bm{y}_{1},\ldots,\bm{y}_{t},\ldots,\bm{y}_{T-1}\end{bmatrix}$,
$\bm{z}=\begin{bmatrix}\bm{z}_{0},\bm{z}_{1},\ldots,\bm{z}_{t},\ldots,\bm{z}_{T-1}\end{bmatrix}$,
one can show that the matrix vector product
$\nabla_{\bm{u}}\bm{F}(\bm{x}_{0},\bm{u})^{\scriptscriptstyle\top}\bm{y}$ can
be written as

$\displaystyle\begin{bmatrix}\bm{B}_{0}^{\scriptscriptstyle\top}&\bm{B}_{0}^{\scriptscriptstyle\top}\bm{A}_{1}^{\scriptscriptstyle\top}&\bm{B}_{0}^{\scriptscriptstyle\top}\bm{A}_{1}^{\scriptscriptstyle\top}\bm{A}_{2}^{\scriptscriptstyle\top}&\ldots&\bm{B}_{0}^{\scriptscriptstyle\top}\prod_{t=1}^{T-1}\bm{A}_{t}^{\scriptscriptstyle\top}\\\
\bm{0}&\bm{B}_{1}^{\scriptscriptstyle\top}&\bm{B}_{1}^{\scriptscriptstyle\top}\bm{A}_{2}^{\scriptscriptstyle\top}&\ldots&\bm{B}_{1}^{\scriptscriptstyle\top}\prod_{t=2}^{T-1}\bm{A}_{t}^{\scriptscriptstyle\top}\\\
\vdots&\vdots&\vdots&\ddots&\vdots\\\
\bm{0}&\bm{0}&\bm{0}&\ldots&\bm{B}_{T-1}^{\scriptscriptstyle\top}\end{bmatrix}\begin{bmatrix}\bm{y}_{0}\\\
\bm{y}_{1}\\\ \vdots\\\ \bm{y}_{T-1}\end{bmatrix}$
$\displaystyle=\begin{bmatrix}\bm{B}_{0}^{\scriptscriptstyle\top}(\bm{y}_{0}+\bm{A}_{1}^{\scriptscriptstyle\top}\bm{y}_{1}+\bm{A}_{1}^{\scriptscriptstyle\top}\bm{A}_{2}^{\scriptscriptstyle\top}\bm{y}_{2}+\prod_{t=1}^{T-1}\bm{A}_{t}^{\scriptscriptstyle\top}\bm{y}_{T-1})\\\
\bm{B}_{1}^{\scriptscriptstyle\top}(\bm{y}_{1}+\bm{A}_{2}^{\scriptscriptstyle\top}\bm{y}_{2}+\bm{A}_{2}^{\scriptscriptstyle\top}\bm{A}_{3}^{\scriptscriptstyle\top}\bm{y}_{3}+\prod_{t=2}^{T-1}\bm{A}_{t}^{\scriptscriptstyle\top}\bm{y}_{T-1})\\\
\vdots\\\
\bm{B}_{T-2}^{\scriptscriptstyle\top}(\bm{y}_{T-2}+\bm{A}_{T-1}^{\scriptscriptstyle\top}\bm{y}_{T-1})\\\
\bm{B}_{T-1}^{\scriptscriptstyle\top}\bm{y}_{T-1}\end{bmatrix},$

where the terms in parantheses can be computed recursively backward by
$\bar{\bm{z}}_{t+1}=(\bm{y}_{t+1}+\bm{A}_{t}^{\scriptscriptstyle\top}\bar{\bm{z}}_{t})$,
$\bm{z}_{t}=\bm{B}_{t-1}^{\scriptscriptstyle\top}\bar{\bm{z}}_{t}$ and
$\bar{\bm{z}}_{T-1}=\bm{y}_{T-1}$, without having to construct the big matrix
$\nabla_{\bm{u}}\bm{F}(\bm{x}_{0},\bm{u})^{\scriptscriptstyle\top}$.

Note that when there are no constraints on the state and $\bm{h}(\cdot)=0$,
Eq. 8 can be solved directly with the SPG algorithm. We believe that SPG can
be used to solve problems with higher horizons even faster than iLQR. Fig. 2
shows a breakdown of computational times compared to the number of timesteps
for a reaching planning tasks without constraints with a 7-axis manipulator.
Here, we plotted the average convergence time in (s) for both algorithms with
5 different end positions in task space and horizons of 100, 1000, 2000, 3000
and 5000 timesteps. iLQR is implemented with dynamic programming. SPG is
implemented as detailed in the previous section. Both implementations are in
Python.

Figure 2: Comparison of iLQR and SPG in terms of convergence time evolution vs
the number of timesteps or horizon.

##  VI Convex polytope projections and linear transformations

Often, Euclidean projection problems are composed of a linear transformation
of the constraint set onto which the projection is easy, hindering the
analytical projection property of this set. ALSPG can be used directly to
solve this kind of problems in a very efficient way, still exploiting the
projection capability of the base constraint set. For example, consider a unit
second-order cone set as
$\mathcal{C}_{\text{SOC}}=\\{(\bm{z},t)\>|\>\lVert\bm{z}\rVert_{2}\leq t\\}$
and a generic second-order cone (SOC) constraint as
$\mathcal{C}{=}\\{\bm{x}\>|\>\lVert\bm{A}\bm{x}+\bm{b}\rVert_{2}\leq\bm{c}^{\scriptscriptstyle\top}\bm{x}+d\\}$.
This can be transformed to a unit second-order cone set by taking
$\bm{g}(\bm{x}){=}\begin{bmatrix}\bm{A}\bm{x}+\bm{b}&\bm{c}^{\scriptscriptstyle\top}\bm{x}+d\end{bmatrix}$
and therefore
$\mathcal{C}{=}\\{\bm{x}\>|\>\bm{g}(\bm{x})\in\mathcal{C}_{\text{SOC}}\\}$.
Then the optimization problem of projection onto a generic second-order cone,
namely, $\mathop{\sf
arg\,min}_{\bm{x}}\lVert\bm{x}-\bm{x}_{0}\rVert_{2}^{2}\quad\mbox{s.t.}\quad\lVert\bm{A}\bm{x}+\bm{b}\rVert_{2}\leq\bm{c}^{\scriptscriptstyle\top}\bm{x}+d$
can be rewritten as $\mathop{\sf
arg\,min}_{\bm{x}}\lVert\bm{x}-\bm{x}_{0}\rVert_{2}^{2}\quad\mbox{s.t.}\quad\bm{g}(\bm{x})\in\mathcal{C}_{\text{SOC}}$
and can be solved efficiently using the ALSPG algorithm with only unit second-
order cone projections, without requiring explicit derivatives of the cone
constraints.

In the case of convex polytope projections, we can even find some conditions
when the linear transformation does not break the analytical projections.
Especially for rectangular projections, which are a special case of convex
polytope projections, we can find special conditions such that we can still
find analytical expressions even if we rotate and scale the rectangles. In the
next section, we give the development and insights of convex polytope
projections as these are one of the most commonly encountered constraint types
in robotics problems such obstacle avoidance. We then explain what kind of
linear transformations can be applied to projections to preserve their
analytical projection capability.

###  VI-A Convex polytope projections

A convex polytope of $n$ sides can be described by $n$ lines with slopes
$\bm{a}_{i}$ and intercepts $b_{i}$. The inside region of this polytope (e.g.
for reaching) is given by “ _and_ ” constraints
$\mathcal{C}_{\text{polytope}}^{\text{in}}=\\{\bm{x}|\bigwedge_{i=0}^{n}\bm{a}_{i}^{\scriptscriptstyle\top}\bm{x}\leq
u_{i}\\}$ while the outside region (e.g. for obstacle avoidance) is given by
its negative statement with “ _or_ ” constraints
$\mathcal{C}_{\text{polytope}}^{\text{out}}=\\{\bm{x}|\bigvee_{i=0}^{n}\bm{a}_{i}^{\scriptscriptstyle\top}\bm{x}>l_{i}$}.
The projection onto $\mathcal{C}_{\text{polytope}}^{\text{in}}$ can be
described as a summation of $n$ hyperplane projections in ALSPG. Even though
constraints for the set $\mathcal{C}_{\text{polytope}}^{\text{out}}$ can not
be easily described in general optimization solvers, we can show that the
projection of a point $\bm{x}_{0}$ onto this set requires finding the closest
hyperplane $i$ to $\bm{x}_{0}$, then the projection outside the hyperplane
with index $i$, namely
$\Pi^{i}_{\mathcal{C}_{\text{polytope}}^{\text{out}}}(\bm{x}_{0})$. The
minimum value of the objective function of the projection becomes
$\lVert\Pi^{i}_{\mathcal{C}_{\text{polytope}}^{\text{out}}}(\bm{x}_{0})-\bm{x}_{0}\rVert$
which is equal to the distance of $\bm{x}_{0}$ to the hyperplane $i$ (one can
check this by inserting the corresponding values from Table I). This
observation makes significant simplifications for the solvers that can take
projections into account.

In this section, we give the simplifications of this idea for often-
encountered rectangular regions. The constraint of being inside a square
region, also called a box constraint, can be described by infinity norms as
the set
$\mathcal{C}_{\text{rect}}^{\text{in}}{=}\\{\bm{x}\>|\>\lVert\bm{x}\rVert_{\infty}\leq
u\\}$ represents the inside region of a square of width $u$ centered at the
origin. This is basically a compact description of 4 lines (in 2D) describing
the square, i.e., $x\leq u$, $-u\leq x$, $y\leq u$, $-u\leq y$. This
observation allows us to write down
$\mathcal{C}_{\text{rect}}^{\text{out}}{=}\\{x\>|\>l\leq\lVert\bm{x}\rVert_{\infty}\\}$
which represents the outside region of a square of width $l$ centered at the
origin. $\mathcal{C}_{\text{rect}}^{\text{in}}$ is a simple clipping operation
for $\bm{x}_{0}$ as described in Table I. However,
$\mathcal{C}_{\text{rect}}^{\text{out}}$ requires setting up the optimization
problem for the Euclidean projection and checking the KKT conditions. For
conciseness, we give here only the resulting projection. Denoting $k$ the
index where $k{=}\mathop{\sf arg\,max}_{i}\left|\bm{x}_{0,i}\right|$, the
projection onto $\mathcal{C}_{\text{rect}}^{\text{out}}$ is then given by

$\Pi_{\mathcal{C}}(\bm{x}_{0})_{j}=\begin{cases}\bm{x}_{0,j}&\\!\\!\text{if
}\bm{x}_{0,k}{\leq}l,\\\
l\operatorname{sign}{\bm{x}_{0,k}}&\\!\\!\text{otherwise}.\end{cases}$ (9)

###  VI-B Linear transformation of projections

Having stated projections for some basic geometric primitives, one may need to
apply rotation and translation operations to such shapes to exploit more
complex ones. One such example is the transformation of square projections
onto rotated and translated square regions. Considering a convex set
$\mathcal{C}=\\{\bm{x}|f(\bm{x})\leq t\\}$, one can show that the projection
onto $\mathcal{C}^{\prime}=\\{\bm{x}|f(\bm{A}(\bm{x}-\bm{x}_{c}))\leq t\\}$ is
given by
$\Pi_{\mathcal{C}^{\prime}}(\bm{x}_{0})=\bm{A}^{{\raisebox{0.60275pt}{$\scriptscriptstyle-1$}}}\Pi_{\mathcal{C}}(\bm{A}(\bm{x}_{0}-\bm{x}_{c}))+\bm{x}_{c}$,
where $\bm{A}$ is an orthogonal matrix. For creating rectangular regions, one
needs to scale each dimension of the variable, i.e., multiplying by a diagonal
matrix. Even though this does not generalize to all cases, for the rectangular
regions, one can show that $\bm{A}$ can be in the form of a multiplication of
an orthogonal matrix and a diagonal matrix. For example, while a square of
length $L$ can be described by the set
$\mathcal{C}=\\{\bm{x}|\lVert\bm{x}\rVert_{\infty}=L/2\\}$, a rectangle of
length $L$ and width $W$, which is rotated by an angle $\theta$ can be
described with the transformation matrix $\bm{A}=\bm{R}(\theta)\bm{D}$, where
$\bm{R}$ is the rotation matrix, and $\bm{D}=\operatorname{diag}(1,L/W)$.

##  VII Experiments

In this section, we perform experiments solving inverse kinematics problems,
motion planning and MPC for a task with hybrid dynamics, and motion planning
for rectangular obstacle avoidance. The motivation behind these experiments is
to show that: 1) the proposed way of solving these robotics problem can be
unconventionally faster than the second-order methods such as iLQR; and 2)
exploiting projections whenever we can, instead of leaving the constraints for
the solver to treat them as generic constraints, increases the performance
significantly.

###  VII-A Constrained inverse kinematics

A constrained inverse kinematics problem can be described in many ways using
projections. One typical way is to find a $\bm{q}\in\mathcal{C}_{\bm{q}}$ that
minimizes a cost to be away from a given initial configuration $\bm{q}_{0}$
while respecting general constraints $\bm{h}(\bm{q})=\bm{0}$ and projection
constraints $\bm{f}(\bm{q})\in\mathcal{C}_{\bm{x}}$

$\min_{\bm{q}\in\mathcal{C}_{\bm{q}}}\lVert\bm{q}-\bm{q}_{0}\rVert_{2}^{2}\quad\mbox{s.t.}\quad\begin{array}[]{l}\bm{h}(\bm{q})=\bm{0},\\\
\bm{f}(\bm{q})\in\mathcal{C}_{\bm{x}},\end{array}$ (10)

where $\bm{f}(\cdot)$ can represent entities such as the end-effector pose or
the center of mass for which the constraints are easier to be expressed as
projections onto $\mathcal{C}_{\bm{x}}$, and $\mathcal{C}_{\bm{q}}$ can
represent the configuration space within the joint limits. Footnote 4 shows a
3-axis planar manipulator with $\bm{f}(\cdot)$ representing the end-effector
position and $\mathcal{C}_{\bm{x}}$ denoting (a)
$\mathcal{C}_{\bm{x}}=\\{\bm{x}\>|\>\bm{x}{=}\bm{x}_{d}\\}$, (b)
$\mathcal{C}_{\bm{x}}=\\{\bm{x}\>|\>\bm{a}^{\scriptscriptstyle\top}\bm{x}+b{=}0\\}$,
(c) $\mathcal{C}_{\bm{x}}=\\{\bm{x}\>|\>\leq
r_{i}^{2}\leq\lVert\bm{x}-\bm{x}_{d}\rVert_{2}^{2}\leq r_{o}^{2}\\}$ and (d)
$\mathcal{C}_{\bm{x}}=\\{\bm{x}\>\>|\>\>\lVert\bm{x}-\bm{x}_{d}\rVert_{\infty,\bm{W}}\leq
L\\}$. We applied the ALSPG algorithm iteratively to obtain a reactive control
loop and we implemented it on an interactive webpage running Python, see
Footnote 4.

Talos IK: We tested our algorithm on a high dimensional (32 DoF) inverse
kinematics problem of TALOS robot (see Fig. 3(a)) subject to constraints: i)
center of mass inside a box; ii) end-effector constrained to lie inside a
sphere; and iii) foot position and orientations are given. We compared two
versions of ALSPG algorithm: 1) by casting these constraints as projections
onto $\mathcal{C}_{\bm{x}}$; and 2) by keeping all the constraints inside the
function $\bm{h}(\cdot)$ to see direct advantages of exploiting projections in
ALSPG. We ran the algorithm from 1000 different random initial configurations
for both cases and compared the number of function and Jacobian evaluations,
$n_{f}$ and $n_{j}$. For case 1), we obtained $n_{f}{=}897.64\pm 82.84$ and
$n_{j}{=}883.44\pm 81.7$, while for case 2), we obtained $n_{f}{=}6459.4\pm
3756.8$ and $n_{j}{=}3791.79\pm 1061.05$.

Robust IK: In this experiment, we would like to achieve a task of reaching and
staying in the half-space under a plane whose slope is stochastic because, for
example, of the uncertainties in the measurements of the vision system. The
constraint can be written as
$\bm{a}^{\scriptscriptstyle\top}\bm{f}(\bm{q})\leq 0$, where
$\bm{a}\sim\mathcal{N}(\bm{\mu},\bm{\Sigma})$. We can transform it into a
chance-constraint to provide some safety guarantees in a probabilistic manner.
The idea is to find a joint configuration $\bm{q}$ such that it will stay
under a stochastic hyperplane with a probability of $\eta\geq 0.5$. This
inequality can be written as a second-order cone constraint wrt
$\bm{f}(\bm{q})$ as
$\bm{\mu}^{\scriptscriptstyle\top}\bm{f}(\bm{q})+\Psi^{-1}(\eta)\lVert\bm{\Sigma}^{\frac{1}{2}}\bm{f}(\bm{q})\rVert_{2}\leq
0$, where $\Psi(\cdot)$ is the cumulative distribution function of zero mean
unit variance Gaussian variable. Defining
$\bm{g}(\bm{q})=\begin{bmatrix}(\bm{\Sigma}^{\frac{1}{2}}f(\bm{q}))^{\scriptscriptstyle\top}&\bm{\mu}^{\scriptscriptstyle\top}\bm{f}(\bm{q})\end{bmatrix}^{\scriptscriptstyle\top}$,
the optimization problem can then be defined as

$\displaystyle\min_{\bm{q}\in\mathcal{C}_{\bm{q}}}\lVert\bm{q}-\bm{q}_{0}\rVert_{2}^{2}\quad\mbox{s.t.}\quad\bm{g}(\bm{q})\in\mathcal{C}_{\text{SOC}},$
(11)

which can be solved efficiently without using second-order cone (SOC)
gradients, by using the proposed algorithm. We tested the algorithm on the
3-axis robot shown in Fig. 3(b) by optimizing for a joint configuration with a
probability of $\eta=0.8$ and then computing continuously the constraint
violation for the last 1000 time steps by sampling a line slope from the given
distribution. We obtained a constraint violation percentage of around 80%, as
expected.

(a)

(b)

Figure 3: Inverse kinematics problems solved with the proposed algorithm. (a)
Talos inverse kinematics problem with foot pose, center of mass stability (red
point inside yellow rectangular prism) and end-effector inside a (pink) sphere
constraints. (b) Robust inverse kinematics solution with
$\mathcal{C}_{\bm{p}}=\\{\bm{p}\>|\>\bm{\mu}^{\scriptscriptstyle\top}\bm{p}+\Psi^{-1}(\eta)\lVert\bm{\Sigma}^{\frac{1}{2}}\bm{p}\rVert_{2}\leq
0$}.

###  VII-B Motion planning and MPC on planar push

Non-prehensile manipulation has been widely studied as a challenging task for
model-based planning and control, with the pusher-slider system as one of the
most prominent examples (see Fig. 4). The reasons include hybrid dynamics with
various interaction modes, underactuation and contact uncertainty. In this
experiment, we study motion planning and MPC on this planar push system,
without any constraints, to compare to a standard iLQR implementation. Motion
planning convergence results for 10 different tasks are given for iLQR and
ALSPG along with means and variances in Fig. 4(b). Although iLQR seems to
converge to medium accuracy faster than ALSPG, because of the difficulties in
the task dynamics, it seems to get stuck at local minima very easily. On the
other hand, ALSPG seems to perform better in terms of variance and local
minima. We applied MPC with iLQR and ALSPG with an horizon of 60 timesteps and
stopped the MPC as soon as it reached the goal position with a desired
precision. Table II shows this comparison in terms of convergence time (s),
number of function evaluations and number of Jacobian evaluations. According
to these findings, ALSPG performs better than a standard iLQR, even when there
are no constraints in the problem.

TABLE II: Comparison of MPC with iLQR and ALSPG for planar push | iLQR | ALSPG
---|---|---
Convergence time (s) | $14.5\pm 1.3$ | $2.9\pm 0.5$
Number of function ev. | $26689.5\pm 1830.5$ | $6104.0\pm 1455.6$
Number of jacobian ev. | $225.9\pm 27.4$ | $78.4\pm 8.5$

(a) Pusher-slider system path optimized by the proposed algorithm to go from
the state $(0,0,0)$ to $(0.1,0.1,\pi/3)$. Optimal control solved with SPG
results in a smooth path for the pusher-slider system.

(b) Convergence error mean and variance plot for iLQR and ALSPG motion
planning algorithm for 10 different goal conditions starting from the same
initial positions and control commands.

Figure 4: SPG algorithm applied to a pusher-slider system.

###  VII-C Motion planning with obstacle avoidance

Obstacle avoidance problems are usually described using geometric constraints.
In robot manipulation tasks, capsules and spheres are typically used to
represent the robot and the environment, such that the shortest distance
computations and their gradients can be computed efficiently. In autonomous
parking tasks, obstacles and cars are usually described as 2D rectangular
objects. In this experiment, we take a 2D double integrator point car reaching
a target pose in the presence of rectangular obstacles (see Fig. 7). We apply
ALSPG algorithm with and without projections (ALSPG-Proj. and ALSPG-WoProj.)
to illustrate the main advantages of having an explicit projection function
over direct constraints. The main difference is without projections, the
solvers need to compute the gradient of the constraints, whereas with
projections, this is not necessary. In order to understand the differences
between first-order and second-order methods, we also compared ALSPG-Proj to
AL-SLSQP with projections (SLSQP-Proj.), which is the same algorithm except
the subproblem is solved by a second-order solver SLSQP from Scipy. The
objective function is
$c(\bm{x},\bm{u})=10^{-1}\lVert\bm{x}_{T}-\bm{x}_{T}^{\text{G}}\rVert_{2}^{2}+10^{-4}\lVert\bm{u}_{T}\rVert_{2}^{2}$.
We performed 5 experiments, each with different settings of 4 rectangular
obstacles and compared the convergence properties. The results are given in
Table III. The comparison between ALSPG-Proj. and ALSPG-WoProj. reports a
clear advantage of using projections instead of plain constraints in the
convergence properties. Although the convergence time comparison is not
necessarily fair for SPG implementations as the SLSQP solver calls C++
functions, the comparison of ALSPG-Proj. and SLSQP-Proj. shows that ALSPG-
Proj. still achieves lower convergence time.

###  VII-D MPC on 7-axis manipulator

We tested ALSPG algorithm on the MPC problem of tracking an object with box
constraints on the end-effector position of a Franka Emika robot (see Fig. 5).
An Aruco marker on the object is tracked by a camera held by another robot. In
this experiment, the goal is to show the real-time applicability of the
proposed algorithm for a constrained problem in the presence of disturbances.
In Fig. 6, the error of the constraints and the objective function is given
for a 1 min. time period of MPC with a short-horizon of 50 timesteps. Between
20s and 30s, the robot is disturbed by the user thanks to the compliant torque
controller run on the robot. We can see that the algorithm drives smoothly the
error to zero, see accompanying video.

Figure 5: MPC setup for tracking an object subject to box constraints. Figure 6: Error in the objective and the squared norm of the constraint value during 1 min execution of MPC on Franka Emika robot. Figure 7: Motion planning problem in the presence of 4 scaled and rotated rectangular obstacles. TABLE III: Comparison of motion planning for obstacle avoidance for three cases | ALSPG-Proj. | SLSQP-Proj. | ALSPG-WoProj.
---|---|---|---
Convergence time (ms) | $392.0\pm 66.2$ | $679.0\pm 260.0$ | $2780.0\pm 530.9$
Number of function ev. | $332.0\pm 60.6$ | $438.4\pm 200.7$ | $571.8\pm 109.9$
Number of jacobian ev. | $187.8\pm 34.6$ | $389.2\pm 164.3$ | $399.0\pm 93.9$

##  VIII Conclusion

In this work, we presented a fast first-order constrained optimization
framework based on geometric projections, and applied it to various robotics
problems ranging from inverse kinematics to motion planning. We showed that
many of the geometric constraints can be rewritten as a logic combination of
geometric primitives onto which the projections admit analytical expressions.
We built an augmented Lagrangian method with spectral projected gradient
descent as subproblem solver for constrained optimal control. We demonstrated:
1) the advantages of using projections when compared to setting up the
geometric constraints as plain constraints with gradient information to the
solvers; and 2) the advantages of using spectral projected gradient descent
based motion planning compared to a standard second-order iLQR algorithm
through different robot experiments.

Sample-based MPC have been increasingly popular in recent years thanks to its
fast practical implementations, despite their lack of theoretical guarantees.
In contrast, second-order methods for MPC require a lot of computational power
but with somewhat better convergence guarantees. We argue that ALSPG, being
already in-between these two methodologies in terms of these properties,
promises great future work to combine it with sample-based MPC to further
increase its advantages on both sides.

## References

  * [1] P. E. Gill, W. Murray, and M. A. Saunders, “Snopt: An sqp algorithm for large-scale constrained optimization.” _SIAM J. Optimization_ , vol. 12, no. 4, pp. 979–1006, 2002.
  * [2] D. Kraft, “A software package for sequential quadratic programming,” _Forschungsbericht- Deutsche Forschungs- und Versuchsanstalt fur Luft- und Raumfahrt_ , 1988.
  * [3] A. R. Conn, N. I. M. Gould, and Ph. L. Toint, _LANCELOT : a Fortran package for large-scale nonlinear optimization (Release A)_. Heidelberg, Berlin, New-York: Springer-Verlag, 1992.
  * [4] A. Wachter, “An interior point algorithm for large-scale nonlinear optimization with applications in process engineering,” Ph.D. dissertation, Carnegie Mellon University, 2002.
  * [5] Y. Aoyama, G. Boutselis, A. Patel, and E. A. Theodorou, “Constrained differential dynamic programming revisited,” in _Proc. IEEE Intl Conf. on Robotics and Automation (ICRA)_ , 2021, pp. 9738–9744.
  * [6] V. Sindhwani, R. Roelofs, and M. Kalakrishnan, “Sequential operator splitting for constrained nonlinear optimal control,” in _American Control Conference (ACC)_ , 2017, pp. 4864–4871.
  * [7] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel, “Finding locally optimal, collision-free trajectories with sequential convex optimization.” in _Proc. Robotics: Science and Systems (RSS)_ , vol. 9, no. 1, 2013, pp. 1–10.
  * [8] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in _Proc. IEEE Intl Conf. on Robotics and Automation (ICRA)_ , 2009, pp. 489–494.
  * [9] T. A. Howell, B. E. Jackson, and Z. Manchester, “Altro: A fast solver for constrained trajectory optimization,” in _Proc. IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS)_ , 2019, pp. 7674–7679.
  * [10] E. G. Birgin, J. Martinez, and M. Raydan, “Spectral projected gradient methods,” _Encyclopedia of Optimization_ , vol. 2, 2009.
  * [11] E. G. Birgin, J. M. Martínez, and M. Raydan, “Spectral projected gradient methods: review and perspectives,” _Journal of Statistical Software_ , vol. 60, pp. 1–21, 2014.
  * [12] R. Andreani, E. G. Birgin, J. M. Martínez, and M. L. Schuverdt, “On augmented lagrangian methods with general lower-level constraints,” _SIAM Journal on Optimization_ , vol. 18, no. 4, pp. 1286–1309, 2008.
  * [13] E. G. Birgin and J. M. Martínez, _Practical augmented Lagrangian methods for constrained optimization_. SIAM, 2014.
  * [14] X. Jia, C. Kanzow, P. Mehlitz, and G. Wachsmuth, “An augmented lagrangian method for optimization problems with structured geometric constraints,” _Mathematical Programming_ , pp. 1–51, 2022.
  * [15] M. Schmidt, E. Berg, M. Friedlander, and K. Murphy, “Optimizing costly functions with simple constraints: A limited-memory projected quasi-newton algorithm,” in _Artificial intelligence and statistics_. PMLR, 2009, pp. 456–463.
  * [16] H. H. Bauschke, “Projection algorithms and monotone operators,” Ph.D. dissertation, Theses (Dept. of Mathematics and Statistics)/Simon Fraser University, 1996.
  * [17] H. H. Bauschke, P. L. Combettes, _et al._ , _Convex analysis and monotone operator theory in Hilbert spaces_. Springer, 2011, vol. 408.
  * [18] I. Usmanova, M. Kamgarpour, A. Krause, and K. Levy, “Fast projection onto convex smooth constraints,” in _International Conference on Machine Learning_. PMLR, 2021, pp. 10 476–10 486.
  * [19] H. H. Bauschke and V. R. Koch, “Projection methods: Swiss army knives for solving feasibility and best approximation problems with halfspaces,” _Contemporary Mathematics_ , vol. 636, pp. 1–40, 2015.
  * [20] J. P. Boyle and R. L. Dykstra, “A method for finding projections onto the intersection of convex sets in hilbert spaces,” in _Advances in order restricted statistical inference_. Springer, 1986, pp. 28–47.
  * [21] G. Torrisi, S. Grammatico, R. S. Smith, and M. Morari, “A projected gradient and constraint linearization method for nonlinear model predictive control,” _SIAM Journal on Control and Optimization_ , vol. 56, no. 3, pp. 1968–1999, 2018.
  * [22] M. Giftthaler and J. Buchli, “A projection approach to equality constrained iterative linear quadratic optimal control,” in _2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids)_. IEEE, 2017, pp. 61–66.