# Designing Multi-Stage Coupled Convex Programming with
Data-Driven McCormick Envelope Relaxations for Motion Planning

Xuan Lin1∗, Min Sung Ahn1∗, and Dennis Hong1 1X. Lin, M. S. Ahn, and D. Hong
are with the Robotics and Mechanisms Laboratory, Department of Mechanical and
Aerospace Engineering, University of California, Los Angeles, CA 90095, USA.
<EMAIL_ADDRESS>Lin and M. S. Ahn contributed
equally to this work.

###### Abstract

For multi-limbed robots, motion planning with posture and force constraints
tends to be a difficult optimization problem due to nonlinearities, which also
present extended solve times. We propose a multi-stage optimization framework
with data-driven inter-stage coupling constraints to address the nonlinearity.
Both clustering and evolutionary approaches to find the McCormick envelope
relaxations are used to find the problem-specific parameters. The learned
constraints are then used in the prior stages, which provides advanced
knowledge of the following stages. This leads to improved solve times and
interpretability of the results. The planner is validated through multiple
walking and climbing tasks on a 10 kg hexapod robot.

##  I Introduction

Legged robots present a unique advantage compared to their wheeled
counterparts, where discrete contact with the environment can be made to
maneuver discontinuous, complex terrains. To fully realize this capability,
they often use motion planning to autonomously choose their footholds and plan
their body movements to avoid slipping and tumbling. Optimization-based
techniques have been exploited by researchers to resolve the motion planning
problem [1, 2, 3, 4]. The easiest way to implement optimization for motion
planning would be to include all the linear or nonlinear equations into a
single optimization problem. Unfortunately, such an approach often suffers
from time complexity [2], although recent efforts to parallelize optimizations
have shown speedups [5]. To reduce the solving time without such techniques,
one approach is to separate the entire problem into multiple stages, where
each stage only solves for a subset of the decision variables [6, 7, 8, 9].
However, if each stage independently finds solutions without considering the
subsequent stages’ constraints, the later stages may result in no feasible
solution. This issue exacerbates when the problem scales to many stages.

Figure 1: The proposed approach to find $\textbf{B}_{\mathcal{F}}$ inside the
intersection of solution set $F_{i}$’s and exclude infeasible solution set
$\overline{F}_{i}$’s. While we can finely grid the variable space and
accurately approximate $F_{i}$’s in each grid (shown in $F_{1}$ plot), the
proposed approach results in less integer variables.

This paper proposes a multi-staged trajectory optimization (TO) formulation
with inter-stage couplings learned from data. We relax the nonlinear
constraints in the later stages into convex forms, and embed them into the
prior stages, resulting in the first stage solving an approximated form of the
original problem and subsequent stages projecting the solutions back to the
nonlinear manifolds to correct the error.  While dealing with the TO problem
as a whole is complicated, the clear interpretability of the convex
optimization permits us to segment the constraints out, perform dedicated
learning, and place them back into the structure. Learning is done using both
clustering and genetic algorithm (GA), where we identify the regions that the
data populate and fit constraints around that region. The resultant
formulation is tailored to the given type of problem, and solve time and
interpretability are improved.

While the proposed method is applicable for general constraints, we tackle a
simple nonlinear constraint—the bilinear constraint—by embedding it as a
McCormick envelope in the first optimization stage. McCormick envelopes are
the best linear relaxation for bilinear constraints [10]. They are widely used
to formulate bilinear constraints into mixed-integer convex programming
(MICPs) [11], where the complete bilinear surface is divided evenly into
multiple envelopes. This way of segmentation is general but usually results in
too many integer variables and extended solving time. To reduce solve time,
previous studies learned optimal strategies with neural networks to provide
the best choice of integer variables to the MICP solver [12, 13].  However,
since the number of strategies is an exponential function of the number of
integer variables, the number of class labels quickly becomes intractable as
the problem gets more complicated. It may be more efficient to directly
conduct learning on the MICP framework to reduce the number of integer
variables before it is handed to any learning agent. Instead of evenly
dividing the bilinear surface into envelopes, we propose to adapt the
placement of envelopes to the given type of problems, resulting in a reduced
number of integer variables. Adaptive envelope tuning can be seen in mixed-
integer nonlinear programming (MINLP) [14, 15, 16] where the general idea is
to fit envelopes to the more promising regions on the solution manifold as the
problem is being solved. Our method makes use of learning to perform
adaptation before the solving process is started. In this paper, we focus on
solving the motion planning problem applied to multi-legged robot locomotion
on multiple terrains. Hardware testings are done to show feasibility. Our
contributions are:

  1. 1.

A multi-stage coupled optimization-based motion planning framework with the
later stages relaxed and embedded into the earlier stages.

  2. 2.

Learning best McCormick envelope relaxation with clustering and GA techniques
from data.

  3. 3.

Demonstration of the planning results on hardware.

##  II Problem setup

This section presents the formulations of the motion planning problem which is
to be ultimately solved.

A multi-limbed robot is assumed to make $N$ point contacts (i.e. pure contact
force, no contact moment) with the environment. We denote the contact points
with index $i$ where $i=1,...,N$. We model the environment with polygon meshes
( e.g. a triangular mesh is depicted in Fig. 2). Additionally, we assume that
mesh $i$’s vertices (corresponding to the limb $i$ in contact with the mesh)
are denoted by $\textbf{v}_{iu}$, where $u$ is the indices of the vertices,
$u=1,...,U$ and $U$ is the number of vertices for mesh $i$. The normal
direction $\textbf{n}_{i}$ can be retrieved via a perception system. If we
define $\textbf{p}^{w}_{i}$ to be the position of toe $i$ within mesh $i$ with
respect to the world frame:

$\textbf{p}^{w}_{i}=\sum_{u}p^{w}_{iu}\textbf{v}_{iu},\ \sum_{j}p^{w}_{iu}=1,\
p^{w}_{iu}\in[0,1]$ (1)

If we define $\textbf{p}^{b}_{i}$ to be the position of toe $i$ with respect
to the origin of the body coordinate system, we have:

$\textbf{p}^{w}_{i}=\textbf{p}_{COM}+\textbf{p}^{b}_{i}$ (2)

where $\textbf{p}_{COM}$ is the position of the center of mass (COM) with
respect to the origin of the global coordinate system.

Figure 2: A robot making 3 contacts with the environment, subject to gravity.
Contact planes are represented by triangular meshes with vertices
$\textbf{v}_{sj}(i,j=1,2,3)$. $\textbf{F}_{sn}$, $\textbf{F}_{sy}$,
$\textbf{F}_{sz}$ are constant vectors along the normal and two pre-defined
shear directions. The three contact points form a contact triangle.

Let the contact force on limb $i$ be denoted by $\textbf{f}_{i}$. Since
$\textbf{n}_{i}$ is known, we can define two mutually perpendicular shear
directions $\textbf{y}_{i}$ and $\textbf{z}_{i}$, which can be pre-defined for
every mesh. The three directional vectors form a local mesh coordinate frame,
and the contact force can be represented as
$\textbf{f}_{i}=\textbf{f}_{in}+\textbf{f}_{iy}+\textbf{f}_{iz}$ in the mesh
frame, where $\textbf{f}_{in}$ is the normal force, and $\textbf{f}_{iy}$ and
$\textbf{f}_{iz}$ are the shear force components. Define $\textbf{F}_{in}$,
$\textbf{F}_{iy}$, $\textbf{F}_{iz}$ as constant vectors along the normal and
two shear directions on mesh $i$, we can express the dimensional force
components using the non-dimensional force components $f_{in}$, $f_{iy}$,
$f_{iz}$ as $\textbf{f}_{ik}=f_{ik}\textbf{F}_{ik},\;\;k=n,y,z$. The total
contact force is:

$\textbf{f}_{i}=\sum_{k=n,y,z}f_{ik}\textbf{F}_{ik}$ (3)

Similarly, defining $\textbf{P}_{x}$, $\textbf{P}_{y}$, $\textbf{P}_{z}$ as
constant vectors along the 3 axes of the world frame, $\textbf{p}^{b}_{i}$ can
be expressed as:

$\textbf{p}^{b}_{i}=\sum_{j=x,y,z}p^{b}_{ij}\textbf{P}_{j}$ (4)

Both position and force characteristic quantities are chosen such that
$p^{b}_{ij}\in[-1,1]$, $f_{ik}\in[-1,1]$.

Also note that equation (3) and (4) non-dimensionalized the force and position
variables $\textbf{f}_{i}$ and $\textbf{p}^{b}_{i}$ into quantities $f_{ik}$
and $p^{b}_{ij}$. This is suggested before utilizing certain mathematical
operations to avoid unit mismatch as seen in [17].

Having set up the contact positions and forces, whole body constraints can be
imposed. The robot motion is assumed to be quasi-static, thus the robot is
always subject to the static equilibrium constraint:

$\sum_{i=1}^{N}\textbf{f}_{i}+\textbf{F}=0$ (5)

$\sum_{i=1}^{N}\textbf{p}^{b}_{i}\times\textbf{f}_{i}+\textbf{M}=0$ (6)

where F and M are known external forces (gravity G in this work) and moments.
COM is also assumed to always be at the geometric center, irrespective of the
robot limb motion.

Equation (6) introduces a bilinear term
$\textbf{p}^{b}_{i}\times\textbf{f}_{i}$. Utilizing (3) and (4), the bilinear
term can further be expressed as:

$\textbf{p}^{b}_{i}\times\textbf{f}_{i}=\sum_{j=x,y,z}\sum_{k=n,y,z}p^{b}_{ij}f_{ik}\textbf{P}_{j}\times\textbf{F}_{ik}$
(7)

To isolate the bilinear terms, let the moment variables be:

$m_{ijk}=p^{b}_{ij}f_{ik}$ (8)

where $m_{ijk}$’s are the moment components. Plugging (8) into (7), and
further back into (6) results in:

$\sum_{i=1}^{N}\sum_{j=x,y,z}\sum_{k=n,y,z}m_{ijk}\textbf{P}_{j}\times\textbf{F}_{ik}+\textbf{M}=0$
(9)

In the predominant case where legged robots are not equipped with grippers on
its toes, the point contacts with the environment are pure frictional
contacts. When the robot places its toes, the contact forces are subject to
friction cone constraints. Given (3) and defining $\mu$ as the friction
coefficient, the constraint can be written as:

$\sqrt{f_{iy}^{2}+f_{iz}^{2}}<{\mu}f_{in}$ (10)

To plan a complete quasi-static motion to the specified goal, the motion
planner generates a series of “key frames"—body COM positions, body
orientations, and footstep positions—to the goal position. The number of key
frames is pre-specified as $M$, and each key frame posture needs to satisfy
kinematics constraints. Between two consecutive key frames, step size
constraints are enforced. Similar to [3], the following constraints are used:

for $r=1,\ldots,\textit{M},$ (11)
$\displaystyle\Delta\textbf{\text@underline{p}}_{min}\leq\|\textbf{\text@underline{p}}_{COM}[r]-\textbf{\text@underline{p}}_{COM}[r-1]\|_{2}\leq\Delta\textbf{\text@underline{p}}_{max}$
$\displaystyle\Delta\textbf{\text@underline{P}}_{min}\leq\|\textbf{\text@underline{p}}_{i}[r]-\textbf{\text@underline{p}}_{i}[r-1]\|_{2}\leq\Delta\textbf{\text@underline{P}}_{max}$
$\displaystyle\Delta\underline{\Theta}_{min}\leq\|\underline{\Theta}_{b}[r]-\underline{\Theta}_{b}[r-1]\|_{2}\leq\Delta\underline{\Theta}_{max}$
$\displaystyle\|\textbf{\text@underline{p}}_{i}[r]-\textbf{\text@underline{p}}_{COM}[r]-\textbf{R}[r]\underline{v}\|_{2}\leq\Delta_{FK}$

where ${\Theta}_{b}[r]$ is body orientation,
$\Delta\textbf{\text@underline{p}}_{min}$,
$\Delta\textbf{\text@underline{p}}_{max}$,
$\Delta\textbf{\text@underline{P}}_{min}$,
$\Delta\textbf{\text@underline{P}}_{max}$, $\Delta\underline{\Theta}_{min}$,
and $\Delta\underline{\Theta}_{max}$ are bounds for the toe, body COM, and
orientation step sizes. The limb workspace is simplified into a sphere, with
$\Delta_{FK}\in\mathbb{R}$ its radius. $\underline{v}$ is the constant
shoulder vector from body COM to the first joint of the limb (depicted in Fig.
2). $\textbf{R}[r]$ is the rotation matrix as a function of
$\underline{\Theta}_{b}[r]$. We assume the body rotation angles are small ($<$
15 degrees), thus the rotation matrix can be linearized in terms of
$\underline{\Theta}_{b}[r]=[\alpha,\beta,\gamma]$ [3].

In summary, there are kinematics constraints
(LABEL:Eqn:kinematics_constraints) (convex), (1) (2) (4) (linear), static
equilibrium constraints (3) (5) (9) (linear), with additional constraints (8)
(bilinear), and friction cone constraints (10) (convex). The objective
function minimizes the distance of the planned final configuration to the goal
configuration and penalizes the step size in similar fashion as [1][3]. Let us
denote the complete problem by $P$. This problem is parametrized by $\theta$
which is the terrain geometry $\textbf{v}_{iu}$. Fig. 2 illustrates the scene
and the notations that are used in this paper. For simplicity, the decision
variables are grouped into two sets - kinematics variables
$\mathit{\Gamma}_{p}=\\{\ \textbf{\text@underline{p}}_{i}[r],\ p_{ij}[r],\
\textbf{\text@underline{p}}_{COM}[r],\ \underline{\Theta}_{b}[r]\ \\}$ and
force variables $\mathit{\Gamma}_{f}=\\{\ \textbf{f}_{i}[r],\ f_{ik}[r],\
m_{ijk}[r]\ \\}$.

A standard approach to convert bilinear constraints into MICPs is to grid the
variable space (shown in Fig. 1 with $F_{1}$) and use McCormick envelopes to
approximate the nonlinear constraint inside each grid. This approach
accurately describes solutions for any problem parameter $\theta$, but
introduces hundreds of binary variables into the optimization problem [11],
hence solving speed is slow. In this paper, we solve problem $P$ by
introducing a 2-stage convex optimization process $P_{1}\rightarrow P_{2}$.
During the first stage $P_{1}$, we approximate the bilinear constraints in
$P_{2}$ using McCormick envelopes and solve for both sets of bilinear
variables $p_{ij}$ and $f_{ik}$. In the second stage $P_{2}$, we choose to
keep one set of bilinear variables in the solution of $P_{1}$ and project the
other set onto the bilinear surface. If one set of bilinear variables are
given, the bilinear constraint (8) in $P_{2}$ is linearized. Because of the
McCormick envelope in $P_{1}$, the 2-stage planner is coupled, with the
approximation of the nonlinear constraint in $P_{2}$ embedded in $P_{1}$. This
process is shown in Fig. 3.

Figure 3: Optimization formulation of the motion planning problem

We propose to learn the best envelope to a certain type of problem, such that
the envelope formulation captures the heuristics behind the given type of
input. Since trajectory optimization solves a series of postures with
identical mathematical formulation, it is sufficient to learn the constraints
on a single posture. We generate data by solving the problem with $M=1$ using
the accurate MICP formulation [11], and collect data for bilinear variables.
We then fit McCormick envelopes around the regions where data clusters,
excluding the infeasible regions. Through this approach, the relaxation can be
made tighter with a fixed number of envelopes.

##  III Learning setup

We present the mathematical setup for the learning problem. As solutions of
similar types of problems could be densely populated in certain regions,
learning methods could be used to identify those regions to form a “tighter"
McCormick envelope. This eliminates regions in the variables’ space where
infeasible solutions are expected.

Our algorithm seeks the overlapping regions for bilinear variables
$(\textbf{p},\textbf{f})$. The position variable p to be learned is
$\textbf{p}^{b}$. We omit the superscript for simplicity. Suppose we define
problem $P(\theta_{i})$ where $\theta_{i}$ is the parameter drawn from a
distribution $G(\theta)$ and represents the terrain shape in our work. If
$P(\theta_{i})$ is feasible, we define the solution set
$X(\theta_{i})=\\{\textbf{x}|\textbf{x}=(\textbf{p},\textbf{f},\textbf{m},\textbf{y})\
\text{feasible for}\ P(\theta_{i})\\}$. $\textbf{p},\textbf{f},\textbf{m}$ are
the variables that satisfy the bilinear constraint $m_{d}=p_{d}f_{d}$ in each
dimension $d=1,...,D$, while y’s are other optimization variables. If
$P(\theta_{i})$ is infeasible, we define the infeasible solution set
$\overline{X}(\theta_{i})$ by removing the bilinear constraint from
$P(\theta_{i})$ that generates $\overline{P}(\theta_{i})$, and
$\overline{X}(\theta_{i})=\\{\textbf{x}|\textbf{x}=(\textbf{p},\textbf{f},\textbf{m},\textbf{y})\
\text{feasible for}\ \overline{P}(\theta_{i})\\}$. A McCormick envelope
relaxation of $m_{d}=p_{d}f_{d}$ [18] can be defined tightly over a pair of
lower/upper bounds $[\textbf{p}_{d}^{L},\textbf{p}_{d}^{U}]$ and
$[\textbf{f}_{d}^{L},\textbf{f}_{d}^{U}]$ that describes a rectangular region
B over $[\textbf{p}^{T},\textbf{f}^{T}]^{T}$. McCormick envelopes
$M_{c}(\textbf{B})$ satisfy the property such that
$[\textbf{p}^{T},\textbf{f}^{T}]^{T}\in\textbf{B}$ and $m_{d}=p_{d}f_{d},\
\forall d$ implies $M_{c}(\textbf{B})\leq 0$. Furthermore,
$[\textbf{p}^{T},\textbf{f}^{T}]^{T}\notin\textbf{B}\Rightarrow
M_{c}(\textbf{B})\nleq 0$. Define $S_{i}$ as the projection of $X(\theta_{i})$
onto $\textbf{p}\times\textbf{f}$ (“$\times$” is Cartesian product) subspace
giving all feasible $[\textbf{p}^{T},\textbf{f}^{T}]^{T}$ for problem
$P(\theta_{i})$. Then, $\forall(\textbf{p},\textbf{f})\in S_{i}$, $\exists$
$(\textbf{m},\textbf{y})$ such that
$(\textbf{p},\textbf{f},\textbf{m},\textbf{y})$ is feasible for
$P(\theta_{i})$. We also define the projection of infeasible solution set of
$[\textbf{p}^{T},\textbf{f}^{T}]^{T}$ as $\overline{S}_{i}$ by projecting
$\overline{X}(\theta_{i})$ onto $\textbf{p}\times\textbf{f}$ subspace.

Assume that for any feasible $P(\theta_{i})$ that generates $S_{i}$, there
exists an overlapping region: $\mathcal{S}=\cap S_{i}\neq\emptyset$. We can
find a rectangular region $\textbf{B}_{\mathcal{S}}$ inside $\mathcal{S}$ to
construct a McCormick envelope relaxation $M_{c}(\textbf{B}_{\mathcal{S}})\leq
0$, and define the 2-step optimization process $P_{1}\rightarrow P_{2}$.
$P_{1}$ is defined by replacing the biliner constraints in $P(\theta_{i})$
with the McCormick relaxation, and gives a solution
$(\widetilde{\textbf{p}},\widetilde{\textbf{f}},\widetilde{\textbf{m}},\widetilde{\textbf{y}})$.
$P_{2}$ has identical constraints as $P$, but uses
$(\widetilde{\textbf{f}},\widetilde{\textbf{y}})$ (or
$(\widetilde{\textbf{p}},\widetilde{\textbf{y}})$) from $P_{1}$ to solve for
$(\textbf{p}^{*},\textbf{m}^{*})$ (or $(\textbf{f}^{*},\textbf{m}^{*})$), as
the bilinear constraints are linearized.

######  Theorem.

If there exist a McCormick envelope $M_{c}(\textbf{B}_{\mathcal{S}})\leq 0$
that satisfies the following 3 conditions:

  1. 1.

$\forall\ \theta_{i}$ that makes $P(\theta_{i})$ feasible and generates
$S_{i}$, $\textbf{B}_{\mathcal{S}}\cap S_{i}\neq\emptyset$.

  2. 2.

$\forall\ \theta_{i}$ that makes $P(\theta_{i})$ infeasible,
$\textbf{B}_{\mathcal{S}}\cap\overline{S}_{i}=\emptyset$.

  3. 3.

(Complete Projectability) $\forall\textbf{f}\in\textbf{B}_{\mathcal{S}}$,
$\forall\textbf{y}$, (or $\forall\textbf{p}\in\textbf{B}_{\mathcal{S}}$,
$\forall\textbf{y}$) such that $(\textbf{f},\textbf{y})$ (or
$(\textbf{p},\textbf{y})$) is feasible for $P_{1}$, $(\textbf{f},\textbf{y})$
(or $(\textbf{p},\textbf{y})$) is also feasible for $P$.

Then the 2-step process $P_{1}\rightarrow P_{2}$ defined above satisfy:

  1. 1.

$P$ is feasible $\Rightarrow$ $P_{1}\xrightarrow{}P_{2}$ is feasible. In
addition, any feasible solution for $P_{1}\xrightarrow{}P_{2}$ is also
feasible for $P$.

  2. 2.

$P$ is infeasible $\Rightarrow$ $P_{1}$ is infeasible.

Note that depending on how $P_{2}$ projects the solution of $P_{1}$, the
required projectability condition (3) is different. If we keep
$(\textbf{f},\textbf{y})$ and project $(\textbf{p},\textbf{m})$, then we need
$(\textbf{f},\textbf{y})$ to be feasible for $P$, and vise versa.

###### Proof.

1) Suppose $P$ is feasible with solution subspace $S_{i}$. Since
$\textbf{B}_{\mathcal{S}}\cap S_{i}\neq\emptyset$,
$\exists[\textbf{p}_{i}^{T},\textbf{f}_{i}^{T}]^{T}\in{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\textbf{B}_{\mathcal{S}}\cap{S_{i}}}$.
$[\textbf{p}_{i}^{T},\textbf{f}_{i}^{T}]^{T}\in S_{i}\Rightarrow$
$\exists(\textbf{m}_{i},\textbf{y}_{i})$ such that
$\textbf{x}=(\textbf{p}_{i},\textbf{f}_{i},\textbf{m}_{i},\textbf{y}_{i})$ is
feasible for $P$ (satisfying each constraint except
$M_{c}(\textbf{p}_{i},\textbf{f}_{i},\textbf{m}_{i})\leq 0$). In particular,
$(\textbf{p}_{i},\textbf{f}_{i},\textbf{m}_{i})$ satisfies the bilinear
constraint $m_{d}=p_{d}f_{d}$, $\forall d$. This together with
$[\textbf{p}_{i}^{T},\textbf{f}_{i}^{T}]^{T}\in\textbf{B}_{\mathcal{S}}$
implies $M_{c}(\textbf{p}_{i},\textbf{f}_{i},\textbf{m}_{i})\leq 0$. Thus $P$
is feasible $\Rightarrow P_{1}$ is feasible.
Now we show that $P_{1}$ is feasible $\Rightarrow$ $P_{2}$ is feasible, and
the solution of $P_{1}\xrightarrow{}P_{2}$ is feasible for $P$. Any feasible
solution $(\textbf{p},\textbf{f},\textbf{m},\textbf{y})$ for $P_{1}$ satisfies
$M_{c}(\textbf{p},\textbf{f},\textbf{m})\leq
0\Rightarrow(\textbf{p},\textbf{f})\in\textbf{B}_{\mathcal{S}}$. By condition
(3), $(\textbf{f},\textbf{y})$ (or $(\textbf{p},\textbf{y})$) is feasible for
$P$, which means $\exists(\widetilde{\textbf{p}},\widetilde{\textbf{m}})$ (or
$(\widetilde{\textbf{f}},\widetilde{\textbf{m}})$) such that
$\widetilde{\textbf{x}}=(\widetilde{\textbf{p}},\textbf{f},\widetilde{\textbf{m}},\textbf{y})$
(or $(\textbf{p},\widetilde{\textbf{f}},\widetilde{\textbf{m}},\textbf{y})$)
satisfies $P$. Since $P_{2}$ has identical constraints as $P$,
$\widetilde{\textbf{x}}$ is feasible for $P_{2}$. As $P_{2}$ is feasible and
has identical constraints as $P$, any solution for $P_{2}$ is feasible for
$P$.
2) By condition (2), $\textbf{B}_{\mathcal{S}}\cap\overline{S}_{i}=\emptyset$,
$\forall[\textbf{p}^{T},\textbf{f}^{T}]^{T}\in\overline{S}_{i}$ (satisfies all
but the bilinear constraints),
$[\textbf{p}^{T},\textbf{f}^{T}]^{T}\notin\textbf{B}_{\mathcal{S}}$, thus
$M_{c}({\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\textbf{p}},\textbf{{f}},\forall\textbf{m})\nleq
0$ $\Rightarrow$ $P_{1}$ is infeasible. ∎

######  Remark.

It may look like result 2) is too strong. Normally, a relaxation is feasible
doesn’t guarantee that the original problem is feasible. However, in this case
it does. Basically, 2) shifts the region of relaxation on (p, f) completely
away from the danger zone \- region that potentially makes infeasible problems
feasible. The relaxation will admit new m points. However, no matter what m
is, since (p, f) is infeasible for the original problem, (p, f, m, y) wll not
be feasible. A good example is that, for some problem,
$\textbf{m}=\textbf{p}\textbf{f}$ does not constraint anything. This means
whenever $\theta_{i}$ is infeasible for $P$, $\overline{P}$ is still
infeasible. In this case, the envelope can be all the domain, as other
constraints for $P$ are already making the problem infeasible.

The idea behind finding $\textbf{B}_{\mathcal{S}}$ is shown in Fig. 1.
Condition (3) guarantees projectability for any point in
$\textbf{B}_{\mathcal{S}}$. Strictly satisfying it guarantees $P_{1}$ will not
give solutions that cause $P_{2}$ to be infeasible. In practice, verifying
condition (3) is very difficult. We put additional safety factors to make
constraints in $P_{1}$ even tighter to reduce the the relaxation (at the risk
of $P_{1}$ not a complete relaxation of $P$). As an simple extension of this
paper, one can also learn an overlapping region of $y_{i}$’s and formulate as
an additional constraint into $P_{1}$. This guarantees that $P_{1}$ does not
give any “strange” $y_{i}$ that makes $P_{2}$ infeasible. Another approach is
to set an optimal criteria for $P_{1}$ and guarantee that the optimal solution
of $(\textbf{f},\textbf{y})$ is always projectable to $P_{2}$ [19]. If the
projection still fails with the above efforts, the problem is over-relaxed,
suggesting that the envelope should be divided into multiple smaller pieces.
Interesting future work remains where an objective function that guarantees
projectability at optimal points could potentially exist. In addition, if we
do not confine the formulation to convex ones, we can use nonlinear
optimization (NLPs) in $P_{2}$ to do the projection. In this case, we only
keep $y_{i}$ from $P_{1}$ and project $(\textbf{p},\textbf{y})$
simultaneously. Properly initialized NLPs can have fast speed with a solvable
rate close to 100% [11]. Finally, even if $\cap S_{i}=\emptyset$, we can still
identify multiple mutually exclusive $\textbf{B}_{\mathcal{S}}$’s that give
multiple envelopes. An example is shown in the next section.

Algorithm 1 $\operatorname{DataGeneration}$

Input Number of samples $N$

1: Initialize feasible solution set $S$ and infeasible solution set
$\overline{S}$

2: while $i$ < $N$ do

3: Sample $\theta_{i}\sim G(\theta)$

4: Solve $P(\theta_{i})$ with high precision MICP

5: if ($\textbf{p}_{i}$, $\textbf{f}_{i}$) is a feasible solution for
$P(\theta_{i})$ then

6: Add ($\textbf{p}_{i}$, $\textbf{f}_{i}$) to $S$

7: else

8: Remove bilinear constraint (8) to produce problem
$\overline{P}(\theta_{i})$

9: Solve $\overline{P}(\theta_{i})$

10: if ($\overline{\textbf{p}}_{i}$, $\overline{\textbf{f}}_{i}$) is a
feasible solution for $\overline{P}(\theta_{i})$ then

11: Add ($\overline{\textbf{p}}_{i}$, $\overline{\textbf{f}}_{i}$) to
$\overline{S}$

12: Increase $i$

13: return $S$, $\overline{S}$

We present two data-based approaches to identify $\textbf{B}_{\mathcal{S}}$,
one based on clustering to directly fit envelopes and the other one based on
evolutionary algorithms.

####  III-1 Clustering Approach

Based on the high-accuracy MICP formulation [11], we can generate feasible
solutions or prove infeasibility for problems $P(\theta_{i})$ sampled from
$G(\theta)$. We recognize that if we sample the same amount of feasible
solutions inside each $S_{i}$, the overlapping region $\mathcal{S}$ will
receive more samples, indicating a clustering approach may identify
$\mathcal{S}$. We also need to draw samples from the infeasible solution set
$\overline{S}_{i}$ and make the envelope exclude those points. The data
generation algorithm is shown in Algorithm 1, where we generate a random
terrain from a pre-defined distribution and collect the subsequent
optimization’s solutions depending on their feasibilities.

Having collected the data in $S$ and $\overline{S}$, we use Algorithm 2 to fit
the envelopes. With a specified number of envelopes, the algorithm first
performs clustering with Gaussian Mixture Models (GMMs) to identify the center
of clusters. Then, an optimization problem named Boundary_fit is solved to fit
the largest rectangular region $B$ around the centers excluding any points in
$\overline{S}_{i}$. Multiple formulations can be used to achieve this. We used
a formulation based on mixed-integer programming, but other options exist
[20]. We then collect all points in $B$ but remove those whose probability of
belonging to the cluster is less than the threshold $p_{th}$. The exterior
boundary formed by the remaining points gives $\textbf{B}_{\mathcal{S}}$.

Algorithm 2 $\operatorname{EnvelopeFitting}$

Input Threshold probability $p_{th}$, Number of clusters $n$, DataGeneration’s
$S$, $\overline{S}$

1: Initialize dictionary $\textbf{B}_{\mathcal{S}}$

2: for $k=1,...,n$ do

3: Get cluster means ($p_{k}$, $f_{k}$) = GMM($S$)

4: $B_{k}$ = Boundary_fit(($p_{k}$, $f_{k}$), $\overline{S}$)

5: $S_{k}$ = points in $S$ that are in $B_{k}$

6: $S^{o}_{k}$ = GMM_filter($p>p_{th}$)

7: $\textbf{B}_{\mathcal{S}\\_k}$ = get_exterior_boundary($S^{o}_{k}$)

8: Add $\textbf{B}_{\mathcal{S}\\_k}$ to $\textbf{B}_{\mathcal{S}}$

9: return $\textbf{B}_{\mathcal{S}}$

####  III-2 Evolutionary Approach

Contrasting to the fitting approach, a sampling-based evolutionary approach
could, by nature, find bounds that are even tighter. Using a sufficient number
of random terrain (input) and feasibility (output) pairs, a genetic algorithm
(GA) can be tailored to solve a bilevel optimization that is indicative of the
original problem at hand. While mostly following the conventional GA approach,
we choose our chromosomes to be the lower and upper bounds of the envelope,
while uniform crossover is done per lower/upper bound pair as opposed to per
gene value. We design the fitness function to be representative of the bilevel
optimization that occurs between the two stages of the original problem. To
achieve this, we define two key metrics that help in finding better envelopes.
We define $a$ to be the percentage of original infeasible values becoming
feasible, and $b$ to be the percentage of original feasible values becoming
infeasible. At each solution’s fitness calculation, a random pair of terrain
parameter and ground truth feasibility $z_{g}$ are selected from a pre-
generated dataset built using the data generator. Then, the individual and the
terrain parameter are used to find the feasibility $z_{1}$ in $P_{1}$. If
$z_{g}$ is infeasible while $z_{1}$ is feasible, $a$ increases, whereas if
$z_{g}$ is feasible but $z_{1}$ is infeasible, $b$ increases. Per generation,
each individual is tested against $K$ number of terrains from the training set
and the average $a$ and $b$ are used in the calculation of the fitness
function. While we try to minimize $a$, we keep $b$ below a certain threshold
$\delta$. To ensure that the number of mutations also decreases over the
generations and that the variance of the population’s fitness decreases, the
mutation rate is set to be a function of the generation number decreasing over
time.

##  IV Experiment

###  IV-A Training

To validate our proposed algorithm, we choose the random distribution $\theta$
to provide two different kinds of terrains—ground and wall—whose
$(\textbf{p},\textbf{f})$ are expected to show distinct distributions. For
training, we provide one terrain mesh to each leg, while varying terrain
position, orientation, and friction coefficient $\mu$ randomly. For ground
data, we uniformly sample the angle of normal vectors within the $None$
$\textdegree$ region around the straight-up direction, while varying $\mu$
between [0.1, 0.8]. The wall data are collected to plan trajectories for the
robot to climb up between two walls [21] with pure frictional contact. We vary
the angles within the $None$ region around the nominal direction as shown in
the top right of Fig. 5, and vary $\mu$ between [0.1, 1.2]. We use Algorithm 1
to gather a set $S$ of 500 feasible points and 500 infeasible points for
envelope fitting, and another set $\overline{S}$ of the same number for
validation. We set the number of envelopes to be 1. Both ground and wall
envelopes are fit with 3 legs (two right, one left). Envelopes are also fitted
with 5 legs on the wall. For validation, we separate the success rate into two
categories showing respectively if our convex optimization can identify
feasible solutions correctly and identify infeasible solutions correctly. We
show the results of both stages. If $\textbf{B}_{\mathcal{S}}\in S_{i}$,
$P_{1}$ should remain feasible if $P$ is feasible. If condition (2) in our
theorem holds, $P_{1}$ should be infeasible if $P$ is infeasible. If condition
(3) holds, the feasible solutions for $P_{1}$ should be projectable to make
$P_{2}$ feasible, thus success rate for feasible solutions should not drop
from $P_{1}$ to $P_{2}$. We use both convex and NLP methods mentioned in the
previous section for $P_{2}$. For convex method, we use $\textbf{p}^{b}$ from
$P_{1}$ to solve f in $P_{2}$. The NLP method solves $\textbf{p}^{b}$ and f
together.

Expert human heuristics are used to create envelope parameters as baselines.
Bounds for $\textbf{p}^{b}$ are measured outer boundaries of each leg’s
workspace, while that for f are from our understanding of force profiles.
Normal force bounds are $[0.0,0.9]$/$[0.0,0.5]$ for climbing/walking, as large
normal forces are expected to prevent slipping. Shear force bounds are
$[-0.3,0.3]$/$[-0.15,0.15]$ for climbing/walking. For wall climbing the
vertical shear force boundeds are $[0.0,0.4]$ as they need to counteract the
gravity. We perform hand-tuning to optimize the performance. The results are
in Table I.

The results show that $P_{1}$ tends to match $P$ well for all test cases when
using clustering, as both success rates are close to 100%. For convex
projection, about 20%$\sim$30% of $P_{1}$’s solutions cannot be projected for
3 leg case. This is due to a violation of condition (3) indicating a single
envelope may over-relax the original problem. For the 5 leg test, both stages
perform well. Intuitively, since the problem dimension is higher, p has more
room to adjust at $P_{2}$. NLP projections perform well, with the rate of
projection close to $100\%$ and solving speed no more than a few hundred
milliseconds.

Aside from training a single cluster, we tried to fit envelopes with combined
ground and wall data. By giving $n=2$ in Algorithm 2, two mutually exclusive
envelopes representing the ground and the wall are identified. This results in
an MICP formulation with one binary variable $z\in\\{0,1\\}$ per posture that
switches between 2 modes as the robot traverses from one type of terrain to
the other.

TABLE I: Validation results for trained envelopes

Problem | $P_{1}$ | $P_{1}\rightarrow P_{2}$ |  $P_{1}\rightarrow P_{2}$ (NLP)
---|---|---|---
| correct
---
feasible
| correct
---
infeasible
| correct
---
feasible$*$
| correct
---
feasible
3 leg ground | Cluster | 98.16% | 77.78% | 83.54% | 98.16%
GA | 70.08% | 91.24% | 37.40% | 70.08%
Heuristic | 99.77% | 50.00% | 84.91% | 99.77%
3 leg wall | Cluster | 93.62% | 80.00% | 63.32% | 93.62%
GA | 77.85% | 97.72% | 67.07% | 77.85%
Heuristic | 78.72% | 51.33% | 60.75% | 78.72%
5 leg wall | Cluster | 95.27% | 79.31% | 90.41% | 93.49%
GA | 68.21% | 98.68% | 62.70% | 68.21%
Heuristic | 92.90% | 72.41% | 88.10% | 92.25%

  * *

Correct infeasible results for $P_{2}$ are identical to $P_{1}$ thus omitted.

Figure 4: The average fitness of the population is shown with its variance.
Certain individuals maximize the fitness early on in the generation.

Contrary to the clustering approach, the GA approach shows lopsided results.
This could be an artifact of suboptimal initial solutions and a lack of
generations as seen by the variance in Fig. 4. However, the extremely high
infeasibility detection suggests that possibly a combined approach between a
structured clustering and a conventional learning based approach could achieve
higher accuracies.

The results suggest that our methodological approach based on data immediately
provides comparable results to heuristics that require expert knowledge.

###  IV-B Planning

Figure 5: Motion plans generated by the proposed planner. Top left: the robot
climbs stairs. Top right: the robot switches from walking to climbing. Bottom
left: the robot climbs up inside a tube. Bottom right: the robot climbs up
between two walls and avoid patches of low friction materials.

We use the learned envelopes in our 2-stage planner, and plan the motion for a
24 DoF hexapod. Trajectories to traverse in multiple terrains are found,
including walking on flat ground with stairs, climbing between two flat
vertical walls of varying friction, and climbing inside a tube. The coupling
achieved in the planner is compared against the decoupled approaches [3]. The
planner is verified on hardware. All results are included in the accompanying
video [22].

####  IV-B1 Walking on Ground

The proposed planner generates trajectories for walking on a flat ground with
stairs (Fig. 5 top left). This shows the feasibility of the approach on a
simple environment because the terrains are effectively flat.

####  IV-B2 Climbing between Walls

This problem is first studied by [21], which uses a stiffness based approach
allowing the robot to brace between two walls and climb. A 2-stage decoupled
approach is then used [3] to plan the climbing motion, where it plans the
position p’s without any knowledge of force. To test the proposed coupled
planner, we plan the trajectory for the robot to climb on the walls with
patches of zero friction $\mu=0$ (green regions in Fig. 5, bottom right) and
$\mu=1$ in other areas. $P_{1}$ will have to place the toes outside the region
to avoid $P_{2}$ being infeasible. For this, decoupled planner [3] would fail.
We also plan a climbing motion inside a tube consisting of meshes at different
angles (Fig. 5, bottom left), to test the adaptability to irregular walls. In
both cases, the planer solves feasible motion plan on hardware.

####  IV-B3 Automatic switching motion plan from ground to wall

We demonstrate that with the learned 2 mode formulation presented in Section
IV-A, trajectories with contact forces that automatically switch between
walking and climbing can be generated as seen in Fig. 5 top right. Beyond
finding a feasible trajectory, the planner finds $z=0$/$z=1$ when the robot is
on the ground/wall, indicating that it interprets the current motion as
walking/climbing. This result shows the interpretability of the proposed
planner. If we divide the variable space into smaller pieces and assign an
integer variable for each piece, the information found by the planner can
quickly get submerged by the large number of possible combinations of integer
variables. Instead, our planner gives interpretable information that humans
can process.

TABLE II: Solving time for vertical two flat wall climbing

[t]

Test Name | Rounds $M$ | Variables | Constraint | Solve Time [s]*
---|---|---|---|---
2-stage Convex | 8 | 4128 | 1176 | 0.16
MICP without Envelope | 4 | 16608 (276 binary) | 3360 | 91
MICP with Envelope | 4 | 16608 (276 binary) | 4008 | 33
MICP without Envelope | 8 | 33216 (552 binary) | 6720 |  $>$ 1,000
MICP with Envelope | 8 | 33216 (552 binary) | 8016 | 152

  * *

Data taken on an Intel i5-6260U 1.80GHz machine with Gurobi [23]

####  IV-B4 Solving Time

We benchmark the solving time on the problem of climbing between two flat
walls with uniform $\mu$. Table II row 1 shows the solving time for the
proposed two-stage convex planner. The convex solver generates trajectories in
hundreds of milliseconds. Comparison with similar works using NLPs suggests a
possible speed up of around 100 times [2]. This justifies the motivation
behind using a multi-staged convex algorithm. The designed envelope can also
be used to speed up MICP. We compare the accurate MICP formulation with and
without our learned envelope as an additional constraint for the problem of
climbing between two flat walls. A typical MICP solver deals with integer
variables through a branch and bound algorithm [24] that expands nodes on each
binary variable. Since [11] does not utilize the problem-specific knowledge,
the solver wastes time expanding nodes inside regions that we already know are
infeasible. Our approach reduces the solving time, and has minimum impact on
the solutions (Table II, row 2-4).

##  V Conclusion, Discussion and Future Work

We propose a 2-stage motion planner based on convex optimization, with the
inter-stage coupling formulated as McCormick envelopes learned from data. We
performed learning through clustering and GA approaches and validated against
labeled data. The results show that a smaller number of integer variables and
envelopes tailored to the type of problem can reduce solve time and help
interpret the outcome. We also demonstrated the planner on the hardware.

Finer relaxations with smaller envelopes could be possible if one envelope is
insufficient. Learning an objective function that guides the 1st stage
solution to a projectable point is another future work. Since we require
exploring the solution set, which could be efficient on hardware, training on
hardware is of interest. While we focused on a specific problem, this work may
be generalizable for more complicated problems with multiple stages, and a
larger class of nonlinear constraints.

## References

  * [1] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Permenter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot,” _Autonomous Robots_ , vol. 40, no. 3, pp. 429–455, 2016.
  * [2] A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, “Gait and trajectory optimization for legged systems through phase-based end-effector parameterization,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1560–1567, 2018.
  * [3] X. Lin, J. Zhang, J. Shen, G. Fernandez, and D. W. Hong, “Optimization based motion planning for multi-limbed vertical climbing robots,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2019, pp. 1918–1925.
  * [4] M. S. Ahn, H. Chae, and D. W. Hong, “Stable, autonomous, unknown terrain locomotion for quadrupeds based on visual feedback and mixed-integer convex optimization,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 3791–3798.
  * [5] B. Plancher and S. Kuindersma, “Realtime model predictive control using parallel ddp on a gpu,” in _Toward Online Optimal Control of Dynamic Robots Workshop at the 2019 International Conference on Robotics and Automation (ICRA), Montreal, Canada_ , 2019.
  * [6] P. Vernaza, M. Likhachev, S. Bhattacharya, S. Chitta, A. Kushleyev, and D. D. Lee, “Search-based planning for a legged robot over rough terrain,” in _2009 IEEE International Conference on Robotics and Automation_. IEEE, 2009, pp. 2380–2387.
  * [7] S. Boyd and L. Vandenberghe, _Convex optimization_. Cambridge university press, 2004.
  * [8] M. Kalakrishnan, J. Buchli, P. Pastor, M. Mistry, and S. Schaal, “Learning, planning, and control for quadruped locomotion over challenging terrain,” _The International Journal of Robotics Research_ , vol. 30, no. 2, pp. 236–258, 2011.
  * [9] M. Zucker, J. A. Bagnell, C. G. Atkeson, and J. Kuffner, “An optimization approach to rough terrain locomotion,” in _2010 IEEE International Conference on Robotics and Automation_. IEEE, 2010, pp. 3589–3595.
  * [10] G. P. McCormick, “Computability of global solutions to factorable nonconvex programs: Part i—convex underestimating problems,” _Mathematical programming_ , vol. 10, no. 1, pp. 147–175, 1976.
  * [11] H. Dai, G. Izatt, and R. Tedrake, “Global inverse kinematics via mixed-integer convex optimization,” _The International Journal of Robotics Research_ , vol. 38, no. 12-13, pp. 1420–1441, 2019.
  * [12] D. Bertsimas and B. Stellato, “The voice of optimization,” _Machine Learning_ , pp. 1–29, 2020.
  * [13] A. Cauligi, P. Culbertson, B. Stellato, D. Bertsimas, M. Schwager, and M. Pavone, “Learning mixed-integer convex optimization strategies for robot planning and control,” _arXiv preprint arXiv:2004.03736_ , 2020.
  * [14] A. M. Gleixner and S. Weltge, “Learning and propagating lagrangian variable bounds for mixed-integer nonlinear programming,” in _International Conference on AI and OR Techniques in Constriant Programming for Combinatorial Optimization Problems_. Springer, 2013, pp. 355–361.
  * [15] H. Nagarajan, M. Lu, E. Yamangil, and R. Bent, “Tightening mccormick relaxations for nonlinear programs via dynamic multivariate partitioning,” in _International Conference on Principles and Practice of Constraint Programming_. Springer, 2016, pp. 369–387.
  * [16] H. Nagarajan, M. Lu, S. Wang, R. Bent, and K. Sundar, “An adaptive, multivariate partitioning algorithm for global optimization of nonconvex programs,” _Journal of Global Optimization_ , vol. 74, no. 4, pp. 639–675, 2019.
  * [17] B. Ponton, A. Herzog, S. Schaal, and L. Righetti, “A convex model of humanoid momentum dynamics for multi-contact motion generation,” in _2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)_. IEEE, 2016, pp. 842–849.
  * [18] P. M. Castro, “Tightening piecewise mccormick relaxations for bilinear problems,” _Computers & Chemical Engineering_, vol. 72, pp. 300–311, 2015\.
  * [19] B. Acikmese and S. R. Ploen, “Convex programming approach to powered descent guidance for mars landing,” _Journal of Guidance, Control, and Dynamics_ , vol. 30, no. 5, pp. 1353–1366, 2007.
  * [20] R. L. H. Deits, “Convex segmentation and mixed-integer footstep planning for a walking robot,” Ph.D. dissertation, Massachusetts Institute of Technology, 2014\.
  * [21] X. Lin, H. Krishnan, Y. Su, and D. W. Hong, “Multi-limbed robot vertical two wall climbing based on static indeterminacy modeling and feasibility region analysis,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 4355–4362.
  * [22] RoMeLa. Icra2021 - multi-stage coupled convex programming with data-driven mccormick envelopes. YouTube. [Online]. Available: https://youtu.be/1WqTIVpFpIw
  * [23] G. Optimization, “Inc.,“gurobi optimizer reference manual,” 2015,” 2014.
  * [24] H. P. Williams, _Model building in mathematical programming_. John Wiley & Sons, 2013.