# Reactive Locomotion Decision-Making and Robust Motion Planning
for Real-Time Perturbation Recovery

Zhaoyuan Gu, Nathan Boyd, and Ye Zhao The authors are with the Laboratory for
Intelligent Decision and Autonomous Robots, Woodruff School of Mechanical
Engineering, Georgia Institute of Technology. {zgu78, nboyd31,
<EMAIL_ADDRESS>work was funded by the NSF grant # IIS-1924978 and
Georgia Tech Institute for Robotics and Intelligent Machines Seed Grant.

###### Abstract

In this paper, we examine the problem of push recovery for bipedal robot
locomotion and present a reactive decision-making and robust planning
framework for locomotion resilient to external perturbations. Rejecting
perturbations is an essential capability of bipedal robots and has been widely
studied in the locomotion literature. However, adversarial disturbances and
aggressive turning can lead to negative lateral step width (i.e., crossed-leg
scenarios) with unstable motions and self-collision risks. These motion
planning problems are computationally difficult and have not been explored
under a hierarchically integrated task and motion planning method. We explore
a planning and decision-making framework that closely ties linear-temporal-
logic-based reactive synthesis with trajectory optimization incorporating the
robot’s full-body dynamics, kinematics, and leg collision avoidance
constraints. Between the high-level discrete symbolic decision-making and the
low-level continuous motion planning, behavior trees serve as a reactive
interface to handle perturbations occurring at any time of the locomotion
process. Our experimental results show the efficacy of our method in
generating resilient recovery behaviors in response to diverse perturbations
from any direction with bounded magnitudes.

##  I Introduction

As legged robots are increasingly deployed in complex environments, the need
for robots to accomplish tasks through symbolic planning and decision-making
becomes more apparent. Although locomotion robustness has been extensively
explored at the motion planning level, resilience to uncertainties and
external disturbances at the task planning level has been largely overlooked.
Hierarchically integrated task and motion planning (TAMP) is capable of
handling logical and whole-body dynamics objectives simultaneously. Unexpected
errors or even failures at the lower-level can lead to expensive re-planning
at the higher task planning level. On the other hand, high-level discrete task
plans can result in infeasible low-level motion plans. With these cascading
effects, novel TAMP methods are imperative to make robust locomotion decisions
resilient to environmental perturbations and enable robots to efficiently
recompute plans at both task and motion planning levels.

At the motion planning level, push recovery of bipedal locomotion has been
extensively studied in previous works and inspired by human locomotion
biomechanics [1, 2]. Various strategies such as hip, ankle, and foot placement
strategies are proposed to handle external perturbations [3, 4, 5]. However,
many of these push recovery strategies employ reduced-order models (RoMs) such
as inverted pendulum or centroidal momentum models, making it difficult to
guarantee leg self-collision avoidance. This challenge arises from solving
full-leg kinematic constraints in these RoMs. It is a strong assumption to
state that a robot will never be in close contact with itself in highly
dynamic locomotion. Liu et al. [6] demonstrated a complete control framework
that considers self-collision under various disturbances, but does not
consider more complicated multi-step or non-periodic recoveries. Reactive
approaches for high dimensional robots have also been explored [7, 8, 9],
which rely on a distance metric to generate safe repulsive motions, but can
lead to significant motion plan discrepancies. Behavior libraries have also
been used to generate robust real-time walking in unstructured or constrained
environments [10, 11]. In addition, few motion planning strategies incorporate
higher-level task planning.

Figure 1:  a) Human is forced to cross legs to recover from an external
disturbance. b) Human must execute leg crossing to traverse stepping stones.
c) An illustration of recovery motion of bipedal robot Cassie.

For high-level task planning, reactivity is critical to account for
environmental changes at runtime. Temporal-logic-based reactive synthesis [12,
13, 14] has been widely explored to find strategies that generate formally-
guaranteed safe and provably correct robot actions in response to
environmental events. However, this method has been under-explored for dynamic
locomotion problems until recent years. Recent works [15, 16, 17, 18] adopted
linear temporal logic (LTL) to synthesize reactive locomotion navigation plans
over rough terrains. Although bipedal walking only involves alternating left
and right foot contacts, incorporating external perturbations into formal foot
placement decision-making in a provably correct manner remains challenging.
Moreover, the feasibility of executing synthesized task plans on high degree-
of-freedom legged robots is unexplored. To address these challenges, this
study combines collision-avoidance-aware trajectory optimization (TO) with LTL
methods to guarantee the task completion of the robot locomotion.

Figure 2: Block diagram of the proposed framework. a) Experiments of Cassie
disturbed during stable walking; b) The high-level task planner synthesis,
employing an LTL two-player game; c) The BTs act as a middle layer that
reactively execute subtrees based on real-time environmental disturbances; d)
A whole-body motion planner is used to generate feasible motions and refine
LTL specifications $\psi$. The high-level task planner and the phase-space
planner are integrated in an online fashion as shown by the solid black
arrows.

Behavior Trees (BTs), as graphical mathematical models, have been widely
explored to schedule autonomous tasks and handle unexpected environmental
changes [19, 20]. Their reactive and modular structure can authorize multiple
behavioral plans and achieve fault-tolerant task executions [21, 22]. [23]
devised finite state machine (FSM) controllers for unexpected terrain height
variation, but relied on large handmade state machines. Intuitively speaking,
BTs can be viewed as a feature-rich, acyclic version of FSM for complex
behavior execution. Recent LTL-based reactive synthesis work [15] proposed
reactive TAMP in combination with robust reachability analysis for dynamic
maneuvers and disturbance rejection, but only accounts for perturbations
applied at specific instances. Formal methods handling perturbation at any
locomotion phase require further investigation. The BTs naturally handle the
continuous environmental perturbations by designing actions online to amend
the synthesized discrete automaton.

This study addresses the push recovery problem for legged robots subject to
external perturbations that can happen anytime. We propose a combined TAMP
framework composed of hierarchical planning layers operating at different
temporal and spatial scales (Fig. 2). First, the LTL planning designs safety-
guaranteed decisions on keyframe states, including center of mass (CoM) state
or foot placements, in response to the keyframe perturbations. When
perturbations occur at non-keyframe instants, analytical Riemannian manifolds
are used to recalculate a new keyframe transition online for the current
walking step. BTs are integrated to allow updated keyframes to be any
continuous value within the allowable range, instead of a finite set of
discrete values quantified in the LTL-based planner. Finally, full-body legged
motions are generated using kinodynamic-aware TO for non-periodic multi-step
locomotion with self-collision constraints. Compared to our previous robust
locomotion work [24, 25], this work (i) studies perturbation recovery from
comprehensive perturbations from all directions and during various locomotion
phases, and (ii) solves full-body TO to generate dynamically feasible
trajectories that will refine high-level decisions.

The core contributions of this paper are summarized as:

  * •

We present a hierarchically integrated LTL-BT TAMP framework for dynamic
locomotion that reacts to continuous environmental perturbations for resilient
task execution.

  * •

We employ Riemannian manifolds to quantify locomotion keyframe robustness
margins and design robust transitions enabled by the reactive task planner.

  * •

We propose a collision-aware, kinodynamic TO that generates collision-free and
non-periodic full-body motions and use this TO to refine feasibility
specifications in reactive synthesis.

##  II Planning Methods

This section details the symbolic decision-making and motion planning
framework (Fig. 2). Our hierarchical reactive framework is composed of (i)
LTL-level reactive synthesis handling perturbations at keyframe instants, (ii)
BT for robust execution of one walking step (OWS) between keyframe instances,
(iii) full-body motion primitive generation from kinodynamic-aware TO.

###  II-A Keyframe-based Non-periodic Locomotion

To define a multi-step walking motion for bipedal robot walking, we separate
the entire trajectory into multiple OWS phases that start and end at keyframe
states. The keyframe state is defined based on a step-to-step discretization
of the continuous walking process, allowing the robot to make CoM apex
parameter decisions for each walking step. The $i^{th}$ OWS cycle can be
represented by a discrete keyframe transition pair $(k^{i},k^{i+1})$. The
keyframe contains the sagittal and lateral CoM apex state, as well as the
stance foot index (Sec. II-B). Given two consecutive keyframe states in the
sagittal plane, forward and backward numerical integration is used to solve
for the contact switching time of a OWS ($t_{1}$ and $t_{2}$ for the first-
half and second-half OWS phases, respectively). Here $t_{1}$ and $t_{2}$ are
not fixed, therefore the contact switch timing is not constant and it enables
non-periodic locomotion. The numerical integration is based on the linear
inverted pendulum dynamics. The next sagittal keyframe will determine the next
lateral keyframe state. Namely, given two consecutive keyframe states in the
sagittal plane, the next lateral keyframe can be calculated by meeting the
$t_{1}$ and $t_{2}$ timing constraint, due to the simultaneous contact switch
in both directions. Then the lateral keyframe transition is determined as well
as the lateral CoM state $(p_{\rm switch,l},\dot{p}_{\rm switch,l})$ at
contact switch instant. The next lateral foot placement can be computed with
the analytical solution:

$\displaystyle p_{\rm foot,l}=p_{\rm switch,l}+\frac{(e^{2\omega_{\rm
asym}t_{2}}-1)\dot{p}_{\rm switch,l}}{(e^{2\omega_{\rm
asym}t_{2}}+1)\omega_{\rm asym}}$ (1)

where the asymptote slope $\omega_{\rm asym}=\sqrt{g/h_{\rm apex}}$ and
$h_{\rm apex}$ is the relative apex CoM height with respect to the stance foot
height. $g$ is the gravity constant. The subscript $\rm l$ indicates the
lateral space.

Compared to periodic walking, where the robot repeats the same motion pattern
periodically, keyframe-based walking allows for non-periodic walking that
better accommodates rough terrain and environment disturbances.

###  II-B LTL Specifications for Push Recovery

As the complexity of locomotion tasks increases, making safe decisions on
keyframe states to recovery from push becomes intricate. To address this
challenge, we employ reactive synthesis, which is built upon task
specifications and an abstraction of dynamical systems [26, 13]. The tasks are
represented by LTL specifications, which describe temporal and logical
relations of the system properties. The abstraction (i.e., transition system)
is a discrete description of the system and environment dynamics. An LTL
formula operates over atomic propositions (APs) that can be True
($\varphi\vee\neg\varphi$) or False ($\neg\textsf{True}$). The formulas use
logical symbols of negation ($\neg$), disjunction ($\vee$), and conjunction
($\wedge$). Temporal operators such as “next” ($\bigcirc$), “eventually”
($\Diamond$), and “always” ($\square$) are used as extensions to the
propositional logic. Detailed LTL semantics are omitted due to space limit and
can be found in [27].

To formally guarantee locomotion task completion under environmental
disturbances, we adopt the General Reactivity of Rank 1 (GR(1)) [28], a
fragment of LTL. GR(1) provides correct-by-construction guarantees of the
realizability of LTL specifications. Provided a transition system
$\mathcal{T}_{\rm SE}$ and LTL specification $\psi$, the reactive synthesis
problem aims for a winning strategy for the robot system such that the
execution path satisfies $\psi$ [17]. If the specification is realizable, an
automaton will be constructed and provide correct transitions for any
environmental actions obeying the assumptions.

Figure 3:  An illustration of a phase-space Riemannian partition and non-
deterministic lateral keyframe transition for disturbance recovery.

######  Definition II.1 (Riemannian partition)

The transition system discretizes the continuous robot state space (i.e.,
robot’s CoM phase-space near the apex state) into Riemannian partitions
defined as:

$\displaystyle\mathcal{R}$ $\displaystyle\coloneqq\mathcal{R}_{\rm
position}{\color[rgb]{0,0,0}\times}\mathcal{R}_{\rm velocity}$
$\displaystyle\,=\\{r_{p,n},r_{p,z},r_{p,p}\\}{\color[rgb]{0,0,0}\times}\\{r_{v,z},r_{v,s},r_{v,m},r_{v,f}\\}$

where the elements in $\mathcal{R}_{\rm position}$ define the relative
position (negative, zero, positive) of CoM with respect to the stance foot
frame, and $\mathcal{R}_{\rm velocity}$ defines the CoM apex velocity (zero,
slow, medium, fast). Riemannian partitions are defined for both sagittal and
lateral phase-space, each constitutes 12 cells.

Fig. 3 shows a disturbed keyframe state $(r_{p,n},r_{v,m})$, which represents
a negative position and medium velocity. A keyframe state whose CoM velocity
is zero in sagittal axis is noted as $(r_{v})_{s}=(r_{v,z})_{s}$. The
Riemannian partitions use the analytical manifolds of CoM dynamics derived
from the Prismatic Inverted Pendulum Model (PIPM). More details will be
introduced in Sec. II-E.

######  Definition II.2 (Locomotion keyframe)

A keyframe $\mathcal{K}$ is defined as a system apex state composed of the
sagittal partition $\mathcal{R}_{s}$, the lateral partition $\mathcal{R}_{l}$,
as well as the stance foot index set $\mathcal{F}_{\rm
st}=\\{\textsf{left},\textsf{right}\\}$ (used to identify the leg crossing or
wider lateral step strategies).

$\mathcal{K}\coloneqq\mathcal{R}_{s}{\color[rgb]{0,0,0}\times}\mathcal{R}_{l}{\color[rgb]{0,0,0}\times}\mathcal{F}_{\rm
st}.$

The system takes actions $a_{\rm sys}\in\mathcal{A}_{\rm
sys}\subseteq\mathcal{R}_{s}{\color[rgb]{0,0,0}\times}\mathcal{R}_{l}{\color[rgb]{0,0,0}\times}\mathcal{L}{\color[rgb]{0,0,0}\times}\mathcal{W}$
to decide the next keyframe state $k^{n}$.
$\mathcal{L}=\\{\textsf{small},\textsf{medium},\textsf{large}\\}$ and
$\mathcal{W}=\\{\textsf{small},\textsf{medium},\textsf{large}\\}$ represent
the step length and width. $l\in\mathcal{L}$ and $w\in\mathcal{W}$ are the
nominal distances between the current and the next foot placements projected
on sagittal and lateral axis. Note that $\mathcal{L},\mathcal{W}$ are the
nominal global distances between footholds while
$\mathcal{R}_{s},\mathcal{R}_{l}$ are the relative CoM apex states in nominal
foot frame.

The environment state is represented by a perturbation set $p_{\rm
env}\in\mathcal{P}_{\rm
env}\coloneqq\mathcal{R}_{s}{\color[rgb]{0,0,0}\times}\mathcal{R}_{l}\cup\\{\emptyset\\}$
that pushes the system to a specific Riemannian cell center. In the task
planner, we assume that the environment action is a perturbation only applied
at a keyframe instant. The perturbation induces a CoM position and velocity
jump after applying an external force to the robot’s pelvis frame. The
environment can also choose to not perturb, i.e., $p_{\rm env}=\emptyset$. The
system action $\mathcal{A}_{\rm sys}$ and environment action $\mathcal{P}_{\rm
env}$ together decide the next apex keyframe state $k^{n}=\mathcal{T}_{\rm
SE}(k^{c},a_{\rm sys},p_{\rm env})$. Both actions are a part of the automaton
state $\mathcal{S}$.

######  Definition II.3 (Steady state keyframe)

A special set of keyframes are defined as steady state keyframes $k^{\rm
ss}\in\mathcal{K}^{\rm ss}$ during perturbation-free walking.

$\mathcal{K}^{\rm ss}=\\{k^{\rm ss}|k^{\rm
ss}=\big{(}(r_{p,z},r_{v,\cdot})_{s},(r_{p,\cdot},r_{v,z})_{l},f_{\rm
st}\big{)}\\}$

where $(r_{p,z},r_{v,\cdot})_{s}$ means that the sagittal CoM apex position is
on top of the nominal foot placement and can take any allowable sagittal
velocities, while $(r_{p,\cdot},r_{v,z})_{l}$ means that the lateral CoM apex
position can take any values and the apex velocity has to be zero $r_{v,z}$
(see Fig. 3).

Let the system start from a steady state $k^{\rm ss}_{\rm
sys}=((r_{p,z},r_{v,m})_{s},(r_{p,z},r_{v,z})_{l},\textsf{right})$. We have

$\displaystyle s^{\rm init}$ $\displaystyle=(k^{\rm init},a_{\rm sys}^{\rm
init},p_{\rm env}^{\rm init})$ $\displaystyle=\big{(}k^{\rm
ss},((r_{p,z},r_{v,m})_{s},(r_{p,z},r_{v,z})_{l}),\emptyset\big{)}$

The robot chooses to maintain stable walking so long as there is no
perturbation from the environment. In the presence of perturbations, the
keyframe state returns to a steady state within two steps:

$\square\big{(}k=\neg k^{\rm ss}\Rightarrow(\bigcirc k=k^{\rm
ss})\vee(\bigcirc\bigcirc k=k^{\rm ss})\big{)}$

The feasibility of transitions must be verified by the low-level full-body TO
(Sec. II-F). Certain high-level transitions should be removed due to
infeasible full-body kinematics and dynamics constraints. In this way, we
define a set of TO-refined task specifications. For example, after the TO
refinement, we obtain all full-body-dynamics-feasible transitions offline and
encode TO-refined specifications. An example of TO-refined specification can
be:

$\displaystyle\square\big{(}k$
$\displaystyle=((r_{p,z},r_{v,m})_{s},(r_{p,z},r_{v,m})_{l},\textsf{right})\Rightarrow$
$\displaystyle a$
$\displaystyle=((r_{p,z},r_{v,m})_{s},(r_{p,z},r_{v,s})_{l},\textsf{small},\textsf{small})$
$\displaystyle...$ $\displaystyle\vee a$
$\displaystyle=((r_{p,z},r_{v,m})_{s},(r_{p,z},r_{v,m})_{l},\textsf{small},\textsf{medium})\big{)}$

In the presence of perturbations, recovering to a steady state $k^{\rm ss}$
requires the next keyframe $k^{n}$ to decrease the lateral apex velocity and
minimizes the sagittal apex deviation from its normal value. For example, a
current medium apex velocity indicates the next apex velocity is either
medium, small or zero: $\square\big{(}r_{v}=r_{v,m}\Rightarrow(\bigcirc
r_{v}=r_{v,m}\vee r_{v,s}\vee r_{v,z})\big{)}$. A smaller step width
$w\in\mathcal{W}$ will be chosen rather than larger ones.
$\square\big{(}(w=\textsf{small}\vee w=\textsf{large})\Rightarrow(\bigcirc
w=\textsf{small})\big{)}$.

For the recovery motion execution not to be interrupted, we assume the
environment perturbation happens at most once per two steps:
$\square\big{(}p_{\rm env}=\neg\emptyset\Rightarrow(\bigcirc p_{\rm
env}=\emptyset)\big{)}$

###  II-C Task Planner Synthesis

Given the LTL specifications above, the task planner models the robot system
and the environment interplay as a two-player game. We construct the keyframe
transition game structure in the form of a tuple
$\mathcal{G}:=(\mathcal{S},s^{\rm init},\mathcal{T}_{\rm SE})$ with:

  * •

$\mathcal{S}=\mathcal{K}\times\mathcal{A}_{\rm sys}\times\mathcal{P}_{\rm
env}$ is the possible automaton state of the transition system,

  * •

$s^{\rm init}=(k^{\rm init},a_{\rm sys}^{\rm init},p_{\rm env}^{\rm init})$ is
the initial automaton state and

  * •

$\mathcal{T}_{\rm SE}\subseteq\mathcal{S}\times\mathcal{S}$ is a transition
describing the possible moves of the robot system and antagonist environment.

In the extreme case where the disturbance is towards the stance leg (see Fig.
1), the foot placement of the swing leg would naturally move closer to the
stance foothold location or require a crossed-leg motion. A minimum of two
steps is required to recover in such case, during which self-collision poses a
challenge for making safe decisions. The TO-refined transition specifications
guarantee that the task planner makes dynamics-informed decisions on keyframe
transitions. By construction, the TO indicates that all the constraints on the
full-body motion are fulfilled and a keyframe transition is feasible.

At each keyframe instant, the decision maker uses the estimated current system
keyframe state $k^{c}$ and plans a sequence of transitions until the final
state $k^{f}=k^{\rm ss}$. The action roll-out produces an action plan
$P=\\{k^{c},\dots,k^{f}\\}$.

Figure 4:  An illustration of the PABT structure. The PABT groups a set of
locomotion subtrees $\Psi^{i}$. Each subtree is a fallback tree that encodes a
keyframe transition $(k^{c,i},k^{n,i})$ and a Riemannian recalculation action.

###  II-D Behavior-Tree-Based Dynamic Replanning

To address continuous perturbations at non-keyframe instants, we propose a
perturbation-aware behavior tree (PABT) that online modifies the desired
keyframe transition $(k^{c,d},k^{n,d})$. The PABT complements the reactive
synthesis by locally modifying the keyframe transitions, given the real-time
captured CoM state $(p_{\rm CoM},\dot{p}_{\rm CoM})$.

The PABT groups a set of locomotion subtrees
$\Psi=\bigcup\limits_{i}\Psi^{i}$. Each $\Psi^{i}$ encodes a pair of the
current-to-next keyframe states $(k^{c,i},k^{n,i})$. These pairs are
represented as condition nodes in the locomotion subtrees (Fig. 4). The
locomotion subtrees are fallback BTs that execute their action nodes when the
desired keyframe transition from the high-level matches their condition nodes.
For instance, the pre-condition nodes check if the desired transition
$k^{c,d}$ matches with their keyframe condition $k^{c,i}$, the same for the
post-condition nodes.

The PABT modifies its keyframe transitions locally to handle non-keyframe
perturbations. After the modification, the desired keyframe transition remains
feasible despite the CoM state deviation. The action node $A^{i}$ can also be
a keyframe recalculation procedure. Here we use the recovery strategy [24] to
perform a Riemannian recalculation, which recalculate the keyframe transition
when the CoM state is perturbed off from the nominal manifold. It is worth
noting that the PABT modified keyframe state may not be a Riemannian cell
center or even end up in a different cell.

1 Input: PABT $\Psi$, Decision Maker $DM$, current $time$;

2 Set: status = success;

3 while  _status == success _ do

4 $k^{c},time$ = StateEstimation();

5 if  _ $time$ == keyframe_instant_ then

6 $P=DM(k^{c})$;

7 for  _$(k^{c},k^{n})$ in $P$_ do

8 $\Psi^{c}$ = LocomotionSubtree$(k^{c},k^{n})$;

9 $\Psi$.Insert($\Psi^{c}$);

10

11 end for

12

13 end if

/* PABT Riemannian Recalculation */

14 $status$ = $\Psi$.Tick();

15 $(k^{c},k^{n})^{\prime}$ = $\Psi$.GetModifiedTransition();

16

17 end while

18Output: updated PABT $\Psi$, modified keyframe transition
$(k^{c},k^{n})^{\prime}$;

Algorithm 1 Keyframe Decision Making and PABT Execution

The PABT grows as the new action plan $P$ is commanded from the task planner.
The PABT constructs new subtrees $\Psi^{c}$ that represent the transitions
$(k^{c},k^{n})$ from $P$. The new subtrees are inserted under the root node as
new behaviors. A tick of the PABT will trigger the corresponding subtree that
matches the subtree conditions. The PABT expansion and execution process is
illustrated in Algorithm 1.

###  II-E Riemannian Robustness Margin Design

To quantify robustness margin, we use the Riemannian distance metric to
measure the deviation of CoM state from the nominal CoM manifolds in the CoM
phase-space. This Riemannian metric discretizes the phase-space with tangent
and cotangent locomotion manifolds, instead of using naïve Euclidean-type
discretization. The tangent and cotangent manifolds comply with the PIPM
locomotion dynamics and provide an intuitive trajectory recalculation strategy
for CoM deviation.

We use the position guard strategy [24] to recalculate the next CoM apex
state. Assuming the CoM state jumps to $(p_{\rm CoM}^{\prime},\dot{p}_{\rm
CoM}^{\prime})$ on a new tangent manifold $\sigma^{\prime}$, the recalculated
next CoM apex state is:

$\displaystyle(p_{\rm apex},\dot{p}_{\rm apex})=(p_{\rm
foot},\sqrt{\frac{\dot{p}_{\rm CoM}^{\prime 2}\pm\sqrt{\dot{p}_{\rm
CoM}^{\prime 4}-4\omega_{\rm asym}^{2}\sigma^{\prime}}}{2}})$ (2)

Note that the $(p_{\rm apex},\dot{p}_{\rm apex})$ corresponds to the next
keyframe $k^{n}$ at the LTL level. The motion primitive set interpolates a
full-body motion that connects the current CoM state to the updated next
keyframe.

###  II-F Collision-Aware Kinodynamic Trajectory Optimization

The task planner and PABTs generate keyframe transitions robust to
perturbations. However, mapping the transitions to whole-body trajectories in
real-time often poses a challenge due to the curse of dimensionality. To
address this, we use TO to create a set of motion primitives offline. The TO
generates desired motions that satisfy the physical constraints while
minimizing the trajectory cost [29, 30, 31]. The TO is also used as a
verification to check the feasibility of high-level keyframe transitions. The
nonlinear program (NLP) of TO is formulated as:

$\displaystyle\operatorname*{arg\,min}_{X}\quad$
$\displaystyle\sum_{j=1}^{D}\sum_{i=0}^{N_{j}}\Omega_{j}\cdot\mathcal{L}_{j}(x^{j}_{i},u^{j}_{i})$
(3) s.t. $\displaystyle
H_{j}(x^{j}_{i})\dot{x}_{i}+V_{j}(x^{j}_{i})+G_{j}(x^{j}_{i})=u_{i},\quad(\textnormal{dynamics})$
$\displaystyle
x_{0}^{j+1}=\Delta_{j}(x_{N_{j}}^{j}),\qquad\qquad\qquad\qquad\,\,(\textnormal{reset\,map})$
$\displaystyle\lambda_{c,z}\geq
0,\;|\lambda_{c,xy}|\leq\mu\lambda_{c,z},\qquad\qquad\quad\;\;\,(\textnormal{friction})$
$\displaystyle C_{j}^{\rm kin}(x^{j}_{i})\leq
0,\qquad\qquad\qquad\qquad\quad\,\,(\textnormal{kinematics})$ $\displaystyle
C_{j}^{\rm col}(x^{j}_{i})\leq
0,\qquad\qquad\quad\qquad\;\;\;\;\;\,(\textnormal{self-collision})$
$\displaystyle C_{j}^{\rm
key}(x^{j}_{i},u^{j}_{i})=0\qquad\qquad\;\;\;(\textnormal{keyframe\;boundary})$

where the domains include $D=2$ continuous single stance phases and one
velocity reset map. Each single stance contains $N_{j}$ nodes, which
represents a state-control pair $n^{j}_{i}=(x^{j}_{i},u^{j}_{i})$ at the
$i^{\rm th}$ instant; the state represents the $x=[q;\dot{q}]$ with $q$
denoting the robot generalized coordinate states. The NLP above solves the
optimal state-control trajectory $X^{*}=\\{n^{j*}_{i}\\}$ by minimizing the
pseudo energy $\mathcal{L}(\cdot)=||u_{i}||^{2}$ with weights $\Omega_{j}$
while enforcing the physical constraints of the robot.

The physical constraints shape the resultant trajectory. The dynamics
constraint is enforced between node points using Hermite-Simpson collocation.
$H$, $V$, and $G$ denote the inertia, coriolis, and gravity matrices of the
robot’s rigid body dynamics. We ignore the double stance phase and model the
discrete jump at the ground impact instant with
$x_{0}^{j+1}=\Delta_{j}(x_{N_{j}}^{j})$, which only maps a discrete jump of
the velocity component for the state vector [30]. The horizontal contact
forces $\lambda_{c,xy}$ are bounded by a linearized friction cone. The
kinematics constraints $C_{j}^{\rm kin}(x^{j}_{i})$ ensure that the joint
angles, foot positions, and CoM trajectories are bounded. $M$ geometric point
pairs $(\bm{g}^{m}_{l},\bm{g}^{m}_{r})$ on two legs are selected as self-
collision constraints (see Fig. 2d). The signed distances are evaluated at
each geometric point pair using forward kinematics $FK_{\bm{g}^{m}}(x_{i})$
for all $m\in M$, $i\in N_{j}$. The minimally allowed distance for pair $m$ is
denoted as $\bm{d}^{m}_{\rm min}$.

$\displaystyle\bm{d}^{m}(x_{i})$
$\displaystyle=FK_{\bm{g}^{m}_{l}}(x_{i})-FK_{\bm{g}^{m}_{r}}(x_{i}),$ (4)
$\displaystyle C_{j}^{\rm col}(x_{i})$ $\displaystyle=||\bm{d}^{m}_{\rm
min}||_{2}^{2}-||\bm{d}^{m}(x_{i})||_{2}^{2}.$

The keyframe transition $(k^{c},k^{n})$ commanded from the task planner is
enforced as boundary conditions for the foot placement, the apex CoM position
and velocity.

Figure 5:  Lateral and sagittal responses to diagonal disturbances at keyframe
and non-keyframe instants while walking at $0.5$ m/s apex velocity. Each color
represents a single step generated by the LTL-BT.

##  III Results

To demonstrate the robustness of the proposed methods, we tested various
scenarios in simulation using Matlab Simulink with a bipedal robot, Cassie.
The kinematics and dynamics functions were generated using the Fast Robot
Optimization and Simulation Toolkit (FROST)[30]. The NLP solver IPOPT[32]
solved the TO problems (3). Our framework, together with a virtual constraint
controller[11], ran at a rate of 2kHz online. Impulse forces were measured
through discontinuous changes in CoM velocity. We used SLUGS reactive
synthesis toolbox[33] to design LTL specifications and synthesized the
keyframe-based automaton.

For our crossed-leg experimentation, we used the 9 partitions with non-zero
apex velocities for $\mathcal{R}^{c}_{s}$, $\mathcal{R}^{c}_{l}$ and
$\mathcal{R}^{n}_{s}$, respectively. For each
$(r^{c}_{s},r^{c}_{l},r^{n}_{s})$ pair, phase-space planning calculated the
next $r^{n}_{l}$. This Riemannian abstraction provided $9\times 9\times 9=729$
possible crossed-leg transitions prior to the full-body TO. We evaluated the
feasible transitions and generated feasibility specifications. These
specifications encoded the feasible high-level keyframe transitions. For
stable walking and wider step recovery scenario, the lateral $r^{s}_{l}$ and
$r^{n}_{l}$ were the bottom three Riemannian partitions.

Figure 6:  Maximum allowable velocity change exerted on the CoM for a single
step at $30^{\circ}$ increments. The perturbation happens at different phases
during a right leg stance. Values on the left half resulted in single wider
step recoveries and values on the right half require crossed-leg maneuvers.

We evaluated the performance of our framework through multiple push recovery
studies. As shown in Fig. 5, the system was capable of composing multiple OWS
trajectories according to the reactive synthesis plan. The robot was firstly
disturbed to the non-apex velocity $(\dot{x},\dot{y})=(0.63,0.31)$ m/s at
keyframe instant. The keyframe decision maker planned a two-step recovery
strategy (one crossed-leg step and one succeeding wider step) to come back to
a steady state $k^{\rm ss}$. Disturbances at non-keyframe states required the
robot to recalculate a new CoM trajectory to an updated keyframe state. The
PABT locally modified the desired keyframe transition and allowed the
transitions to start and terminate in non-Riemannian-cell-centers. The
reactive synthesis could update the keyframe transitions as long as the CoM
state was inside the Riemannian robustness bound (grey areas in Fig. 5). This
preserved the notion of continuous recovery rather than that of a finite set
of discrete keyframe transitions.

Figure 7:  Success rate of recovery motion when disturbance happens anytime
during OWS at multiple directions. Three disturbances are used with a) small
0.1 m/s, b) medium 0.2 m/s, and c) large 0.3 m/s disturbances.

Figure 8:  Cassie is given a $0.4$ m/s laterally disturbance from a $0.5$ m/s
stable forward walking. Cassie successfully executes a leg crossing maneuver.

In Fig. 6, we compared the maximum impulse velocity changes the system can
recover from in 12 directions during OWS. The robot walked sagittally
(positive x direction) at $0.5$ m/s apex velocity. After the perturbation, it
was allowed to recover using up to two steps. When the push direction was
lateral left (positive y), the robot would take a wider step to come back at
$k^{\rm ss}$; otherwise, when the push direction was lateral right, the robot
needs to adopt the crossed-leg maneuvers. The perturbations are applied at 4
different phases, with phase $\phi=0\%$ and $90\%$ closer to keyframe states
(boundary phases), and $\phi=30\%$ and $60\%$ closer to the contact switch
phase ($50\%$). The result shows that the phases close to keyframes were
better at absorbing large left perturbations. Closer to the contact switch
phase, the right side push is handled better due to the increased lateral
velocity halfway through the step. The asymmetry of the maximum allowable
disturbances in the lateral directions can be attributed to the more
constrained kinematic workspace of the swing legs in the crossed-leg scenario.

We conducted an experiment to study the recovery success rate with 100 trials
in 4 directions (Fig. 7). Diagonal disturbances were applied at $45^{\circ}$
from the front to the right. For each trial, the robot took the same was
disturbed with 3 instantaneous velocity jumps of $0.1,0.2,0.3$ m/s. The
perturbations for each trial were spaced evenly ($\phi=1\%$) for the entire
phase duration. Failures primarily occurred at the point of maximum velocity
for the stance phase (right stance: $\phi\leq 10\%$ and $\phi\geq 90\%$, left
stance: $40\%\leq\phi\leq 60\%$). Similar trends were seen in the maximum
velocity disturbances Fig. 6.

Finally, the tracking performance for the system was evaluated for $\pm 0.4$
m/s lateral disturbances while the left leg was in stance, during a stable
walking with $0.5$ m/s apex velocity. The positive disturbance forced a two-
step crossed-leg recovery (Fig. 8) and had a RMS tracking error of $0.0084$ m
and $0.0593$ m/s. For negative lateral disturbances, the system stabilized
within one wide step with a RMS tracking error of $0.0039$ m and $0.0363$ m/s
in lateral phase-space.

##  IV Conclusion

In this paper, we presented a locomotion framework for reactive disturbance
rejection at the symbolic decision-making and continuous motion planning
level. We combined reactive synthesis with BTs to demonstrate safe,
continuous, disturbance rejection capabilities. At the low level, the TO
generates full-body locomotion trajectories and refines feasible keyframe
specifications in the reactive synthesis to fill the gap between the high-
level decisions making and the low-level full-body motion planning.

## References

  * [1] B. J. Stephens, Push recovery control for force-controlled humanoid robots.  PhD thesis, Carnegie Mellon University, 2011.
  * [2] P.-b. Wieber, “Trajectory free linear model predictive control for stable walking in the presence of strong perturbations,” in IEEE-RAS International Conference on Humanoid Robots, pp. 137–142, 2006.
  * [3] J. Pratt, J. Carff, S. Drakunov, and A. Goswami, “Capture point: A step toward humanoid push recovery,” in IEEE-RAS international conference on humanoid robots, pp. 200–207, 2006.
  * [4] S.-J. Yi, B.-T. Zhang, D. Hong, and D. D. Lee, “Online learning of a full body push recovery controller for omnidirectional walking,” in IEEE-RAS International Conference on Humanoid Robots, pp. 1–6, 2011.
  * [5] M. Shafiee, G. Romualdi, S. Dafarra, F. J. A. Chavez, and D. Pucci, “Online dcm trajectory generation for push recovery of torque-controlled humanoid robots,” in IEEE-RAS International Conference on Humanoid Robots, pp. 671–678, 2019.
  * [6] C. Liu, J. Ning, K. An, and Q. Chen, “Active balance of humanoid movement based on dynamic task-prior system,” International Journal of Advanced Robotic Systems, vol. 14, no. 3, 2017.
  * [7] C. Zhou, C. Fang, X. Wang, Z. Li, and N. Tsagarakis, “A generic optimization-based framework for reactive collision avoidance in bipedal locomotion,” in IEEE International Conference on Automation Science and Engineering, pp. 1026–1033, 2016.
  * [8] A.-C. Hildebrandt, R. Wittmann, D. Wahrmann, A. Ewald, and T. Buschmann, “Real-time 3d collision avoidance for biped robots,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4184–4190, 2014\.
  * [9] A. Dietrich, T. Wimböck, H. Täubig, A. Albu-Schäffer, and G. Hirzinger, “Extensions to reactive self-collision avoidance for torque and position controlled humanoids,” in IEEE International Conference on Robotics and Automation, pp. 3455–3462, 2011.
  * [10] Q. Nguyen, X. Da, J. Grizzle, and K. Sreenath, “Dynamic walking on stepping stones with gait library and control barrier functions,” in Workshop on the Algorithmic Foundations of Robotics, 2016.
  * [11] Y. Gong, R. Hartley, X. Da, A. Hereid, O. Harib, J.-K. Huang, and J. Grizzle, “Feedback control of a cassie bipedal robot: Walking, standing, and riding a segway,” in American Control Conference, pp. 4559–4566, 2019.
  * [12] H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based reactive mission and motion planning,” IEEE Transactions on Robotics, vol. 25, no. 6, pp. 1370–1381, 2009.
  * [13] J. Liu, N. Ozay, U. Topcu, and R. M. Murray, “Synthesis of reactive switching protocols from temporal logic specifications,” IEEE Transactions on Automatic Control, vol. 58, no. 7, pp. 1771–1785, 2013.
  * [14] K. He, A. M. Wells, L. E. Kavraki, and M. Y. Vardi, “Efficient symbolic reactive synthesis for finite-horizon tasks,” in International Conference on Robotics and Automation, pp. 8993–8999, 2019.
  * [15] Y. Zhao, Y. Li, L. Sentis, U. Topcu, and J. Liu, “Reactive task and motion planning for robust whole-body dynamic locomotion in constrained environments,” The International Journal of Robotics Research, In Press, 2022.
  * [16] S. Kulgod, W. Chen, J. Huang, Y. Zhao, and N. Atanasov, “Temporal logic guided locomotion planning and control in cluttered environments,” in American Control Conference, pp. 5425–5432, 2020.
  * [17] J. Warnke, A. Shamsah, Y. Li, and Y. Zhao, “Towards safe locomotion navigation in partially observable environments with uneven terrain,” in IEEE Conference on Decision and Control, pp. 958–965, 2020.
  * [18] A. Shamsah, J. Warnke, Z. Gu, and Y. Zhao, “Integrated task and motion planning for safe legged navigation in partially observable environments,” arXiv preprint arXiv:2110.12097, 2021.
  * [19] A. Marzinotto, M. Colledanchise, C. Smith, and P. Ögren, “Towards a unified behavior trees framework for robot control,” in IEEE International Conference on Robotics and Automation, pp. 5420–5427, 2014.
  * [20] S. Li, D. Park, Y. Sung, J. A. Shah, and N. Roy, “Reactive task and motion planning under temporal logic specifications,” IEEE International Conference on Robotics and Automation, pp. 12618–12624, 2021.
  * [21] M. Colledanchise and P. Ögren, Behavior trees in robotics and AI: An introduction.  CRC Press, 2018.
  * [22] M. Iovino, E. Scukins, J. Styrud, P. Ögren, and C. Smith, “A survey of behavior trees in robotics and ai,” arXiv preprint arXiv:2005.05842, 2020\.
  * [23] H.-W. Park, A. Ramezani, and J. W. Grizzle, “A finite-state machine for accommodating unexpected large ground-height variations in bipedal robot walking,” IEEE Transactions on Robotics, vol. 29, no. 2, pp. 331–345, 2013\.
  * [24] Y. Zhao, B. R. Fernandez, and L. Sentis, “Robust optimal planning and control of non-periodic bipedal locomotion with a centroidal momentum model,” The International Journal of Robotics Research, vol. 36, no. 11, pp. 1211–1242, 2017.
  * [25] Y. Zhao, B. R. Fernandez, and L. Sentis, “Robust phase-space planning for agile legged locomotion over various terrain topologies.,” in Robotics: Science and Systems, vol. 12, 2016.
  * [26] H. Kress-Gazit, T. Wongpiromsarn, and U. Topcu, “Correct, reactive, high-level robot control,” IEEE Robotics & Automation Magazine, vol. 18, no. 3, pp. 65–74, 2011.
  * [27] C. Baier and J.-P. Katoen, Principles of model checking.  MIT press, 2008.
  * [28] N. Piterman, A. Pnueli, and Y. Sa’ar, “Synthesis of reactive(1) designs,” in Verification, Model Checking, and Abstract Interpretation, pp. 364–380, Springer, 2006.
  * [29] A. Rao, “A survey of numerical methods for optimal control,” Advances in the Astronautical Sciences, vol. 135, 01 2010.
  * [30] A. Hereid and A. D. Ames, “Frost: Fast robot optimization and simulation toolkit,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 719–726, 2017.
  * [31] M. Koptev, N. Figueroa, and A. Billard, “Real-time self-collision avoidance in joint space for humanoid robots,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1240–1247, 2021.
  * [32] A. Wächter and L. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical programming, vol. 106, pp. 25–57, 03 2006.
  * [33] R. Ehlers and V. Raman, “Slugs: Extensible gr(1) synthesis,” in International Conference on Computer Aided Verification, pp. 333–339, 2016.