# Hierarchical Experience-informed Navigation for Multi-modal Quadrupedal
Rebar Grid Traversal

Max Asselmeier1, Jane Ivanova2, Ziyi Zhou3, Patricio A. Vela3 and Ye Zhao1 1M.
Asselmeier and Y. Zhao are with the School of Mechanical Engineering, Georgia
Institute of Technology, Atlanta, GA 30308, USA<EMAIL_ADDRESS>J. Ivanova
is with SkyMul Inc, Atlanta, GA 30339, USA.3 Z. Zhou and P.A. Vela are with
the School of Electrical and Computer Engineering and the Institute for
Robotics and Intelligent Machines, Georgia Institute of Technology, Atlanta,
GA 30308, USA.*This work is supported through a sponsored research grant from
SkyMul Inc. and a Georgia Tech IRIM/IPaT Aware Home Seed Grant

###### Abstract

This study focuses on a layered, experience-based, multi-modal contact
planning framework for agile quadrupedal locomotion over a constrained rebar
environment. To this end, our hierarchical planner incorporates locomotion-
specific modules into the high-level contact sequence planner and solves
kinodynamically-aware trajectory optimization as the low-level motion planner.
Through quantitative analysis of the experience accumulation process and
experimental validation of the kinodynamic feasibility of the generated
locomotion trajectories, we demonstrate that the experience planning heuristic
offers an effective way of providing candidate footholds for a legged contact
planner. Additionally, we introduce a guiding torso path heuristic at the
global planning level to enhance the navigation success rate in the presence
of environmental obstacles. Our results indicate that the torso-path guided
experience accumulation requires significantly fewer offline trials to
successfully reach the goal compared to regular experience accumulation.
Finally, our planning framework is validated in both dynamics simulations and
real hardware implementations on a quadrupedal robot provided by Skymul Inc.

##  I Introduction

The task of legged locomotion elicits a hybridized planning space that
combines discrete elements like robot end effectors and environmental
artifacts that can support footsteps along with continuous footstep positions
along such artifacts. Methods that opt to perform simultaneous contact and
footstep position planning often struggle to do so in a tractable manner
because of this hybrid planning space. The need for kinodynamically-feasible
solutions to this planning problem further compounds the computational efforts
required.

An alternative way to resolve contact planning is through a hierarchical
approach in which a discrete contact sequence is generated at the higher level
and then continuous whole-body trajectories that abide by the generated
contact sequence are synthesized at the lower level. The separation of the
planning space into discrete and continuous components computationally
simplifies the overall planning problem.

This planning decomposition reflects those widely explored in the areas of
task and motion planning (TAMP) [1, 2, 3] and multi-modal motion planning
(MMMP) [4, 5, 6]. These areas have proposed numerous effective planning
heuristics that allow the discrete and continuous planning layers to inform
each other and coordinate useful planning attempts. However, traditional MMMP
have seen limited use in dynamic locomotion due the combinatorial nature of
contacts which makes the problem difficult to scale up and the inherently
dynamic process of legged locomotion which imposes complex constraints on
motion planning.

Figure 1: Illustration of a quadrupedal robot performing rebar grid traversal
in a simulated construction environment [7] and an indoor test-bed.

In this work, we draw inspiration from the Augmented Leafs with Experience on
Foliations (ALEF) framework [5] for multi-modal planning. In particular, we
design a novel hierarchical planning framework to solve kinodynamically-
feasible quadrupedal locomotion plans, taking into account the robot’s
centroidal dynamics and kinematic reachability. This study marks the first
effort that leverages model-based trajectory optimization (TO) in the design
of the experience heuristic for quadrupedal locomotion multi-modal planning.
Our main contributions are summarized as below:

  1. 1.

Adapting mode families and a mode transition graph to quadrupedal contact
planning along with a carefully designed experience heuristic to weight the
mode transition graph and guide contact sequence planning;

  2. 2.

Integrating mode transition graph search with lower-level TO to naturally
embed footstep planners and tightly integrate the kinodynamically-aware
optimal cost from TO into the experience heuristic;

  3. 3.

Integrating this multi-modal contact planner into a navigation framework to
exploit a guiding torso path;

  4. 4.

Experimental validation of the proposed framework for rebar grid traversal
through quantitative analysis in both simulations and hardware implementation.

##  II Related Work

###  II-A Contact Planning

Within the quadrupedal contact planning domain, there exists a trade-off
between solution quality and computational cost. In the simplest case, fixing
contact schedules and generating nominal footstep positions through Raibert
heuristics [8] allows for online planning at high frequencies [9, 10].
However, such approaches sacrifice the ability to adjust footsteps in response
to the environment. To combat this rigidity, some methods augment nominal
footstep positions through learned networks [11, 12], nonlinear programs [13,
14], and control barrier functions [15]. Through simplifications such as pre-
defined gait sequences or discrete search spaces for footholds, these can also
be run in real-time.

Some methods resolve all elements of footstep planning (gait sequences,
contact positions) and whole-body trajectories in one module, often through
contact-timing optimization [16], mixed-integer programs [17, 18], soft
contact modeling [19], or linear complementarity constraints [20]. While such
approaches generate complex, highly dynamic motions, running such planners
online is out of the question.

Instead of solving a joint optimization problem, many contact planning
frameworks [6, 21, 22] employ a hierarchical planning structure, making the
key design choice of selecting contacts first and then synthesizing whole-body
motions. For bipedal platforms, contact transition models are limited enough
to achieve real-time footstep planning through either pure search [23, 24, 25,
26] or pure optimization [17]. Due to the more aggressive scaling in
quadrupedal planning, it becomes crucial to cater particular planning
approaches to particular subproblems.

###  II-B Multi-modal Planning

A common approach to frame complex long-horizon tasks is through a discrete-
continuous or multi-modal motion planning (MMMP) formulation. For
manipulation, a mode typically corresponds to a particular contact or grasp
configuration between end effectors and objects [27, 3, 28, 5, 29]. On the
other hand, in the realm of locomotion, a mode may represent a contact
configuration [21], a gait [30, 31], or a motion primitive [32]. In the
context of TAMP, these modes are often represented by symbolic states or logic
rules depending on particular problem domains [1, 2, 33, 3].

Existing MMMP frameworks often search over a mode graph which defines valid
transitions to obtain mode sequences. Continuous motion planning, whether it
be sampling, spline generation, or trajectory optimization (TO) is then
performed at the lower level to resolve mode transitions. The works of [34,
33, 30] use the results of lower-level TO programs to inform graph edge
weights. Other approaches train neural networks to estimate system dynamics
[35], the costs of optimization programs [32], or the feasibility of a
candidate action [36, 37, 38].

One particular heuristic of interest is the experienced-based framework ALEF
[5] which exploits the implicit continuous manifolds that arise from contact
constraints to disperse the results of offline planning queries throughout the
mode graph. This makes for a sample-efficient framework that can apply
informed weights to unvisited contact transitions. Many such heuristics,
including that of experience, have yet to be leveraged in quadrupedal contact
planning. Recent work [29] has shown how TAMP and MMMP enable quadrupeds to
rapidly perform complex loco-manipulation tasks such as manipulating and
passing through a door.

##  III Preliminaries

We provide background information on the planning problem we attempt to solve
while detailing important concepts of multi-modal planning for quadrupedal
locomotion.

###  III-A Centroidal Dynamics Model

A quadrupedal locomotion model can be modeled by centroidal dynamics, bridging
the complex full-body dynamics and simple center-of-mass (CoM) dynamics. This
model constrains the rate of centroidal momentum to be:

$\mathbf{\dot{h}}=\begin{bmatrix}\sum_{j}\mathbf{f}_{j}+m\mathbf{g}\\\
\sum_{j}(\mathbf{c}_{j}-\mathbf{r})\times\mathbf{f}_{j}+\boldsymbol{\tau}_{j}\end{bmatrix}$
(1)

where $\mathbf{h}=[\mathbf{k},\mathbf{l}]^{T}\in\mathbb{R}^{6}$ is the
centroidal momentum that includes linear $\mathbf{k}\in\mathbb{R}^{3}$ and
angular $\mathbf{l}\in\mathbb{R}^{3}$ momentum. $m$ is the robot mass,
$\mathbf{r}\in\mathbb{R}^{3}$ is the robot CoM position,
$\mathbf{f}_{j}\in\mathbb{R}^{3}$ is the contact force at $j^{\rm th}$ foot,
$\mathbf{g}\in\mathbb{R}^{3}$ is the acceleration vector of gravity,
$\mathbf{c}_{j}\in\mathbb{R}^{3}$ is the contact position of foot $j$, and
$\boldsymbol{\tau}_{j}\in\mathbb{R}^{3}$ is the contact torque of foot $j$. By
using the centroidal momentum matrix (CMM)
$\mathbf{A(q)}\in\mathbb{R}^{6\times(6+n_{j})}$ [39], the centroidal momentum
can also be expressed as:

$\mathbf{h}=\underbrace{\begin{bmatrix}\mathbf{A}_{b}(\mathbf{q})&\mathbf{A}_{j}(\mathbf{q})\end{bmatrix}}_{\mathbf{A}(\mathbf{q})}\begin{bmatrix}\mathbf{\dot{q}}_{b}\\\
\mathbf{\dot{q}}_{j}\end{bmatrix}$ (2)

where $\mathbf{q}=[\mathbf{q}_{b},\mathbf{q}_{j}]^{T}\in\mathbb{R}^{6+n_{j}}$
is the robot configuration. The robot configuration includes $n_{j}$ joints
and the six degrees of freedom (DOF) for the floating base pose.

###  III-B Environment Specifications

We ground this proposed work in the task of quadrupedal rebar traversal.
Therefore, it is important to outline the details of the environment before
moving on to further sections.

The rebar grids in this work are comprised of a set of bars $B$. A bar $b\in
B$ is defined by a starting point $\mathbf{p}_{0}\in\mathbb{R}^{3}$ in the
rebar grid frame $\mathcal{B}$, a length $l$, and an orientation $\theta$ with
respect to the rebar grid frame in the $x-y$ plane of the grid. Grids are
comprised of $N_{h}$ horizontal and $N_{v}$ vertical bars, and they can also
include a set of obstacles $O$ positioned along their surface. Obstacles poses
and shapes are assumed to be known. A set of example grids are shown in Figure
2.

(a)

(b)

(c)

(d)

Figure 2:  Set of rebar grids. (a) Normal grid, (b) skewed grid, (c) grid with
variably spaced bars, (d) grid with obstacles.

###  III-C Contact Manifolds

In this section, we present the manifold terminologies from the ALEF framework
in the context of quadrupedal locomotion. More details can be found at [5].

In legged locomotion, a contact mode $\xi$ can be viewed as a set of footholds
at unique positions along a set of steppable objects such as planar stepping
stones or linear rebar poles. From this contact mode $\xi$, a lower-
dimensional mode manifold $\mathcal{M}^{\xi}$ embedded in the configuration
space $\mathcal{Q}$ arises which encompasses all of the whole-body
configurations that satisfy the foothold positions. As the foothold positions
vary along the steppable objects, different contact manifolds arise.

The set of all contact modes corresponding to the same set of steppable
objects can be grouped into a mode family $\Xi$. From each mode familiy
$\Xi_{i}$ arises a foliated manifold, or a foliation $\mathcal{F}_{\Xi_{i}}$.
An $n$-dimensional foliation $\mathcal{M}$ is a manifold defined by a
$n_{\boldsymbol{\chi}}$-dimensional transverse manifold $X$, a set of non-
overlapping $(n-n_{\boldsymbol{\chi}})$-dimensional leaf manifolds
$\mathcal{L}_{\chi}$ $\forall\boldsymbol{\chi}\in X$, and lastly a projection
operator $\pi:\mathcal{M}\rightarrow X$.

Elements of the transverse manifold $\boldsymbol{\chi}\in X$ are called
coparameters. A coparameter $\boldsymbol{\chi}$ uniquely parameterizes a mode
$\xi$ (and subsequently a leaf manifold
$\mathcal{L_{\boldsymbol{\chi}}}=\mathcal{M}^{\xi}$), and for our use case,
the position of footholds along steppable objects within the given mode
family. Therefore, a mode $\xi$ can be viewed as the tuple of a mode family
$\Xi$ and a unique coparameter $\boldsymbol{\chi}$. The union of leaf
manifolds along the set of coparameters $\bigcup_{\boldsymbol{\chi}\in
X}\mathcal{L}_{\boldsymbol{\chi}}$ recovers the entire foliation
$\mathcal{M}$.

A leaf or mode manifold $\mathcal{M}^{\xi}$ can be implicitly defined through
a constraint function
$F^{\xi}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}_{\boldsymbol{\chi}}$ where a
configuration $\mathbf{q}$ lies on the mode manifold if
$F^{\xi}(\mathbf{q})=\mathbf{0}$.

In the proposed work, a mode represents three stance legs in contact with
three separate rebars. Therefore, an example constraint function for a contact
mode is

$F^{\xi}(\mathbf{q}):=\begin{bmatrix}F^{\xi}_{1}(\mathbf{q}),F^{\xi}_{2}(\mathbf{q}),F^{\xi}_{3}(\mathbf{q})\end{bmatrix}^{T},$
(3)

where for a foot $j$ in contact with bar $b=\\{\mathbf{p}_{0},l,\theta\\}$,
then

$F^{\xi}_{j}(\mathbf{q}):=\begin{bmatrix}\text{FK}^{x}_{j}(\mathbf{q})-(\mathbf{p}^{x}_{0}+\chi_{j}\cdot
l\cdot\cos(\theta))\\\
\text{FK}^{y}_{j}(\mathbf{q})-(\mathbf{p}^{y}_{0}+\chi_{j}\cdot
l\cdot\sin(\theta))\\\
\text{FK}^{z}_{j}(\mathbf{q})-\mathbf{p}^{z}_{0}\end{bmatrix}=\textbf{0},$ (4)

where $\text{FK}_{j}(\mathbf{q})$ gives the position of foot $j$ given
$\mathbf{q}$ via forward kinematics (FK).

##  IV Multi-modal Planning

Given the prior information, the planning problem we are trying to solve is as
follows. Assume a quadrupedal robot with a configuration space
$\mathcal{Q}\subset\mathbb{R}^{n_{q}}$. We seek to find a collision-free path
$\mathbf{q}(s)$ with $s\in[0,1]$ from a start configuration
$\mathbf{q}(0)=\mathbf{q}_{\rm start}$ to a goal configuration
$\mathbf{q}(1)=\mathbf{q}_{\rm goal}$. Contact must strictly be made with the
set of bars $B$, and collisions with obstacles $O$ should be avoided.

###  IV-A Mode Transition Graph Construction

We employ a mode transition graph $\mathcal{G}$ in the spirit of the ALEF
framework in which the mode families comprise the set of vertices
$\mathcal{V}$ in $\mathcal{G}=(\mathcal{V},\mathcal{E})$. The edges
$\mathcal{E}$ are then formed between mode families for which kinematically
feasible transitions exist. In the configuration space, edges are formed
between mode families which have foliations that intersect, enabling mode
transitions. The process in which edges are constructed is defined now.

We utilize locomotion-specific problem constraints to implicitly define
feasible transitions within the mode transition graph. First, a user-defined
contact sequence informs the graph on the robot’s footfall pattern. A
transition from a vertex $v_{i}$ to vertex $v_{j}$ (equivalently, mode family
$\Xi_{i}$ to mode family $\Xi_{j}$) is only added if the swing leg at $v_{i}$
and the swing leg at $v_{j}$ occur sequentially in the contact sequence.

Second, we incorporate kinematic reachability analysis to approximate what
regions of the ground are contactable by the robot’s legs. This is then used
to determine what contact transitions are kinematically feasible. In this
work, we use a family of functions known as superquadrics that has seen recent
use in legged locomotion [40]. The set of points that fall within the
superquadric centered at $(x_{0},y_{0})$ are

$S=\big{\\{}(x,y)\in\mathbb{R}^{2}\hskip 7.11317pt\Big{|}\hskip
7.11317pt\big{|}\frac{x-x_{0}}{A}\big{|}^{a}+\big{|}\frac{y-y_{0}}{B}\big{|}^{b}\leq
1\big{\\}},$ (5)

where scalars $A,B$ and $a,b$ control dimensions and curvature. Parameters
were obtained through randomly sampling configurations, keeping all samples
that reach within a distance threshold $\epsilon=1$ cm of the ground, and
tuning parameter values to encompass the samples in contact (see Figure 3).

Figure 3: Kinematic reachability areas along with projected configuration
samples. Some samples that meet the distance threshold are set as outliers to
avoid reachable areas that yield unstable contacts or self-collisions.

For clarity, a fraction of the mode transition graph with these two
constraints incorporated into the feasible transitions is visualized within
the higher level of Figure 4.

###  IV-B Mode Transition Graph Search

We formulate the task of finding a discrete foothold sequence as a graph
search problem over the aforementioned mode transition graph. We employ the A*
search algorithm for this mode transition graph search. For the search, we
discretize along the transverse manifold of each mode family to generate
slices of the foliations that correspond to intervals of foothold positioning.
Edges are then added between all slices of the source and destination mode
families in which contact transitions exist in the graph. This discretization
allows for the search to reason about both contact sequencing (what rebar
objects to contact with which feet) and foothold sequencing (where to make
contact). This search provides a candidate lead: a sequence of mode families
and coparameter values that define a foothold sequence from the start to goal.
The relevant costs and weights are defined below.

####  IV-B1 Edge weight

For a transition between source mode $\xi_{\rm src}=\langle\Xi_{\rm
src},\boldsymbol{\chi}_{\rm src}\rangle$ and destination mode $\xi_{\rm
dst}=\langle\Xi_{\rm dst},\boldsymbol{\chi}_{\rm dst}\rangle$, the graph edge
$e=(\xi_{\rm src},\xi_{\rm dst})$ is assigned the weight

$\begin{split}&\Delta c(\xi_{\rm src},\xi_{\rm
dst})=w_{\mathcal{D}}\cdot\mathcal{D}^{\Xi_{\rm src},\Xi_{\rm
dst}}(\boldsymbol{\chi}_{\rm src},\boldsymbol{\chi}_{\rm dst})+\\\ &w_{d}\cdot
d_{\text{CoM}}(\xi_{\rm src},\xi_{\rm dst})+w_{\tau}\cdot d_{\tau}(\xi_{\rm
src},\xi_{\rm dst}),\end{split}$ (6)

where the distribution $\mathcal{D}^{\Xi_{\rm src},\Xi_{\rm dst}}(\chi_{\rm
src},\chi_{\rm dst})$ captures the difficulty of transitioning from $\xi_{\rm
src}$ to $\xi_{\rm dst}$. This distribution is estimated offline through the
experience heuristic which is detailed in Section V. The term
$d_{\text{CoM}}(\xi_{\rm src},\xi_{\rm dst})$ is the Euclidean distance
between nominal CoM positions for $\xi_{\rm src}$ and $\xi_{\rm dst}$, and
$d_{\tau}(\xi_{\rm src},\xi_{\rm dst})$ is the deviation of modes nominal CoM
positions for $\xi_{\rm src}$ and $\xi_{\rm dst}$ from a suggested torso path.
This path can come from any planner that generates a sequence of torso poses
from start to goal, and implementation details on this suggested torso path
are given in Section VI.

####  IV-B2 Cost-to-go

For the A* search, a contact mode $\xi=\langle\Xi,\chi\rangle$ is assigned the
search heuristic value

$g(\xi)=w_{d}\cdot d_{\text{CoM}}(\xi,\Xi_{\rm goal})$ (7)

to ensure an admissible search heuristic and therefore provide optimal
foothold sequences with respect to edge weights.

###  IV-C Whole-Body Trajectory Optimization

At the motion planning level, we opt to use trajectory optimization (TO) as
opposed to the sampling-based planning methods [41, 42, 43] commonly use in
MMMP to generate dynamics-aware continuous paths that enable the robot to
transition between mode families. While the TO sacrifices the probabilistic
completeness that is offered by sampling-based methods, our approach enables
kinodynamically-aware multi-modal contact planning which has not been explored
in prior works. Similar to [44, 45, 46], our TO problem solves over the robot
state $\mathbf{x}=[\mathbf{h},\mathbf{q}_{b},\mathbf{q}_{j}]$ and the robot
input $\mathbf{u}=[\mathbf{f},\mathbf{v}_{j}]$, except that the coparameters
$\chi$ of the destination mode family decides the contact position
$\mathbf{c}$.

The TO formulation is shown as:

$\displaystyle\|\mathbf{x}[N]-\mathbf{x}^{\text{des}}[N]\|^{2}_{Q_{f}}+$
$\displaystyle\underset{\mathbf{x},\mathbf{u}}{\min}$
$\displaystyle\sum_{k=0}^{N-1}\Biggl{(}\|\mathbf{x}[k]-\mathbf{x}^{\text{des}}[k]\|^{2}_{Q}+\|\mathbf{u}[k]\|^{2}_{R}\Biggr{)}$
subject to (Mode) $\displaystyle F^{\xi_{\rm
src}}(\mathbf{q}[k])=\mathbf{0},\;F^{\xi_{\rm dst}}(\mathbf{q}[N])=\mathbf{0}$
(8a) (Dynamics) $\displaystyle\begin{bmatrix}\mathbf{\dot{h}}[k]\\\
\mathbf{\dot{q}}_{b}[k]\\\
\mathbf{\dot{q}}_{j}[k]\end{bmatrix}=\begin{bmatrix}\mathcal{D}(\mathbf{q}[k],\mathbf{f}[k])\\\
\mathbf{A}_{b}^{-1}(\mathbf{h}[k]-\mathbf{A}_{j}\mathbf{v}_{j}[k])\\\
\mathbf{v}_{j}[k]\end{bmatrix}$ (8b) (Friction)
$\displaystyle\mathbf{f}_{j}[k]\in\mathcal{F}_{j}(\mu,\mathbf{q})\hskip
28.45274pt\forall j\in\mathcal{C}_{src}$ (8c)
$\displaystyle\mathbf{f}_{j}[k]=\mathbf{0}\hskip
28.45274pt\quad\quad\quad\forall j\notin\mathcal{C}_{src}$ (8d) (Collision)
$\displaystyle g(\mathbf{q}[k])\geq 0\quad\quad\quad\quad\;\forall\hskip
1.42271ptk\in[0,N]$ (8e)

where $\xi_{\rm src}=\langle\Xi_{\rm src},\chi_{\rm src}\rangle,\xi_{\rm
dst}=\langle\Xi_{\rm dst},\chi_{\rm dst}\rangle$, and $\mathcal{C}_{\rm src}$
represents the set of stance feet for the source mode. Note that at the TO
level, the destination mode family is fixed, but the coparameter is treated as
a decision variable, allowing for variation of the destination mode. We
formulate the above TO as a Sequential Quadratic Program (SQP) and solve
through the OCS2 library [47]. We employ a time horizon of $T=0.5$ seconds and
$N=50$ knot points with maximal 250 iterations. For the collision avoidance
constraint, we run the Gilbert-Johnson-Keerthi algorithm [48] provided by the
HPP-FCL library [49] and use the Pinocchio library for kinematics and dynamics
calculations [50]. $\mathbf{x}^{\rm des}$ is generated in two steps, first,
the mode constraint functions from constraint (8a) are used in a contact
projection step to project a randomly sampled target configuration into
contact satisfying the source and destination modes. If this first step is
successful, cubic splines are then synthesized for the swing feet to build out
the remainder of $\mathbf{x}^{\rm des}$. If the optimal cost of an attempted
mode transition is above a threshold $\mathcal{J}_{\rm max}$, then the
planning trial is terminated early.

Figure 4: Overall diagram showcasing problem structure. A graph search is
performed over mode transitions using the estimated weight distributions from
experience (colored by their corresponding swing foot). Then, the suggested
contact sequence is run through trajectory optimization to determine the costs
$\mathcal{J}_{i,i+1}$ of the transitions.

##  V Planning with Experience

The objective of planning with experience is to acquire a continuous function
that captures the difficulty or cost of attempting certain contact transitions
within the environment which are encoded as the edges of our mode transition
graph.

###  V-A Optimal Cost Integration

There are two potential outcomes of a transition attempt:

  1. 1.

Contact projection fails to generate a target configuration, suggesting an
infeasible transition ($\mathcal{J}_{\rm src,\rm dst}=\infty$)

  2. 2.

Contact projection generates a target configuration, triggering a TO instance
($\mathcal{J}_{\rm src,\rm dst}=\text{Equation \ref{eq:whole_to}}$)

The experience heuristic allows us to infer from previously attempted mode
transitions the cost of nearby, possibly unattempted mode transitions. While a
transition between two modes may be kinematically infeasible, infeasibility
does not necessarily hold for all modes between the two mode families.
Therefore, weighting the entire transition with a cost of infinity could
inhibit discovery of a path to the goal, especially in situations where
foothold location is crucial to successful locomotion. To account for this,
cost values $\mathcal{J}_{\rm src,\rm dst}$ are first passed through a
weighted tanh function

$\delta_{\rm src,\rm dst}=w_{1}\tanh{(w_{2}\cdot\mathcal{J}_{\rm src,\rm
dst}+w_{3})},$ (9)

where $w_{1}$, $w_{2}$, and $w_{3}$ are positive scalar values, to map the
costs to finite positive penalties that can be used to populate the edge
weights in the mode transition graph.

###  V-B Experience Accumulation

The smoothness of contact manifolds allows us to exploit prior planning
results to inform estimates regarding similar contact transitions. This is
predicated on the idea that since foliations are smooth, coparameters nearby
on the transverse manifold parameterize modes that are similar in cost.

Once the penalty values are obtained from a given TO run, they can be
distributed throughout the graph in the form of experience. To distribute
penalties throughout all modes within a given mode family, we employ function
regression techniques that involve constructing a weighted sum of basis
functions that is meant to estimate the continuous distribution of the average
penalty value at different contact positions. In this work, we model this
distribution as the weighted sum of radial basis functions (RBFs), where the
update applied to the weights of the traversed edge is

$f^{\Xi_{\rm src},\Xi_{\rm dst}}(\boldsymbol{\chi}_{\rm
src},\boldsymbol{\chi}_{\rm
dst})=w_{e}\cdot\exp(\frac{-d(\boldsymbol{\chi}_{\rm
src},\boldsymbol{\chi}_{\rm dst})^{2}}{2\cdot\sigma^{2}})$ (10)

where

$w_{e}=(\mathcal{J}_{\rm src,\rm dst}-\bar{\mathcal{J}})$ (11)

where $\mathcal{J}_{\rm src,\rm dst}$ is the cost obtained from the attempted
mode transition, $\bar{\mathcal{J}}$ is the average transition cost between
$\xi_{\rm src}$ and $\xi_{\rm dst}$, $d(\boldsymbol{\chi}_{\rm
src},\boldsymbol{\chi}_{\rm dst})$ represents the distance of a coparameter to
$(\boldsymbol{\chi}_{\rm src},\boldsymbol{\chi}_{\rm dst})$, and $\sigma$
represents the standard deviation of the RBF. This update adds a basis
function to the weight distribution of each traversed edge that is centered at
the attempted coparameter value and weighted by its deviation from the average
cost of the attempted mode transition.

##  VI Experimental Results

In this section, we perform offline experience accumulation in which where a
batch of planning trials are run in order to populate the weight and
demonstrate the resultant whole-body motion plans output by the proposed
framework. Within each planning trial, the high-level graph search provides a
candidate contact sequence which is passed to the lower level of the framework
where a sequence of trajectory optimization subproblems are solved to generate
contact transitions.

Case studies in three environments are performed: (i) a grid with low-height
obstacles scattered along its surface (Section VI-A), (ii) a grid with a tall
obstacle positioned between the start and goal configurations (Section VI-B),
and (iii) a grid with various obstacles meant to emulate a real-world
constriction site. These three grids, along with outputted reference
trajectories, are visualized in Figure 5.

Figure 5: Rebar grid layouts used in the case studies in Sections VI-A \-
VI-C.

For three case studies, we record computation times of the graph search
triggered for each planning trial as well as average, minimum, and maximum TO
solve times across all of the attempted subproblems within each planning
trial. Additionally, we report the results of all subproblems – success,
failure, or not attempted due to early trial termination – as well as the
total path costs for the trials that reached the goal. Lastly, reference
trajectories obtained from our framework are deployed on a quadruped on a real
world rebar grid and tracking performance is evaluated (Section VI-D).

###  VI-A Footstep Adjustment through Experience

In this first case study, we demonstrate the key role that the weight
distributions obtained through experience play in successful contact planning.
We deploy the planner on a rebar grid with short, foot-level obstacles along
its surface, and through offline experience accumulation the planner
ascertains what contact transitions allow the robot to reach the goal without
collisions. Results are shown in Figure 6.

Figure 6: Results for Section VI-A: Case Study 1 on the grid in Figure 5(a).

We initially performed 500 planning trials with all experience-based weight
distributions initialized uniformly to $\mathcal{D}^{\Xi_{\rm src},\Xi_{\rm
dst}}(\boldsymbol{\chi}_{\rm src},\boldsymbol{\chi}_{\rm dst})=0.01$, but the
planner failed to reach the goal on any of the trials due to the extensive
period of graph exploration required to appropriately estimate the weight
distributions. We then performed a second run of offline trials where we
initialized the weight distributions to priors based on proximity of the mode
transitions to obstacles in the environment. With these priors, the planner
was able to explore a greater portion of the mode transition graph and
ultimately find successful contact plans to the goal in far fewer trials.
During initial trials, mode transitions that collide with obstacles are
attempted, leading to extremely prohibitive and highly variant TO times.
However, after roughly $30$ trials the planner is able to suggest collision-
free contact plans, greatly reducing both the mean and variance of TO solve
times. In this environment, the torso planner does not provide any useful
insights on planning given that all obstacle exist at the foot level, and the
key heuristic that enables successful planning to the goal in such an
environment is the weight distribution accumulated from experience.

###  VI-B Torso Path-Guided Experience Accumulation

In this section, we perform an ablation study in which the multi-modal contact
planner is run both with and without a guiding torso path planner. The
incorporation of this planner emulates common navigation frameworks which
perform coarse, low-frequency torso planning that provides guiding paths to a
lower-level footstep planner that synthesizes whole-body trajectories. We
formulate the torso path planner as an additional A* graph search where the
graph nodes are the set of intersections along the rebar grid. Edges are added
between adjacent grid intersections and edge weights are assigned based on
proximity to inflated obstacles and Euclidean distances between the source and
destination intersections. Results are shown in Figures 7(a) and 7(b).

(a) Results for experience accumulation with the guiding torso path.

(b) Results for experience accumulation without the guiding torso path.

Figure 7: Results for Section VI-B: Case Study 2 on the grid in Figure 5(b)

Due to the large obstacle positioned between the start and goal, the added
torso planner greatly expedites experience accumulation. The guiding torso
path biases the multi-modal contact planner towards obstacle-free regions of
the grid which take far less time to locomote through with TO. Without the
torso path, the mode transition graph search takes the shortest path from
start to goal which runs through the obstacle, leading to much higher TO times
and less overall exploration of the graph. One drawback of instituting the
guiding torso path is that the graph search times increase significantly due
to the introduction of the complicated torso path deviation term into the
graph edge weight function.

###  VI-C Holistic Collision Avoidance through Experience

In the third case study, we demonstrate our framework’s ability to reason
through complicated rebar environments with obstacles at both the torso level
and the foot level. Larger pillars and beams are avoided through following the
guiding torso path while barriers and debris on the grid surface are avoided
through adjust footstep positioning through experience. Results are shown in
Figure 8.

Figure 8: Results for Section VI-C: Case Study 3 on the grid in Figure 5(c).

In this study, there are some early planning trials that successfully reach
the goal. This is largely due to the presence of the guiding torso path
manuevering the resulting contact sequences around large obstacles. However,
it is over the course of the experience accumulation that the mode transitions
which allow the robot to step over the barrier spanning across the grid and
the clutter on the left side of grid are discovered. This complicated
environment gives rise to higher graph search times than those observed in the
previous case studies. Also, more planning trials are required to
appropriately estimate the edge weights within the graph. However, our
framework still only requires $80$ trials to generate contact sequences with
consistently short solve times at the TO level that allow robot to reach the
goal.

###  VI-D Hardware Implementation

To ensure that trajectories generated by our framework can be robustly
deployed on real systems, we set up a rebar scenario and perform online
executions on a quadrupedal rebar-tying robot – Chotu. We employ an MPC-WBC
tracking controller modified from [46]. The MPC solves a similar centroidal
dynamics optimization as (8) at 100 Hz but with the fixed contact sequence
from our framework. An end-effector constraint is added to accurately track
the reference swing foot trajectory, which is crucial to successful rebar
traversing. The WBC solves a hierarchical QP at 500 Hz. The state estimator
fuses IMU data, joint encoders, and motion capture inputs to provide accurate
body position information.

Figure 9: Hardware demonstration of Chotu performing rebar traversal. (a)
Real-world rebar grid setup. (b) Rebar traversal in dynamics simulation.
Colored lines denote desired position trajectories for robot torso and feet.
(c) Comparison of reference trajectory and measured robot states.

We validate the trajectories generated by our framework on one real-world
example. As shown in Fig. 9 (a) and (b), the robot Chotu is commanded to move
from the top left corner of the rebar grid to the middle right with an
obstacle is blocking in the way Fig. 9 (c) demonstrates a favorable tracking
performance with insignificant body pose and foot error regarding the body
pose and foot locations.

##  VII Conclusion

In this work, we adapt an efficient multi-modal contact planner to the task of
quadrupedal rebar traversal. We accumulate offline experience to estimate
optimal cost distributions using the results of lower-level trajectory
optimization instances. In the future, we aim to incorporate vision as a means
to generate more informed priors on optimal cost distributions and perform
reactive planning.

## References

  * [1] M. Toussaint, K. R. Allen, K. A. Smith, and J. B. Tenenbaum, “Differentiable Physics and Stable Modes for Tool-Use and Manipulation Planning,” pp. 6231–6235, 2019.
  * [2] V. N. Hartmann, A. Orthey, D. Driess, O. S. Oguz, and M. Toussaint, “Long-Horizon Multi-Robot Rearrangement Planning for Construction Assembly,” _IEEE Transactions on Robotics_ , vol. 39, no. 1, pp. 239–252, Feb. 2023.
  * [3] K. Kim, D. Park, and M. J. Kim, “A Reachability Tree-Based Algorithm for Robot Task and Motion Planning,” Mar. 2023.
  * [4] K. Hauser, T. Bretl, and J.-C. Latombe, “Non-gaited humanoid locomotion planning,” in _5th IEEE-RAS International Conference on Humanoid Robots, 2005._ , Dec. 2005.
  * [5] Z. Kingston and L. E. Kavraki, “Scaling Multimodal Planning: Using Experience and Informing Discrete Search,” _IEEE Transactions on Robotics_ , vol. 39, no. 1, pp. 128–146, Feb. 2023.
  * [6] T. Bretl, “Motion Planning of Multi-Limbed Robots Subject to Equilibrium Constraints: The Free-Climbing Robot Problem,” _The International Journal of Robotics Research_ , vol. 25, no. 4, pp. 317–342, Apr. 2006.
  * [7] Clearpath Robotics, “Clearpath Additional Simulation Worlds.” [Online]. Available: https://github.com/clearpathrobotics/cpr_gazebo
  * [8] M. H. Raibert, _Legged robots that balance_. MIT press, 1986.
  * [9] G. Bledt and S. Kim, “Implementing Regularized Predictive Control for Simultaneous Real-Time Footstep and Ground Reaction Force Optimization,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Nov. 2019.
  * [10] D. Kim, J. Di Carlo, B. Katz, G. Bledt, and S. Kim, “Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control,” Sep. 2019, arXiv:1909.06586.
  * [11] O. Villarreal, V. Barasuol, M. Camurri, L. Franceschi, M. Focchi, M. Pontil, D. G. Caldwell, and C. Semini, “Fast and Continuous Foothold Adaptation for Dynamic Locomotion through CNNs,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 2140–2147, Apr. 2019.
  * [12] A. Bratta, A. Meduri, M. Focchi, L. Righetti, and C. Semini, “ContactNet: Online Multi-Contact Planning for Acyclic Legged Robot Locomotion,” Mar. 2023.
  * [13] F. Jenelten, T. Miki, A. E. Vijayan, M. Bjelonic, and M. Hutter, “Perceptive Locomotion in Rough Terrain – Online Foothold Optimization,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 4, pp. 5370–5376, Oct. 2020.
  * [14] R. Grandia, F. Jenelten, S. Yang, F. Farshidian, and M. Hutter, “Perceptive Locomotion through Nonlinear Model Predictive Control,” Aug. 2022, arXiv:2208.08373.
  * [15] R. Grandia, A. J. Taylor, A. D. Ames, and M. Hutter, “Multi-Layered Safety for Legged Robots via Control Barrier Functions and Model Predictive Control,” Jun. 2021, arXiv:2011.00032.
  * [16] 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, Jul. 2018.
  * [17] R. Deits and R. Tedrake, “Footstep planning on uneven terrain with mixed-integer convex optimization,” in _2014 IEEE-RAS International Conference on Humanoid Robots_ , Nov. 2014.
  * [18] B. Aceituno-Cabezas, C. Mastalli, H. Dai, M. Focchi, A. Radulescu, D. G. Caldwell, J. Cappelletto, J. C. Grieco, G. Fernández-López, and C. Semini, “Simultaneous Contact, Gait, and Motion Planning for Robust Multilegged Locomotion via Mixed-Integer Convex Optimization,” _IEEE Robotics and Automation Letters_ , 2018.
  * [19] M. Neunert, M. Stäuble, M. Giftthaler, C. D. Bellicoso, J. Carius, C. Gehring, M. Hutter, and J. Buchli, “Whole-body nonlinear model predictive control through contacts for quadrupeds,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 3, pp. 1458–1465, 2018.
  * [20] M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” _The International Journal of Robotics Research_ , vol. 33, no. 1, pp. 69–81, Jan. 2014.
  * [21] K. Hauser, T. Bretl, J.-C. Latombe, K. Harada, and B. Wilcox, “Motion Planning for Legged Robots on Varied Terrain,” _The International Journal of Robotics Research_ , vol. 27, no. 11-12, pp. 1325–1349, Nov. 2008.
  * [22] M. Zucker, N. Ratliff, M. Stolle, J. Chestnutt, J. A. Bagnell, C. G. Atkeson, and J. Kuffner, “Optimization and learning for rough terrain legged locomotion,” _The International Journal of Robotics Research_ , vol. 30, no. 2, pp. 175–191, Feb. 2011.
  * [23] J. Chestnutt, M. Lau, G. Cheung, J. Kuffner, J. Hodgins, and T. Kanade, “Footstep Planning for the Honda ASIMO Humanoid,” in _Proceedings of the 2005 IEEE International Conference on Robotics and Automation_ , Apr. 2005, pp. 629–634, iSSN: 1050-4729.
  * [24] R. J. Griffin, G. Wiedebach, S. McCrory, S. Bertrand, I. Lee, and J. Pratt, “Footstep Planning for Autonomous Walking Over Rough Terrain,” Jul. 2019.
  * [25] P. Karkowski and M. Bennewitz, “Real-time footstep planning using a geometric approach,” in _2016 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2016, pp. 1782–1787.
  * [26] M. Missura and M. Bennewitz, “Fast Footstep Planning with Aborting A*,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2021.
  * [27] S. J. Jorgensen, M. Vedantam, R. Gupta, H. Cappel, and L. Sentis, “Finding Locomanipulation Plans Quickly in the Locomotion Constrained Manifold,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2020, pp. 6611–6617.
  * [28] C. Chen, P. Culbertson, M. Lepert, M. Schwager, and J. Bohg, “TrajectoTree: Trajectory Optimization Meets Tree Search for Planning Multi-contact Dexterous Manipulation,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Sep. 2021, pp. 8262–8268.
  * [29] J.-P. Sleiman, F. Farshidian, and M. Hutter, “Versatile multicontact planning and control for legged loco-manipulation,” _Science Robotics_ , 2023, _eprint: https://www.science.org/doi/pdf/10.1126/scirobotics.adg5014.
  * [30] L. Amatucci, J.-H. Kim, J. Hwangbo, and H.-W. Park, “Monte Carlo Tree Search Gait Planner for Non-Gaited Legged System Control,” May 2022.
  * [31] W. Ubellacker and A. Ames, “Robust Locomotion on Legged Robots through Planning on Motion Primitive Graphs,” Sep. 2022.
  * [32] H. J. T. Suh, X. Xiong, A. Singletary, A. D. Ames, and J. W. Burdick, “Energy-Efficient Motion Planning for Multi-Modal Hybrid Locomotion,” Aug. 2020.
  * [33] Z. Zhao, Z. Zhou, M. Park, and Y. Zhao, “SyDeBO: Symbolic-Decision-Embedded Bilevel Optimization for Long-Horizon Manipulation in Dynamic Environments,” _IEEE Access_ , vol. 9, pp. 128 817–128 826, 2021.
  * [34] M. Toussaint and M. Lopes, “Multi-bound tree search for logic-geometric programming in cooperative manipulation domains,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2017, pp. 4044–4051.
  * [35] Y.-C. Lin, B. Ponton, L. Righetti, and D. Berenson, “Efficient Humanoid Contact Planning using Learned Centroidal Dynamics Prediction,” Feb. 2019.
  * [36] D. Driess, J.-S. Ha, and M. Toussaint, “Deep Visual Reasoning: Learning to Predict Action Sequences for Task and Motion Planning from an Initial Scene Image,” in _Robotics: Science and Systems XVI_. Robotics: Science and Systems Foundation, Jul. 2020.
  * [37] D. Driess, O. Oguz, J.-S. Ha, and M. Toussaint, “Deep Visual Heuristics: Learning Feasibility of Mixed-Integer Programs for Manipulation Planning,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2020, pp. 9563–9569.
  * [38] A. M. Wells, N. T. Dantam, A. Shrivastava, and L. E. Kavraki, “Learning Feasibility for Task and Motion Planning in Tabletop Environments,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 1255–1262, Apr. 2019.
  * [39] D. E. Orin, A. Goswami, and S.-H. Lee, “Centroidal dynamics of a humanoid robot,” _Autonomous robots_ , vol. 35, pp. 161–176, 2013.
  * [40] O. Melon, R. Orsolino, D. Surovik, M. Geisert, I. Havoutis, and M. Fallon, “Receding-Horizon Perceptive Trajectory Optimization for Dynamic Legged Locomotion with Learned Initialization,” Apr. 2021, arXiv:2104.09078 [cs]. [Online]. Available: http://arxiv.org/abs/2104.09078
  * [41] T. Siméon, J.-P. Laumond, J. Cortés, and A. Sahbani, “Manipulation Planning with Probabilistic Roadmaps,” _The International Journal of Robotics Research_ , Aug. 2004.
  * [42] F. Gravot, S. Cambon, and R. Alami, “aSyMov: A Planner That Deals with Intricate Symbolic and Geometric Problems,” in _Robotics Research. The Eleventh International Symposium_ , ser. Springer Tracts in Advanced Robotics, P. Dario and R. Chatila, Eds., 2005.
  * [43] J. Mirabel and F. Lamiraux, “Manipulation planning: Addressing the crossed foliation issue,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_ , May 2017.
  * [44] H. Dai, A. Valenzuela, and R. Tedrake, “Whole-body motion planning with centroidal dynamics and full kinematics,” in _2014 IEEE-RAS International Conference on Humanoid Robots_ , Nov. 2014, pp. 295–302, iSSN: 2164-0580.
  * [45] M. Chignoli, D. Kim, E. Stanger-Jones, and S. Kim, “The mit humanoid robot: Design, motion planning, and control for acrobatic behaviors,” in _2020 IEEE-RAS 20th International Conference on Humanoid Robots (Humanoids)_. IEEE, 2021, pp. 1–8.
  * [46] J.-P. Sleiman, F. Farshidian, M. V. Minniti, and M. Hutter, “A unified mpc framework for whole-body dynamic locomotion and manipulation,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 4688–4695, 2021.
  * [47] “OCS2: An open source library for Optimal Control of Switched Systems.” [Online]. Available: https://github.com/leggedrobotics/ocs2
  * [48] E. Gilbert, D. Johnson, and S. Keerthi, “A fast procedure for computing the distance between complex objects in three-dimensional space,” _IEEE Journal on Robotics and Automation_ , Apr. 1988.
  * [49] “HPP-FCL — An extension of the Flexible Collision Library.” [Online]. Available: https://github.com/humanoid-path-planner/hpp-fcl
  * [50] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library – A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in _IEEE International Symposium on System Integrations (SII)_ , 2019.