# Socially Acceptable Bipedal Robot Navigation via Social Zonotope Network
Model Predictive Control

Abdulaziz Shamsah1,2, Krishanu Agarwal3, Nigam Katta3, Abirath Raju4, Shreyas
Kousik1,∗, and Ye Zhao1,∗ 1George W. Woodruff School of Mechanical
Engineering, Georgia Institute of Technology, Atlanta, GA, 30332-0405 USA.
<EMAIL_ADDRESS>Engineering Department, College of Engineering
and Petroleum, Kuwait University, PO Box 5969, Safat, 13060, Kuwait3School of
Electrical and Computer Engineering, Georgia Institute of Technology, Atlanta,
GA 30308, USA.5Wallace H. Coulter Department of Biomedical Engineering,
Georgia Institute of Technology, Atlanta, GA 30308, USA.∗Co-advising
authorships.

###### Abstract

This study addresses the challenge of social bipedal navigation in a dynamic,
human-crowded environment, a research area largely underexplored in legged
robot navigation. We present a zonotope-based framework that couples
prediction and motion planning for a bipedal ego-agent to account for
bidirectional influence with the surrounding pedestrians. This framework
incorporates a Social Zonotope Network (SZN), a neural network that predicts
future pedestrian reachable sets and plans future socially acceptable
reachable set for the ego-agent. SZN generates the reachable sets as zonotopes
for efficient reachability-based planning, collision checking, and online
uncertainty parameterization. Locomotion-specific losses are added to the SZN
training process to adhere to the dynamic limits of the bipedal robot that are
not explicitly present in the human crowds data set. These loss functions
enable the SZN to generate locomotion paths that are more dynamically feasible
for improved tracking. SZN is integrated with a Model Predictive Controller
(SZN-MPC) for footstep planning for our bipedal robot Digit. SZN-MPC solves
for collision-free trajectory by optimizing through SZN’s gradients. and Our
results demonstrate the framework’s effectiveness in producing a socially
acceptable path, with consistent locomotion velocity, and optimality. The SZN-
MPC framework is validated with extensive simulations and hardware
experiments.

###### Index Terms:

Social navigation, bipedal robot locomotion, reachability, collision
avoidance, path planning, neural networks.

Note to Practitioners— This paper is motivated by the challenge of navigating
bipedal robots through dynamic, human-crowded environments in a socially
acceptable manner. Existing methods for social navigation often only address
obstacle avoidance and are demonstrated on a robot with simple dynamics. This
paper proposes the Social Zonotope Network (SZN), a novel neural network that
couples pedestrian future trajectory prediction and robot motion planning to
facilitate socially aware navigation for bipedal robots such as Digit,
designed by Agility Robotics.

The social behaviors are learned from real open-sourced pedestrian data using
the SZN, which outputs the future predictions as reachable sets for each agent
in the environment. The SZN is then integrated into a trajectory optimization
problem that takes into account personal space preferences and bipedal robot
capabilities to design trajectories that are both collision-free and socially
acceptable. This work also highlights the computational efficiency of the SZN
design that makes it suitable for real-time integration with motion planners.
The framework is validated through extensive simulations and hardware
experiments.

From a practical standpoint, this research provides a framework that can be
applied to bipedal robots to improve automation in human-populated
environments such as hospitals, shopping centers, and airports. The
framework’s ability to automatically adapt to surrounding human movement helps
minimize disruptions and ensures that the robot’s presence is not a hindrance
to the flow of human traffic. Future work will focus on outdoor deployment,
which will require onboard perception capabilities to detect surrounding
pedestrians.

Figure 1: (top) Snapshot of the proposed social path planner demonstrated on
hardware with $5$ pedestrians with pedestrian’s prediction zonotopes (green),
ego-agent’s social zonotope (cyan), and goal location (yellow star)
superimposed. (bottom) shows a top-down view of the ego-agent’s path,
pedestrians’ prediction, and social zonotopes.

Figure 2: Block diagram of the proposed framework. The framework is developed
around the Social Zonotope Network (SZN) (Sec. V), which is composed of two
sub-networks: the Pedestrian Prediction Network (PPN) and the Ego-agent Social
Network (ESN) shown in green and cyan, respectively. Given an environment with
observed pedestrians and a goal location, the social path planner designs a
social path for Digit (Sec. VII-C). At the middle level, SZN-MPC optimizes
through SZN to generate both collision-free and socially acceptable
trajectories for Digit (Sec. VII). The optimal trajectory is then sent to the
ALIP controller [1] to generate the desired foot placement for reduced-order
optimal trajectory tracking. An ankle-actuated-passivity-based controller [2,
3] is implemented on Digit for full-body trajectory tracking. Digit current
velocity and the optimal trajectory from SZN-MPC are used in the modeling
error GP to compensate for the modeling uncertainty between ROM dynamics and
full-order dynamics (Sec. VI-B).

##  I Introduction

Bipedal locomotion in complex environments has garnered substantial attention
in the robotics community [4, 5, 6, 7, 8, 9, 10, 11]. Social navigation poses
a particular challenge due to the inherent uncertainty of the environment,
unknown pedestrian dynamics, and implicit social behaviours [12]. Recently,
there has been an increasing focus on social navigation for mobile robots in
human environments [13, 14, 15, 16, 17, 18, 19]. Using mobile robots benefits
from stable motion dynamics, which facilitate the investigation of high-level
social path planners. In contrast, the exploration of social navigation for
bipedal robots remains unexplored. This is largely attributed to the
intricacies of the hybrid, nonlinear, and high degrees-of-freedom dynamics
associated with bipedal locomotion. The complexities inherent in stabilizing
and controlling bipedal robots have led researchers to prioritize fundamental
locomotion research in the past [20, 21, 22, 23], delaying advancements in
deploying these robots into more complex dynamic environments such as human-
populated ones (see Fig. 1).

In this study, we present an integrated framework for prediction and motion
planning for socially acceptable bipedal navigation as shown in Fig. 2. We
propose a Social Zonotope Network (SZN) that both predicts pedestrians’ future
trajectories and plans a socially acceptable path for the bipedal robot Digit
[24] with 28 degrees of freedom (DoFs). SZN is trained with locomotion-
specific losses to improve full-body tracking of the desired socially
acceptable paths.

Our SZN model outputs reachable sets for both pedestrians and the ego-agent
represented as zonotopes, a class of convex symmetric polytopes. Zonotopes
enable efficient and robust reachability-based planning, collision checking,
and uncertainty parameterization [25, 26, 27, 28]. In this work, we detect and
avoid collisions between zonotopes corresponding to the ego-agent and
pedestrians. Additionally, the ego-agent’s zonotope is refined to compensate
for (1) robot modeling errors of the ego-agent’s dynamics via Gaussian process
(GP) regression, and (2) ego-agent’s personal space for increased social
acceptance in practice. Zonotopes provide a balance between geometric
complexity and computational efficiency. We specifically take advantage of two
facts: (1) the Minkowski sum of two zonotopes is again a zonotope, allowing us
to easily augment the zonotopes output by a neural network; and (2) collision
checking a pair of zonotopes can be differentiated for use in gradient-based
motion planning methods.

Our framework integrates SZN in a model predictive controller (MPC) at the
middle level as shown in Fig. 2. SZN-MPC optimizes over the output of the
neural network, by encoding it as reachability and collision avoidance
constraints. We incorporate a novel cost function designed to encourage the
generation of socially acceptable trajectories. SZN-MPC employs a reduced-
order model (ROM), i.e., a Linear Inverted Pendulum (LIP) model, for the
bipedal locomotion process, and then sends optimal commands, i.e., center of
mass (CoM) velocity and heading change, to the low-level controller on Digit
for full-body joint trajectory design and control.

###  I-A Contributions and Outline

The main contributions of this study are as follows.

  * •

A reachability-based prediction and planning framework for bipedal navigation
in a social environment: we introduce the Social Zonotope Network (SZN), a
CVAE architecture for coupled pedestrian future trajectory prediction and ego-
agent social planning both parameterized as zonotopes.

  * •

Learning locomotion safety: we encode locomotion safety into the learning
module using signal temporal logic (STL) specifications, and design its
robustness score as loss functions of the CVAE network during training.

  * •

Online zonotope refinements for social acceptability and model discrepancy
compensation: we leverage the adaptable nature of zonotope parameterization to
enhance social acceptability by incorporating personal space refinements and
to efficiently adjust online for learned model discrepancies between the
reduced-order model (ROM) and the full-order model of Digit.

  * •

SZN-MPC: we integrate the SZN with a MPC, where the zonotopes outputted by SZN
are encoded as constraints for reachibility-based motion planning and
collision checking.

  * •

Social acceptability cost function design: a novel MPC cost function to plan
trajectories for the ego-agent using learned paths from human data sets.

  * •

Experimental hardware evaluation: we implement the proposed framework on a
bipedal robot Digit equipped with a low-level passivity controller for full-
body joint control.

This article is organized as follows. Sec. II is a literature review of
related work. Sec, III introduces the problem we are seeking to solve. Then,
the robot dynamics, environment setup and zonotope preliminaries are in Sec.
IV. Sec. V presents SZN’s architecture and loss functions. Zonotope
refinements for social acceptability and uncertainty parameterization in Sec.
VI. Sec. VII formulates the problem as a MPC and introduces the social
acceptability cost. Implementation details and results are in Sec. VIII. In
Sec. IX we discuss the limitations of the proposed framework. Finally, Sec. X
concludes this article.

This paper expands upon a previous conference version [29]. The work presented
here extends the SZN training to include additional locomotion-specific loss
functions, incorporates a social acceptability cost function in the SZN-MPC,
and introduces a coupled SZN-MPC for simultaneous pedestrian prediction and
ego-agent planning. Additionally, this paper refines the zonotope
parameterization by incorporating personal space modulation and uncertainty
parameterization. We also benchmark the collision-avoidance performance of our
method with a control barrier function baseline and validate our framework by
extensive hardware experiments that were not included in the conference
version.

##  II Related Work

This work lies in the intersection of social navigation, human trajectory
prediction, gradient-based safe motion planning, and bipedal locomotion. We
now review each of these topics.

Figure 3: Comparison between two different designs of socially acceptable
paths. (a) shows a minimally invasive path design for the robot as in [30,
17], i.e., the robot adjusts its path to not alter the pedestrian’s original
path or change it in a minimally-invasive way. (b) shows the bidirectionally
influenced path between the robot and the pedestrian that our work employs,
i.e., the robot and the pedestrian mutually react to each other and adjust
their own paths accordingly.

###  II-A Social Navigation

Social navigation literature can be categorized into two main categories
coupled prediction and planning and decoupled prediction and planning [12]. In
this section, we will present different approaches in the literature and
conclude where our proposed work falls in the social navigation field.

####  II-A1 Coupled Prediction and Planning

In coupled prediction and planning literature, the proposed frameworks
recognize the influence of the robot motion on the surrounding pedestrians and
vice versa [12]. Navigating an environment with humans in a socially compliant
manner requires a proactive approach to motion planning [31, 30, 17, 19]. In
[19], the authors use coupled opinion dynamics to proactively design motion
plans for a mobile robot, without the need for human prediction models. It’s
an implicit approach, as it relies only on the observation of the approaching
human position and orientation to form an opinion that alters the nominal path
and avoids collisions with pedestrians. Another implicit approach is [32],
which uses inverse reinforcement learning to learn robot motions that mimics
human behavior.

On the other hand, the approaches in [30, 17] are explicit, as the framework
explicitly predicts the future trajectories of pedestrians. The authors in
[30] propose a social interference metric based on Kullback-Leibler divergence
to measure the interference of the robot’s path plan on the surrounding
humans’ future trajectory. Hypothesizing that minimizing the social
interference metric will result in a socially acceptable trajectory for the
ego-agent. Similarly, a gradient-based trajectory optimization is introduced
in [17] to minimize the difference between the humans’ future path prediction
conditioned on the robot’s plan and the nominal prediction.

The studies of [30, 17] both work under the assumption that a minimally-
invasive robot trajectory, with minimal effect on surrounding humans’ nominal
trajectory, is socially acceptable (see Fig. 3(a)). In contrast, our work
leverages a different notion of social acceptability, where we aim to learn
the socially acceptable trajectory of the ego-agent from human crowd datasets
to avoid any heuristics or biases on prioritizing the minimal invasiveness of
the pedestrians as the socially acceptable metric. Our method enables
bidirectional influence between the pedestrians and the ego-agent, and allows
the ego-agent to change surrounding pedestrians’ paths (see Fig. 3(b)).

####  II-A2 Decoupled Prediction and Planning

In decoupled prediction and planning, social navigation can be viewed as
dynamic obstacle avoidance [12]. The work in [33] ignores any effect the robot
path has on the surrounding pedestrians. They propose a simple linear Kalman
filter to predict the pedestrians’ future paths and an MPC for collision
avoidance. This approach while effective in a small number of pedestrians,
ignoring the coupling effect between the robot and the pedestrians will result
in the reciprocal dance problem, i.e., an oscillatory interaction between the
robot and the pedestrians [34], or the so-called ”freezing robot problem”
[35], i.e., the robot comes to a stop to avoid collisions.

Based on the aforementioned categories, our framework uses a coupled
prediction and planning approach, as the pedestrians’ future predictions are
conditioned on the ego-agent’s future planned motion. Our approach integrates
both explicit and implicit elements. Explicitly, we predict the future
trajectories of pedestrians and utilize these predictions for collision
avoidance. Implicitly, we leverage learned trajectories from pedestrian
datasets to generate ego-agent trajectories that mimic human behavior.

###  II-B Human Trajectory Prediction

Our framework is inspired by the human trajectory prediction community, such
as Trajectron++ [36], SocialGAN [37], PECNet [38], Y-net [39], Sophie [40],
and STAR [41] where we aim to design a socially acceptable trajectory for the
ego-agent that mimics the path from human crowd datasets. The work in [42]
proposes an obstacle avoidance learning method that uses a Conditional
Variational Autoencoder (CVAE) framework to learn a temporary, near-horizon
target distribution to avoid pedestrians actively. However, during the
training phase, the temporary targets are selected heuristically. In contrast,
we aim to learn such temporary waypoints from human crowd datasets to capture
a heuristic-free socially acceptable path. In [38], the authors develop a
simple but accurate CVAE architecture based on Multi-Layer Perceptrons (MLP)
networks to predict crowd trajectories conditioned on past observations and
intermediate endpoints. Our SZN inherits a similar MLP-based CVAE
architecture, where the ego-agent path is conditioned on the final goal
location and surrounding pedestrians’ future trajectories. In addition, the
SZN is also conditioned on the immediate change in the ego-agent state to be
better integrated with MPC for planning. Leveraging a simple network
architecture in our work, i.e., MLP, instead of other advanced architecture
such as LSTMs [36, 25] or Transformers [41], enables real-time planning and
prediction when integrated into gradient-based motion planning for the ego-
agent.

###  II-C Neural-Network-based Motion Planning

The work in [17, 25] both leverage the neural network gradients into a
trajectory optimization (TO) problem for safe motion planning. The method
introduced in [17] employs [36] for generating multimodal probabilistic
predictions and integrates the prediction model gradients in the cost function
of the TO problem, to minimize the invasiveness of the robot’s path to the
surrounding pedestrians’ paths (as previously discussed in Sec. II-A).

However, most prediction frameworks generate a discrete time representation of
the trajectories posing challenges to guarantee safety. Leveraging zonotopes
as reachable sets provides a robust framework for ensuring safety in robot
models by effectively capturing inherent uncertainties in both models and
dynamics [26, 27, 25]. The work of [25] presents a Zonotope Alignment of
Prediction and Planning (ZAPP) that relies on zonotopes to enable continuous-
time reasoning for planning. They use Trajectron++ [36] to predict the
obstacle trajectories as a Gaussian distribution. They construct a zonotope
over these distributions, which leads to an overapproximation of the
uncertainties. The predictive model gradients are used in the TO constraints
for obstacle avoidance [25], rather than in the cost function as designed in
[17].

Compared to [17, 25], we propose learning path prediction distributions
directly as zonotopes, bypassing the initial step of predicting Gaussian
distributions for pedestrian motion. As such, our approach is computationally
efficient and facilitates real-time integration with an MPC, while an online
implementation of [25] is challenging. The learned zonotopes also provide the
gradients for the constraints in the proposed MPC for reachability-based
planning and collision checking.

###  II-D Bipedal Locomotion

Bipedal locomotion has been extensively studied with a wide spectrum of model
representations and methods—such as reduced-order model (ROM) [5, 6, 43, 7],
single rigid body model [44], centroidal model [45], whole-body models [22,
23, 46], ROM-inspired reinforcement learning [47], and model-free
reinforcement learning approaches [48, 49]—to name a few. Generally,
locomotion planning using whole-body models renders a high computation cost
[50, 51] and becomes much less efficient for navigation in complex
environments that often involve long-horizon planning; therefore, in this
work, we focus on ROM-based methods for CoM trajectory and foot placement
planning.

The work in[5] uses an omnidirectional differential-drive wheeled robot model
with a preference for sagittal movement to capture bipedal robots’ behaviors
accurately. They design a control Lyapunov function (CLF) to drive and orient
that robot towards the goal and a control barrier function (CBF) for obstacle
avoidance. The framework is demonstrated for indoor and outdoor navigation in
environments with static obstacles only. In our work, we use Linear Inverted
Pendulum (LIP) [52] as ROM for our bipedal robot. A similar model is used in
[6], while [53, 7] use an angular momentum-based LIP (ALIP) [1] for improved
prediction of CoM velocities.

The authors in [6] present discrete-time CBF (DCBF) [54] constraints with a
sequential LIP-based MPC to plan trajectories for Digit in an environment with
static and dynamic obstacles. However, unlike our approach where we predict
the future trajectory of the pedestrians, the dynamic obstacles trajectory is
assumed to be known in [6] and the framework is only demonstrated in
simulation. The work in [53] also uses a DCBF-MPC to avoid static obstacles in
simulation.

In contrast to the aforementioned studies, the overarching goal of this study
is to develop a bipedal locomotion planner that can generate safe paths while
considering social norms and can be efficiently solved in real-time for
hardware implementation. To this end, we propose SZN-MPC, a LIP-based MPC with
SZN to predict reachable sets of the obstacles for collision avoidance and to
plan a socially acceptable, zonotope-based path for the ego-agent. SZN-MPC
computes the desired ego-agent trajectory, which is then sent to the ALIP
controller [1] for foot placement generation and the passivity controller [2]
for low-level joint control with ankle-actuation for improved ROM tracking
[3].

##  III Problem formulation

We seek to safely navigate a bipedal robot through a crowd. We now present our
robot model and environment model, and then formalize this problem.

###  III-A Robot Model

Figure 4: Illustration of the Linear Inverted Pendulum model for two
consecutive foot contact switching states $\bm{x}_{q}$ and $\bm{x}_{q+1}$. The
shaded yellow regions indicate the kinematics constraint on the control input
$\bm{u}$ detailed in Sec. VII-A.

We leverage a locomotion-specific reduced order model (ROM) to describe our
bipedal robot dynamics in this study. Consider a bipedal ego-agent with
discrete time dynamics $\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$, where
$\bm{x}_{q}$ and $\bm{u}_{q}$ are the state and control input respectively at
the $q^{\rm th}$ walking step111The robot model used in our study represent
step-by-step dynamics, i.e., $\bm{x}_{q}$ and $\bm{x}_{q+1}$ are the CoM state
at the foot contact switching instant of two consecutive walking steps. The
state of the bipedal robot, i.e., the ego-agent, is $\bm{x}=(\bm{p}^{\rm
ego},v^{\rm loc},\theta)$ where $\bm{p}^{\rm ego}=(x,y)$ is the 2-D location
in the world coordinate, $v^{\rm loc}$ is the sagittal velocity at the foot
contact switching instant in the local coordinate, and $\theta$ is the
heading. The control input is $\bm{u}_{q}=(u^{f}_{q}\quad
u^{\Delta\theta}_{q})$, where $u^{f}_{q}$ is the sagittal foot position
relative to the CoM, and $u^{\Delta\theta}_{q}$ is the heading angle change
between two consecutive walking steps. A schematic robot model is shown in
Fig. 4.

###  III-B Environment Setup and Problem Statement

The ego-agent is tasked to navigate to a known goal location $\mathcal{G}$ in
an open environment with $m\in\mathbb{N}$ observed pedestrians treated as
dynamic obstacles. Denote the pedestrian state
$\mathcal{T}^{p_{k}}_{[t_{p},t]}$, which is the 2-D trajectory of $k^{\rm th}$
pedestrian observed over a discrete time interval $[t_{p},t]$, where
$t_{p}<t$. The environment is partially observable as only the pedestrians in
a pre-specified sensory radius of the ego-agent are observed. The path the
ego-agent takes, should ensure locomotion safety, navigation safety, as well
as promoting social acceptability.

######  Definition III.1 (Locomotion safety).

For bipedal robots, locomotion safety is defined as maintaining balance
dynamically throughout its locomotion process.

######  Definition III.2 (Navigation safety).

Navigation safety is defined as maneuvering in human crowded environments
while avoiding collisions with pedestrians, i.e., $\|\bm{p}^{\rm
ego}_{t}-\mathcal{T}^{p_{k}}_{t}\|>d,\;\forall\ t,k\in m$, where $\bm{p}^{\rm
ego}_{t}$ denotes the ego-agent 2-D position and $d$ represent the minimum
allowable distance between the ego-agent and the pedestrians.

######  Definition III.3 (Socially acceptable path for bipedal systems).

A path that a bipedal ego-agent takes in a human-crowded environment is deemed
socially acceptable if it has an Average Displacement Error (ADE) $<\epsilon$
when compared to ground truth human data navigating in the same
environment222$\epsilon$ represents the allowable deviation from the socially
acceptable path. The Average Displacement Error denotes the average error
between the planned path and the ground-truth path..

Based on the aforementioned definitions and environment setup the problem we
seek to solve is as follows:

######  Problem III.1.

Given the discrete dynamics of the bipedal robot
$\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ and an environment state
$\mathcal{E}=(\mathcal{T}^{p_{k}}_{[t_{p},t]},\mathcal{G})$, find an optimal
motion plan for the bipedal robot and a navigation path that promotes social
acceptability for the bipedal ego-agent in a partially observable environment
containing pedestrians while ensuring locomotion and navigation safety.

##  IV Preliminaries

This section introduces the dynamic model used for the robot, learning and
environment assumptions, and zonotopes.

###  IV-A Robot Walking Model

The ROM used to design the 3-D walking motion of Digit, as introduced in the
problem formulation (Sec. III), is the linear inverted pendulum (LIP) model
[52]. For the LIP model we assume that each walking step has a fixed
duration333set to be equal to the timestep between frames in the dataset
($0.4$ s) $T$ [6, 53]. Then we build our model on the discrete sagittal
dynamics444the lateral dynamics are only considered in the ALIP model at the
low level since they are periodic with a constant desired lateral foot
placement (See Fig. 2) $(\Delta x^{\rm loc}_{q},v^{\rm loc}_{q})$, where
$x^{\rm loc}_{q}$ is CoM position at the beginning of the $q^{\rm th}$ step,
$\Delta x^{\rm loc}=x^{\rm loc}_{q+1}-x^{\rm loc}_{q}$ is the local sagittal
CoM position difference between two consecutive walking steps, and $v^{\rm
loc}_{q}$ is the sagittal velocity at the local coordinate for the $q^{\rm
th}$ walking step as shown in Fig. 4 (see Appendix. A for detailed
derivation):

$\Delta x^{\rm loc}(u^{f}_{q})=\left(v^{\rm loc}_{q}\frac{\sinh(\omega
T)}{\omega}+(1-\cosh(\omega T))u^{f}_{q}\right)$ (1) $v^{\rm
loc}_{q+1}(u^{f}_{q})=\cosh(\omega T)v^{\rm loc}_{q}-\omega\sinh(\omega
T)u^{f}_{q}$ (2)

where $\omega=\sqrt{g/H}$, $g$ is the gravitational constant, and $H$ is the
constant CoM height. The heading angle change is governed by
$u^{\Delta\theta}_{q}=\theta_{q+1}-\theta_{q}$. Based on the sagittal dynamics
(1) and (2), we introduce coordinate transformation based on the heading angle
$\theta_{q}$ to control the LIP dynamics in 2-D Euclidean space. Therefore the
full LIP dynamics in 2-D Euclidean space become:

$\displaystyle x_{q+1}$ $\displaystyle=x_{q}+\Delta x^{\rm
loc}(u^{f}_{q})\cos(\theta_{q})$ (3a) $\displaystyle y_{q+1}$
$\displaystyle=y_{q}+\Delta x^{\rm loc}(u^{f}_{q})\sin(\theta_{q})$ (3b)
$\displaystyle v^{\rm loc}_{q+1}$ $\displaystyle=\cosh(\omega T)v^{\rm
loc}_{q}-\omega\sinh(\omega T)u^{f}_{q}$ (3c) $\displaystyle\theta_{q+1}$
$\displaystyle=\theta_{q}+u^{\Delta\theta}_{q}$ (3d)

A detailed derivation of (3) is in Appendix. A. For notation simplicity, and
hereafter, (3) will be referred to as:

$\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ (4)

Later on, (4) will be enforced as dynamic constraints in our proposed SZN-MPC
in Sec. VII.

###  IV-B Environment Assumptions and Observations

In this work, we hypothesize that in a social setting, the information
accessible by a human for determining their future path is threefold: (i)
their final destination $\mathcal{G}=(x^{\rm dest},y^{\rm dest})$ (navigation
intent), (ii) the surrounding pedestrians’ past trajectory555the subscripts
$t_{p}$, $t$, and $t_{f}$ represent discrete time indices denoting the past,
current and future trajectories, respectively, where $t_{p}<t<t_{f}$.
$\mathcal{T}^{p_{k}}_{[t_{p},t]}=\\{x^{p_{k}}_{q},y^{p_{k}}_{q}\\}^{t}_{q=t_{p}},\forall
k$, where $k$ indexes the $k^{\rm th}$ pedestrian, and (iii) the human’s
social experience, i.e., their assumptions on how to navigate the environment
in a socially-acceptable manner. We treat the social experience as latent
information that is not readily available in human crowd datasets. Therefore
we make the following assumption.

######  Assumption IV.1.

Suppose we learn a model of the future trajectory of a human as a function of
their final goal $\mathcal{G}$ and surrounding pedestrians’ past trajectories
$\mathcal{T}^{p_{k}}_{[t_{p},t]}$. We assume that this model will implicitly
represent each human’s social experience.

In this work, to learn a socially acceptable future path for an ego-agent
$\mathcal{T}^{\rm ego}_{[t,t_{f}]}=\\{x^{\rm ego}_{q},y^{\rm
ego}_{q}\\}^{t_{f}}_{q=t}$, we use real human crowd datasets, and substitute a
single human in the dataset as the ego-agent.

Only the pedestrians within a prespecified radius of the ego-agent are
observable, and we assume that their past trajectories were observable over a
specified time interval from $t_{p}$ to $t$.

###  IV-C Zonotope Preliminaries

Figure 5: An illustration of zonotopes: (a) a 3-D zontope ($n=3$) with
$n_{G}=13$ (b) a 2-D zonotope ($n=2$) with $n_{G}=3$. Red arrows indicate the
generators in $G$, with only 6 out of 13 generators are illustrated in (a). In
this study, we will use the 2-D zonotopes for our reachability path design.

A zonotope $\mathcal{Z}\in\mathbb{R}^{n}$ is a convex, symmetrical polytope
parameterized by a center $\bm{c}\in\mathbb{R}^{n}$ and a generator matrix
$G\in\mathbb{R}^{n\times n_{G}}$ (see Fig. 5).

$\mathcal{Z}={{\mathscr{Z}\\!}\\!\left(\bm{c},G\right)}=\\{\bm{c}+G\beta\;|\;\|\beta\|_{\infty}\leq
1\\}$ (5)

The Minkowski sum of
$\mathcal{Z}_{1}={{\mathscr{Z}\\!}\\!\left(\bm{c}_{1},G_{1}\right)}$ and
$\mathcal{Z}_{2}={{\mathscr{Z}\\!}\\!\left(\bm{c}_{2},G_{2}\right)}$ is
$\mathcal{Z}_{1}\oplus\mathcal{Z}_{2}=\mathscr{Z}{(\bm{c}_{1}+\bm{c}_{2},[G_{1}\;G_{2}])}$.
To Check collisions between two zonotopes, [55, Lemma 5.1] is used:

######  Proposition IV.2.

([55, Lemma 5.1]) $\mathcal{Z}_{1}\cap\mathcal{Z}_{1}=\emptyset$ iff
$\bm{c}_{1}\notin{{\mathscr{Z}\\!}\\!\left(\bm{c}_{2},[G_{1}\;G_{2}]\right)}$

Per [28, Theorem 2.1], zonotopes can be parameterized using a half-space
representation $\mathcal{P}=\\{x\;|\;Ax\leq b\\}$, where
$x\in\mathcal{P}\iff\max(Ax-b)\leq 0$ and $x\notin\mathcal{P}\iff\max(Ax-b)>0$
(see Fig. 5(b)), which we show is useful for collision checking. In the
special case of a 2-D zonotope, the center-generator representation to the
half-space representation is given analytically as follows:

######  Proposition IV.3.

([25, Proposition 2]) Let $C=\begin{bmatrix}-G[2,:]&G[1,:]]\end{bmatrix}$ and
$l_{G}[i]$ be the norm of the $i^{\rm th}$ generator
$l_{G}[i]=\|G[:,i]\|_{2}$, the half-space representation of a 2-D zonotope:

$A[i,:]=\frac{1}{l_{G}[i]}\cdot\begin{bmatrix}C\\\
-C\end{bmatrix}\in\mathbb{R}^{2n_{G}\times 2}$ (6)
$b=A\cdot\bm{c}+|{AG}|\;1_{m\times 1}\in\mathbb{R}^{2n_{G}}$ (7)

where $i=1,\ldots,n_{G}$ indexes the number of generators.

In this work, zonotopes are used to describe the social reachable set for all
agents, i.e., the ego-agent and pedestrians. We seek to learn a sequence of
social zonotopes for the ego-agent $\mathcal{Z}^{\rm ego}_{q}$, each of which
contains two consecutive waypoints of the ego-agent’s future social trajectory
$\mathcal{T}^{\rm ego}_{[t,t_{f}]}$, thereby approximating the agent’s
continuous-time motion similar to [25].

######  Definition IV.1 (Social Zonotope $\mathcal{Z}^{\rm ego}_{q}$).

A social zonotope for the ego-agent’s $q^{\rm th}$ step is $\mathcal{Z}^{\rm
ego}_{q}={{\mathscr{Z}\\!}\\!\left(\bm{c}_{q},G_{q}\right)}$, satisfying that
the future traj … $\mathcal{T}^{\rm
ego}_{[t,t_{f}]}\in\bigcup\limits_{q=t}^{t_{f-1}}\mathcal{Z}^{\rm ego}_{q}$.

##  V Social Zonotope Network

This section introduces the Social Zonotope Network (SZN) architecture and the
loss functions used during training, which are designed both for shaping the
social zonotopes and for ensuring the physical viability of the path for
bipedal locomotion. A salient feature of our social zonotope network is to
learn the zonotope representation directly as an output of the neural network
enabling real-time reachability-based planning and collision avoidance in the
MPC introduced later in Sec. VII.

###  V-A Learning Architecture

Figure 6: The Social Zonotope Network (SZN) is comprised of two coupled neural
networks to both predict the reachable set of the surrounding pedestrians and
learn the social reachable set of the ego-agent. (a) shows the pedestrian
prediction network, conditioned on the pedestrian endpoints and the immediate
change in the ego-agent’s state. (b) shows the ego-agent social network
conditioned on the pedestrians’ future prediction, the immediate change in the
ego-agent’s state, and the ego-agent’s goal location. Dashed connections are
used during training only.

We use a conditional variational autoencoder (CVAE) architecture to learn the
ego-agent’s future trajectory conditioned on the final destination goal, the
immediate change in the ego-agent’s state, and the surrounding pedestrians’
past trajectories. The proposed architecture uses Multi-Layer Perceptrons
(MLP) with ReLU non-linearity for all the sub-networks.

Our Social Zonotope Network (SZN) is comprised of two coupled neural networks
to not only predict the reachable set of the surrounding pedestrians but also
learn the social reachable set of the ego-agent as shown in Fig. 6.

####  V-A1 Pedestrian Prediction Network (PPN)

The pedestrian prediction network (shown in Fig. 6(a)) is inspired by PECNet
[38], where the endpoint of the predicted pedestrian trajectory
$\mathcal{T}^{p_{k}}_{t_{f}}$ ($t_{f}=8$ indicating a 8-step horizon) is
learned first, and then the future trajectory for the $[t,t_{f}]$ horizon is
predicted. Our proposed network deviates from PECNet in three ways. First, the
pedestrian future trajectory is also conditioned on the immediate change in
the ego-agent’s state $\mathcal{T}^{\rm ego}_{t+1}$ (shown in red in Fig.
6(a)). This coupling of the pedestrian prediction and ego-agent planning
networks is intended to capture the effect of the ego-agent’s control on the
future trajectories of the surrounding pedestrians [17, 12]. Second, the
output of the network is the pedestrian’s future reachable set parameterized
as zonotopes $\mathcal{Z}^{p_{k}}_{[t,t_{f}]}$ rather than point-based
trajectories for robust collision checking and uncertainty parameterization
[25, 26, 27]. Third, we replace PECNet’s social pooling module with a simple
ego-agent sensory radius threshold for computational efficiency, and avoid the
complexity when integrating our trained SZN into the MPC for a unified
prediction and planning framework in Sec. VII.

The pedestrians’ past trajectories $\mathcal{T}^{p_{k}}_{[t_{p},t]}$ are
encoded by a neural network $E_{\rm ped}$ as seen by the purple arrow in Fig.
6(a), while the incremental change in the ego-agent state representing the
ego-agent control is encoded by $E_{\rm next}$ as seen by the red arrow in
Fig. 6(a). This allows us to condition the prediction of the pedestrians’
trajectory on the ego-agent’s control. The resultant latent features $E_{\rm
ped}(\mathcal{T}^{p_{k}}_{[t_{p},t]})$ and $E_{\rm next}(\mathcal{T}^{\rm
ego}_{t+1})$ are then concatenated and used as the condition features
$\mathbb{F}_{\rm cond}$. The pedestrian’s endpoint locations are encoded as
$E_{\rm end}$ as seen by the orange arrows in Fig. 6(a). The resultant latent
features $E_{\rm end}(\mathcal{T}^{p_{k}}_{t_{f}})$ are then concatenated with
$\mathbb{F}_{\rm cond}$ as global features $\mathbb{F}_{\rm global}$ and
encoded by the latent encoder $E_{\rm latent}$. We randomly sample features
from a normal distribution $\mathcal{N}(\bm{\mu},\bm{\sigma})$ generated by
the $E_{\rm latent}$ module, and concatenate them with $\mathbb{F}_{\rm
cond}$. This concatenated information is then passed into the latent decoder
$D_{\rm latent}$. Then $D_{\rm latent}$ outputs the predicted endpoint that is
passed again through $E_{\rm end}$. The output is concatenated again with
$\mathbb{F}_{\rm cond}$ and passed to another encoder $P_{\rm future}$ to
output the predicted zonotopes of the pedestrians
$\mathcal{Z}^{p_{k}}_{[t,t_{f}]}$.

####  V-A2 Ego-agent Social Network (ESN)

Our ESN architecture is shown in Fig. 6(b). The surrounding pedestrians’
future zonotope centers $\bm{c}^{p_{k}}_{[t,t_{f}]}$ are aggregated through
summation to take into account the collective effect of surrounding
pedestrians666Other human trajectory learning modules include a social module
to take into account the surrounding pedestrians effect such as social non-
local pooling mask [38], max-pooling [37], and sorting [40]. while keeping a
fixed-input-size architecture[36, 56, 57]. The summed pedestrian features are
then encoded by $E_{\rm future}$ as seen by the green arrows in Fig. 6(b). The
goal location for the ego-agent is encoded by $E_{\rm goal}$, while the
incremental change in the ego-agent state is encoded by $E_{\rm next}$ as seen
by the orange and red arrows respectively in Fig. 6(b). The resultant latent
features $E_{\rm future}(\sum_{k=1}^{m}\bm{c}^{p_{k}}_{[t,t_{f}]})$, $E_{\rm
goal}(\mathcal{G})$ and $E_{\rm next}(\mathcal{T}^{\rm ego}_{t+1})$ are then
concatenated and used as the condition features $\mathbb{F}^{\rm ego}_{\rm
cond}$ for the CVAE. The ground truth of the ego-agent’s future trajectory
$\mathcal{T}^{\rm ego}_{[t,t_{f}]}$ is encoded by $E_{\rm traj}$ as shown by
the cyan arrows in Fig. 6(b). The resultant latent features $E_{\rm
traj}(\mathcal{T}^{\rm ego}_{[t,t_{f}]})$ are then concatenated with
$\mathbb{F}^{\rm ego}_{\rm cond}$ as global features $\mathbb{F}^{\rm
ego}_{\rm global}$ and encoded by the latent encoder $E^{\rm ego}_{\rm
latent}$. Similarly, we randomly sample features from a normal distribution
$\mathcal{N}(\bm{\mu},\bm{\sigma})$ generated by the $E^{\rm ego}_{\rm
latent}$ module, and concatenate them with $\mathbb{F}^{\rm ego}_{\rm cond}$.
This concatenated information is then passed into the latent decoder $D^{\rm
ego}_{\rm latent}$, resulting in our prediction of the ego-agent’s future
reachable set $\mathcal{Z}^{\rm ego}_{[t,t_{f}]}$.

######  Remark 1.

Including the $E_{\rm next}$ encoder in both the PPN and ESN facilitates
seamless integration with a step-by-step MPC, as one of the MPC’s decision
variables, i.e., $\Delta\bm{p}^{\rm ego}$, is used as inputs to $E_{\rm next}$
as detailed in Sec. VII.

###  V-B Zonotope Shaping Loss Functions

Figure 7: Our zonotope shaping loss functions. The loss aims to learn
interconnected zonotopes that engulf the ground truth path.

We propose novel zonotope shaping loss functions for both PPN and ESN, since
both networks output zonotopes. These losses seek to achieve three goals: (i)
penalize deviation of the centers of the zonotopes from the ground truth
future trajectory; (ii) generate intersecting zonotopes between consecutive
walking steps; and (iii) reduce the size of the zonotopes to avoid
unnecessary, excessively large zonotopes. Based on these goals, we implement
the following losses to shape the output zonotopes (see Fig. 7):

  1. 1.

Average displacement error between the predicted centers and midpoint of the
ground truth trajectory $\mathcal{T}_{{\rm mid},i}$:

$\mathcal{L}_{\rm ADE}=\frac{\sum_{i=1}^{t_{f}-1}\|\mathcal{T}_{{\rm
mid},i}-\bm{c}_{i}\|}{t_{f}-1}$

  2. 2.

Final displacement error between the last predicted center and the final
midpoint of the ground truth trajectory:

$\mathcal{L}_{\rm FDE}=\|\mathcal{T}_{{\rm mid},t_{f}-1}-\bm{c}_{t_{f}-1}\|$

  3. 3.

The midpoint between the current center and previous center $\bm{c}^{p}_{{\rm
mid},i}$ is contained in the current zonotope:

$\mathcal{L}_{\rm prev}=\sum_{i=0}^{t_{f}-1}{\rm ReLU}(A_{i}\cdot\bm{c}_{{\rm
mid},i-1}-b_{i})$

where $\bm{c}_{{\rm mid},-1}$ is the initial point of the ground truth
trajectory $\mathcal{T}_{[t,t_{f}]}$, i.e., the current location of the ego-
agent.

  4. 4.

The midpoint between the current center and the next center $\bm{c}^{n}_{{\rm
mid},i}$ is contained in the current zonotope:

$\mathcal{L}_{\rm next}=\sum_{i=0}^{t_{f}-1}{\rm ReLU}(A_{i}\cdot\bm{c}_{{\rm
mid},i+1}-b_{i})$

where $\bm{c}_{{\rm mid},t_{f}}$ is the endpoint of the ground truth
trajectory $\mathcal{T}_{[t,t_{f}]}$.

  5. 5.

Regulating the size of the zonotope, by penalizing the norm of the generators
such that the neural network does not produce excessively large zonotopes that
contain the ground truth trajectory:

$\mathcal{L}_{G}=\sum^{n_{G}}_{i=1}\|l_{G}[i]-d_{i}\|$

where $d_{i}$ is the desired length for each $i^{\rm th}$ generator. We sum
the zonotope shaping losses listed above into the total loss term
$\mathcal{L}_{\mathcal{Z}}$. Similar to PECNet [38], we use Kullback–Leibler
divergence to train the output of the latent encoder, aiming to regulate the
divergence between the encoded distribution
$\mathcal{N}(\bm{\mu},\bm{\sigma})$and the standard normal distribution
$\mathcal{N}(0,\bm{I})$:

$\mathcal{L}_{\rm KL}=D_{\rm KL}\big{(}\mathcal{N}(\bm{\mu},\bm{\sigma})\ \|\
\mathcal{N}(0,\bm{I})\big{)}$

Next, we introduce robot-specific losses for ESN to promote locomotion safety
and ensure that the Digit robot is able to reach consecutive zonotopes in
consecutive walking steps.

###  V-C Incorporating Robot Safety Specifications

To deploy the learning-based social path planner on the Digit robot, it is
essential to consider the features of bipedal locomotion such as kinematic and
dynamic constraints. To this end, we introduce additional losses to ensure the
learned path is viable for bipedal locomotion.

Signal Temporal Logic (STL) is a well-established temporal logic language to
formally encode natural language into mathematical representation for control
synthesis [58]. More importantly, the quantitative semantics of STL offer a
measure of the robustness of an STL specification $\rho(s_{t},\phi)$, i.e.,
quantify the satisfaction or violation of the specification $\phi$ given a
specific signal $s_{t}$. Positive robustness values, i.e.,
$\rho(s_{t},\phi)>0$, indicate specification satisfaction, while negative
robustness values indicate a violation. The authors in [59] present STLCG, a
tool that transforms STL formulas into computational graphs to be used in
gradient-based problems such as neural network learning. To this end, we
leverage a similar technique to formally incorporate desired locomotion safety
behaviors into our learning framework by encoding STL specifications as
additional loss functions that penalize STL formula violation [59, 60].

The locomotion specifications are derived based on our previously introduced
Reduced-Order Model (ROM) safety theorems [3] and our empirical knowledge
about the locomotion safety of Digit [24] during our experiments. To maintain
balance, the ROM Center of Mass (CoM) velocity ought to be bounded based on
the step length and heading change [3]. Therefore, we design STL
specifications to regulate $\bm{c}^{\rm ego}_{[t,t_{f}]}$ to limit the
sagittal and lateral COM velocities as well as the heading change between
consecutive walking steps, based on prespecified thresholds.

####  V-C1 Locomotion Velocity Specification $\phi_{\rm vel}$

Let $s^{v_{\rm sag}}_{[t+1,t_{f}]}$ and $s^{v_{\rm lat}}_{[t+1,t_{f}]}$ be
signals representing the velocity of $\bm{c}^{\rm ego}_{[t,t_{f}]}$ (via
finite difference) in the sagittal and lateral directions, respectively. The
locomotion velocity specification has:

$\displaystyle\begin{split}\phi_{\rm sag}&=\square_{[t+1,t_{f}]}(s^{v_{\rm
sag}}_{[t+1,t_{f}]}\leq v_{\rm max}\wedge s^{v_{\rm sag}}_{[t+1,t_{f}]}\geq
v_{\rm min}),\\\ \phi_{\rm lat}&=\square_{[t+1,t_{f}]}(s^{v_{\rm
lat}}_{[t+1,t_{f}]}\leq v_{\rm lat}\wedge s^{v_{\rm lat}}_{[t+1,t_{f}]}\geq-
v_{\rm lat}),\\\ \phi_{\rm vel}&=\phi_{\rm sag}\wedge\phi_{\rm
lat},\end{split}$ (8)

where $\square_{[a,b]}(c)$ denotes that specification $c$ must be satisfied
for all times $t\in[a,b]$.

Accordingly, the loss of the locomotion velocity specification is defined as:

$\mathcal{L}_{\phi_{\rm vel}}=\underbrace{{\rm ReLU}(-\rho((s^{v_{\rm
sag}},s^{v_{\rm lat}}),\phi_{\rm vel}))}_{\textit{velocity violation}}$ (9)

####  V-C2 Heading Change Specification $\phi_{\Delta\theta}$

Let $s^{\Delta\theta}_{[t+1]}$ be a signal equal to the heading change between
$\bm{c}^{\rm ego}_{t}$ and $\bm{c}^{\rm ego}_{t+1}$. The heading change
specification is:

$\phi_{\Delta\theta}=\square_{[t+1,t_{f}]}(s^{\Delta\theta}_{[t+1,t_{f}]}<\Delta\theta_{\rm
max}\wedge s^{\Delta\theta}_{[t+1,t_{f}]}>-\Delta\theta_{\rm max})$

Therefore, the loss of the heading change specification is:

$\mathcal{L}_{\phi_{\Delta\theta}}=\underbrace{{\rm
ReLU}(-\rho(s^{\Delta\theta},\phi_{\Delta\theta}))}_{\textit{heading change
violation}}$ (10)

We sum the STL locomotion losses into a single term $\mathcal{L}_{\rm STL}$.
The network is trained end to end using the following loss function:

$\displaystyle\mathcal{L}=\alpha_{1}\mathcal{L}_{KL}+\alpha_{2}\mathcal{L}_{\mathcal{Z}}+\alpha_{3}\mathcal{L}_{\rm
STL}$ (11)

where these $\alpha$ parameters are weighting coefficients.

##  VI Zonotope Refinement for Social Acceptability and Uncertainty
Parameterization

Zonotopes have a desirable property of allowing computationally efficient
downstream online refinements. In this section, we introduce two types of
zonotope refinements for the ego-agent based on (1) personal space preference
for increased social acceptability, and (2) modeling error compensation of
robot dynamics discrepancy between the ROM and the full-order model of our
Digit robot.

######  Remark 2.

Our key insight is that, by using zonotopes as the output format for our
neural networks, we can easily postprocess the network outputs to incorporate
sources of error that would be either difficult for the neural network to
learn directly, or may change at runtime when the network cannot be retrained.

###  VI-A Personal Space Modulation

Another consideration for social acceptability is the personal space
surrounding every agent [61]. We leverage the adaptable nature of zonotope
parameterization, which can be modulated in specific directions, to develop
personal space generators. These generators are inspired by the sociological
study of proxemics [61] and its application to social navigation [62, 63]. The
personal space generator matrix, ${G}^{\rm PS}$, is formulated as follows:

${G}^{\rm
PS}=\text{diag}(a,b)\cdot\begin{bmatrix}\cos(\theta)&-\sin(\theta)\\\
\sin(\theta)&\cos(\theta)\end{bmatrix}$ (12)

The parameters $a$ and $b$ represent the scalar distances that define the
personal space along the sagittal and lateral axes, respectively. These
distances are rotated by the angle $\theta$ to align the personal space with
the ego-agent’s current walking orientation in both directions as shown in
Fig. 8(a). The new ego-agent’s zonotope is then augmented as
$\hat{\mathcal{Z}}^{\rm ego}=\mathscr{Z}(\bm{c}^{\rm ego},[{G}^{{\rm
ego}}\;{G}^{\rm PS}])$.

###  VI-B Robot Modeling Error Compensation

Using zonotopes as a representation of reachable sets allows for online
compensation for the robot dynamics discrepancy between the ROM and the full-
order model of our Digit robot.

We learn the modeling errors using Gaussian Process (GP) regression[64, 65].
We introduce a GP777The GP model is trained offline. that takes in the current
sagittal velocity of the robot $v^{\rm loc}_{q}$ and the MPC previous optimal
solution for $(v^{\rm loc}_{*},u^{\Delta\theta}_{*})$. To characterize the
discrepancy, we chose these parameters as representative state variables
because they effectively represent the key parameters contributing to the
discrepancy between the ROM and low-level ALIP controller [1]. The GP model
then outputs the expected mean deviation $\bm{\mu}=(\mu_{x},\mu_{y})$ and
variance $\bm{\sigma}=(\sigma^{2}_{x},\sigma^{2}_{y})$ in robot’s Euclidean
position at the next walking step (See Fig. 2). The Gaussian mean from the GP
model is used to design a new generator matrix
$G^{\mu}=\text{diag}(\mu_{x},\mu_{y})$ such that the ego-agent’s zonotope
becomes $\hat{\mathcal{Z}}^{\rm ego}_{q+1}=\mathscr{Z}(\bm{c}^{\rm
ego},[{G}^{{\rm ego}}\;{G}^{\rm PS}\;G^{\mu}])$ to compensate for the
anticipated mismatch between the ROM dynamics and the full-order dynamics (see
Fig. 8(b).

Figure 8: Two types of zonotope refinements: (a) zonotope refinement based on
personal space preference for improved social acceptability, where the
generators in $G^{\rm PS}$ are in the local sagittal and lateral directions as
shown by the pink arrows. (b) zonotope refinement based on a learned GP model
of the model discrepancy between ROM and full-order models, where the
generators in $G^{\mu}$ are in the global $x$ and $y$ directions as shown by
the orange arrows.

##  VII Social MPC

To safely navigate the human-crowded environment we propose to solve the
following trajectory optimization problem that encodes the SZN in the previous
section as constraints:

$\displaystyle\min_{X,U}\quad$
$\displaystyle\sum_{q=0}^{N-1}J(\bm{x}_{q},\bm{u}_{q})+J_{N}(\bm{x}_{N})$
(13a) s.t. $\displaystyle\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q}),\forall q$
(13b) $\displaystyle\bm{x}_{0}=\bm{x}_{\rm
init},\;(\bm{x}_{q},\bm{u}_{q})\in\mathcal{XU}_{q},\forall q$ (13c)
$\displaystyle\bm{p}^{\rm ego}_{q+1}\in\hat{\mathcal{Z}}^{\rm
ego}_{q+1}(\Delta\bm{p}^{\rm ego}_{q},\mathcal{E}_{q}),\forall q$ (13d)
$\displaystyle\hat{\mathcal{Z}}^{\rm ego}_{q+1}(\Delta\bm{p}^{\rm
ego}_{q},\mathcal{E}_{q})\bigcap\mathcal{Z}^{{p_{k_{q}}}}_{q+1}=\emptyset,\;\forall\;q,k_{q}$
(13e)

where the decision variables include a state sequence
$X=\\{\bm{x}_{1},\ldots,\bm{x}_{N}\\}$ and a control sequence
$U=\\{\bm{u}_{1},\ldots,\bm{u}_{N-1}\\}$, the running and terminal costs (13a)
are designed to reach the goal and promote social acceptability, subject to
the ROM dynamics (13b) (Sec. IV-A). Constraint (13d) requires the ego-agent at
the next $(q+1)^{\rm th}$ walking step to stay within the reachable set, while
constraint (13e) requires the ego-agent to avoid collision with the
pedestrians. $\mathcal{E}_{q}=(\mathcal{T}^{p_{k}}_{[t_{p},t],q},\mathcal{G})$
denotes the environment state at the $q^{\rm th}$ walking step. Next, we will
introduce the kinematics constraints in Sec. VII-A, and navigation constraints
in Sec. VII-B, social acceptability cost function (Sec. VII-C), and finally
reformulate the MPC in (13) with a version for implementation (Sec. VII-D).

###  VII-A Kinematics Constraints

To prevent the LIP dynamics from taking a step that is kinematically
infeasible by the Digit robot, we implement the following constraint

$\mathcal{XU}_{q}=\\{(\bm{x}_{q},\bm{u}_{q})\;|\;\bm{x}_{\rm
lb}\leq\bm{x}_{q}\leq\bm{x}_{\rm ub}\;\text{and}\;\bm{u}_{\rm
lb}\leq\bm{u}_{q}\leq\bm{u}_{\rm ub}\\}$ (14)

where $\bm{x}_{\rm lb}$ and $\bm{x}_{\rm ub}$ are the lower and upper bounds
of $\bm{x}_{q}$ respectively, and $\bm{u}_{\rm lb}$ and $\bm{u}_{\rm ub}$ are
the bounds for $\bm{u}_{q}$ (See Fig. 4).

###  VII-B Reachability and Navigation Safety Constraints

To enforce navigation safety (i.e., collision avoidance), we require that
Digit remains in the social zonotope $\mathcal{Z}^{\rm ego}$ and outside of
the surrounding pedestrians reachable set $\mathcal{Z}^{{p_{k}}}$.

####  VII-B1 Reachability constraint

For the robot’s CoM to remain inside the desired zonotope for the next walking
step $\hat{\mathcal{Z}}^{\rm ego}_{q+1}$, we represent the zonotope using
half-space representation as shown in Prop. IV.3. The constraint is
reformulated as such:

$\max(\hat{A}^{\rm ego}\bm{p}^{\rm ego}-\hat{b}^{\rm ego})\leq 0$ (15)

####  VII-B2 Navigation Safety Constraint

For pedestrian collision avoidance, we require that the reachable set of the
ego-agent does not intersect with that of the pedestrians for each walking
step. Therefore, we design a new zonotope for the ego-agent as Minkowski sum
of the ego-agent’s zonotope and the pedestrian’s zonotope centered around the
ego-agent $\mathcal{Z}^{\rm mink}=\mathscr{Z}(\bm{c}^{\rm ego},[{G}^{{\rm
ego}}\;{G}^{\rm PS}\;G^{\mu}\;G^{p_{k}}])$ to check for collision with the
pedestrians’ zonotope following Prop. IV.2. We then represent
$\mathcal{Z}^{\rm mink}$ using a half-space representation parametersized by
$A_{k}^{\rm mink}$ and $b_{k}^{\rm mink}$, as per Prop. IV.3, and require that
the pedestrian position is outside the Minkowski-summed zonotope. Thus, for
each $k$th pedestrian, we have the following constraint:

$\max(A_{k}^{\rm mink}\bm{p}_{k}-b_{k}^{\rm mink})>0.$ (16)

###  VII-C Cost Function and Social Acceptability Metric

Figure 9: Illustration of the terminal goal angle $\theta_{\mathcal{G}}$, a
state dependent on the initial state $\bm{x}_{0}$. $\theta_{\mathcal{G}}$ is
designed to guide the ego-agent towards the final goal location. SZN-MPC
optimizes over $N$ walking step horizon such that the ego-agent heading at the
end of the planning horizon $\theta_{N}$ aligns with
$\theta_{\mathcal{G}}(\bm{x}_{0})$. After executing a walking step (see the
figure on the right), we update $\theta_{\mathcal{G}}(\bm{x}_{0})$ based on
the new initial state $\bm{x}_{0}$ for the ego-agent.

Figure 10: Illustration of the social acceptability cost. In the initial step
in the optimization horizon (figure on the left), the learned social path
$\hat{\mathcal{Z}}^{\rm ego}(\Delta\bm{p}^{\rm ego}_{0},\mathcal{E}_{0})$ is
shown in the red zonotopes, and the active constraint is
$\bm{x}_{1}\in\hat{\mathcal{Z}}^{\rm ego}_{1}(\Delta\bm{p}^{\rm
ego}_{0},\mathcal{E}_{0})$. In the following step in the optimization horizon
(figure on the right) the learned social path $\hat{\mathcal{Z}}^{\rm
ego}(\Delta\bm{p}^{\rm ego}_{1},\mathcal{E}_{1})$ (shown by the cyan
zonotopes) is based on the current environment state $\mathcal{E}_{1}$ and
ego-agent current location $\bm{p}^{\rm ego}_{1}$, and the active constraint
is $\bm{x}_{2}\in\hat{\mathcal{Z}}^{\rm ego}_{2}(\Delta\bm{p}^{\rm
ego}_{1},\mathcal{E}_{1})$. The social acceptability cost aims to minimize the
difference between the centers $\hat{\bm{c}}^{\rm social}_{q}$ (red dots) of
$\hat{\mathcal{Z}}_{q}^{\rm ego}(\Delta\bm{p}^{\rm
ego}_{0},\mathcal{E}_{0})\;\forall q$ and the ego-agent ROM CoM $\bm{p}^{\rm
ego}_{q}$ (cyan dots). The arrows between the social path centers
$\hat{\bm{c}}^{\rm social}$ and ego-agent ROM CoM $\bm{p}^{\rm ego}$ indicate
the distance that the social acceptability cost aims to minimize
$\|\hat{\bm{c}}^{\rm social}_{q}-\bm{p}^{\rm ego}_{q}\|$.

The MPC cost function is designed to drive the CoM state to a goal location
$\mathcal{G}$ and to promote social acceptability. The terminal cost penalizes
(1) the distance between the current ROM state and the global goal state
$\mathcal{G}$ in the 2D world coordinate, and (2) the ego-agent heading angle
deviation from the heading angle pointing toward the final goal location (see
Fig.9) to avoid abnormal walking gaits, such as the robot moving toward the
goal but in a way of heading opposite to the goal location while walking
backward.

$J_{N}(\bm{x}_{N})=\|\bm{x}_{N}-\bm{x}_{\mathcal{G}}\|^{2}_{W_{1}}+\|\theta_{N}-\theta_{\mathcal{G}}(\bm{x}_{0})\|^{2}_{W_{2}}$
(17)

where $\bm{x}_{\mathcal{G}}=(\mathcal{G},v_{\rm terminal})$,
$\mathcal{G}=(x^{\rm dest},y^{\rm dest})$, and
$\theta_{\mathcal{G}}(\bm{x}_{0})$ is the angle between the ego-agent’s
current position and the final goal location $\mathcal{G}$ (see more details
in Fig. 9).

The MPC constrains the ROM CoM $\bm{p}^{\rm ego}_{q}$ to stay within the ego-
aganet’s zonotope (13d) and avoid collisions (13e). These constraints, along
with the changes in the environment
$(\mathcal{E}_{q}\neq\mathcal{E}_{q+1},\forall q)$, might cause the generated
ROM CoM $\bm{p}^{\rm ego}_{q}$ trajectory to deviate from the learned social
path $\hat{\mathcal{Z}}_{q+1}^{\rm ego}(\Delta\bm{p}^{\rm
ego}_{0},\mathcal{E}_{0})$ with the centers $\hat{\bm{c}}^{\rm
social}_{q},\forall q$. This social path is generated from the initial
environment $\mathcal{E}_{0}$.

Therefore, we incorporate a social acceptability metric by creating a cost
that penalizes deviation of the ROM CoM $\bm{p}^{\rm ego}_{q}$ from the
learned social path $\hat{\mathcal{Z}}^{\rm ego}_{q+1}(\Delta\bm{p}^{\rm
ego}_{0},\mathcal{E}_{0})$ as shown in Fig. 10. We set the social
acceptability metric as: (1) the distance between the ROM CoM $\bm{p}^{\rm
ego}_{q}$ and the centers of the learned social path $\hat{\bm{c}}^{\rm
social}_{q}$, and (2) the difference between the ego-agent current heading
$\theta_{q}$ and social heading angle $\theta^{\rm social}$, i.e., the angle
between the ego-agent’s initial position and the $\hat{\bm{c}}^{\rm
social}_{N}$, as shown in Fig. 10. Thus, we set the running cost of social
acceptability as follows:

$J_{\rm social}(\bm{x}_{q})=\|\hat{\bm{c}}^{\rm social}_{q}-\bm{p}^{\rm
ego}_{q}\|^{2}_{W_{3}}+\|\theta^{\rm social}-\theta_{q}\|^{2}_{W_{4}}$ (18)

Including such social acceptability metric as a cost function, will guide SZN-
MPC to generate the CoM trajectory that (1) tracks the learned social path
$\hat{\mathcal{Z}}^{\rm ego}_{q+1}(\Delta\bm{p}^{\rm
ego}_{0},\mathcal{E}_{0})$, (2) is within the next zonotope based on the
current environment state $\mathcal{E}_{q}$, and (3) is collision-free (i.e.,
constraints (13d)-(13e)).

######  Remark 3.

The initial output of the neural network $\hat{\mathcal{Z}}^{\rm
ego}_{q+1}(\Delta\bm{p}^{\rm ego}_{0},\mathcal{E}_{0})$ is not guaranteed to
be collision-free at every walking step in the planning horizon. Therefore,
the assumed socially acceptable path is set as a cost and not a constraint, to
prioritize safety over social acceptability.

###  VII-D Social Zonotope Network MPC Formulation

We reformulate our Social Zonotope Network MPC (SZN-MPC) shown in (13) based
on the aforementioned costs and constraints for implementation as follows:

$\displaystyle\min_{X,U}\quad\sum_{q=0}^{N-1}$ $\displaystyle J_{\rm
social}(\bm{x}_{q})+J_{N}(\bm{x}_{N})$ (19a) s.t.
$\displaystyle\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q}),\forall q$ (19b)
$\displaystyle\bm{x}_{0}=\bm{x}_{\rm
init},\;(\bm{x}_{q},\bm{u}_{q})\in\mathcal{XU}_{q},\forall q$ (19c)
$\displaystyle\max(\hat{A}^{\rm ego}_{q+1}\bm{p}^{\rm ego}_{q+1}-\hat{b}^{\rm
ego}_{q+1})\leq 0,\forall q$ (19d) $\displaystyle\max(A^{\rm
mink}_{q+1}\bm{p}_{k_{q+1}}-b^{\rm mink}_{q+1})>0,\;\forall\;q,k_{q}$ (19e)

where the kinematics constraint in (19c) is implemented using the local
dynamics $(\Delta x^{\rm loc},v^{\rm loc})$. We provide detailed
implementation parameters in Table I.

##  VIII Results

In this section, we present comprehensive results and their analysis of three
distinct experiments designed to evaluate various aspects of our framework.
The first experiment assesses the social acceptability and locomotion
feasibility of the SZN-MPC. The second experiment benchmarks our SZN-MPC with
a baseline approach for the LIP-based MPC that uses a discrete-time control
barrier function (DCBF-MPC) for dynamic obstacle collision avoidance and
trajectory planning. The final experiment tests the feasibility of
implementing SZN-MPC on our Digit robot hardware. The section starts with
implementation details.

###  VIII-A Implementation Details

####  VIII-A1 Training

The social path planner module introduced in Sec. V was trained on the UCY
[66] and ETH [67] crowd datasets with the common leave-one-out approach,
reminiscent of prior studies [36, 38, 37, 68]. More specifically, we excluded
the UNIV dataset from the training examples, and used it for testing. To
evaluate the performance of incorporating robot-locomotion-specific STL
specifications into the training, we trained two neural network models with
and without the added robot-specific losses introduced in Sec. V-C. We employ
a historical trajectory observation $\mathcal{T}^{p_{k}}_{[-8,0]}$ for all
neighboring pedestrians that are within a radius of $4$ m and a prediction
horizon $\hat{\mathcal{T}}^{\rm ego}_{[0,8]}$. For both pedestrians and the
ego-agent, the duration of $8$ timesteps takes $3.2$ s ($8\times 0.4$ s $=3.2$
s). The network architecture details are shown in Appendix B. The SZN is
implemented and trained using PyTorch [69].

####  VIII-A2 Pedestrian Simulation

We utilize SGAN (Social Generative Adversarial Network) [37], a state-of-the-
art human trajectory prediction model, for simulating pedestrians during our
testing. SGAN is specifically designed to grasp social interactions and
dependencies among pedestrians. It considers social context, including how
people influence each other and move in groups. This is important for creating
realistic simulations of pedestrian motion. Employing a prediction model
different from our model ensures a fair evaluation by eliminating any inherent
advantages of our proposed method[17]. In our simulation framework, SGAN
incorporates the historical trajectories of both the pedestrians and the ego-
agent, where the ego-agent is also treated as a pedestrian. This approach
enhances the realism of the simulation by accounting for the interaction
between the ego-agent and pedestrians in the environment.

####  VIII-A3 Testing Environment Setup

The environment for all the following simulation tests is an open space of
$14\times 14$ m2, with randomly generated pedestrians’ initial trajectory. The
goal location is $\mathcal{G}=(6,12)$ m, and the ego-agent starting position
is uniformly sampled along the $y$-axis as such
$\bm{x}_{0}=(0,\mathcal{U}_{[0,13]},0)$ with $\theta_{0}=0$. The MPC is solved
with a planning horizon of $N=4$, SZN-MPC parameters are included in Table. I.
Simulations and training are conducted using a 16-core Intel Xeon W-2245 CPU
and an RTX-5000 GPU with 64 GB of memory. The SZN-MPC is implemented using do-
mpc Python libraray [70] and CasADi [71]. Digit is simulated using the MuJoCo
simulator provided by Agility Robotics [24] and visualized using Nvidia Isaac
Gym [72], which allows an animation of pedestrian characters in the
environment.

TABLE I: SZN-MPC Parameters parameter | value | parameter | value
---|---|---|---
$v^{\rm loc}_{\rm ub}$ |  $1.0$ m/s | $v^{\rm loc}_{\rm lb}$ |  $-0.1$ m/s
$\Delta x^{\rm loc}_{\rm ub}$ |  $0.2$ m | $\Delta x^{\rm loc}_{\rm lb}$ |  $-0.2$ m
$u^{\Delta\theta}_{\rm ub}$ | $15^{\circ}$ | $u^{\Delta\theta}_{\rm lb}$ |  $-15^{\circ}$
$u^{f}_{\rm ub}$ |  $0.4$ m | $u^{f}_{\rm lb}$ |  $-0.1$ m
$d_{1}$ | $0.1$ | $d_{2}$ |  $0.005$
$v_{\rm terminal}$ |  $0$ m/s | $n_{G}$ |  $4$
$W_{1}$ | $3$ | $W_{2}$ |  $1$
$a$ |  $0.3$ m | $b$ |  $0.2$ m
$\alpha_{1}$, $\alpha_{3}$ | $1$ | $\alpha_{2}$ |  $100$

###  VIII-B Experiment 1: Social Acceptability and Locomotion Feasibility

In this section, we aim to quantify the social acceptability of the generated
paths of SZN, and the feasibility of the generated path for bipedal
locomotion. We quantify the social acceptability using two methods: First, we
measure the average displacement error (ADE) and final displacement error
(FDE) of ESN’s planned path compared to the ground truth; Second, we measure
the social acceptability metric running SZN-MPC with and without the social
acceptability cost function. For locomotion feasibility, we compare the
tracking performance of the ROM to the social path with and without the
locomotion losses introduced in Sec. V-C.

####  VIII-B1 Social Acceptability

Figure 11: Quantitative results of social acceptability: (a) Show the
displacement error between the prediction of ESN $\bm{c}^{\rm ego}$ and the
ground truth data $\mathcal{T}^{\rm ego}_{\rm mid}$. The data is collected
based on the UNIV dataset with $7831$ unique frames. The solid line shows the
average displacement error at each prediction horizon. (b) shows the social
acceptability metric for $5$ different trials with $30$ pedestrians and $100$
walking steps in each trial. The social acceptability metric is reduced when
Problem (19) is solved with the social cost (18), thus yielding a socially
acceptable path.

Figure 12: Qualitative results of social acceptability: comparison of
different social acceptability levels. (a) shows a socially acceptable
trajectory generated by SZN-MPC with social acceptability cost (18), as the
ego-agent’s planned path (shown in blue) follows the predicted social path of
ESN (shown in red), while (b) shows a trajectory generated by SZN-MPC without
social acceptability cost (18) where a larger deviation is observed between
the ego-agent’s planned path and the predicted social path of ESN.

To quantify social acceptability we rely on displacement errors between the
ground truth data $\bm{c}^{\rm ego}_{i}$ and ESN prediction output
$\mathcal{T}^{\rm ego}_{{\rm mid},i}$.

${\rm ADE}=\frac{\sum_{i=1}^{t_{f}-1}\|\mathcal{T}^{\rm ego}_{{\rm
mid},i}-\bm{c}^{\rm ego}_{i}\|}{t_{f}-1}$ (20)

Fig. 11(a) shows that ESN produces an ADE$=0.218$ m over the prediction
horizon of $7$ timesteps888The prediction horizon timesteps is $7$ and not
$8$, since the displacement error is calculated based on the middle points
$\mathcal{T}^{\rm ego}_{\rm mid}$ of $\mathcal{T}^{\rm ego}_{[1,8]}$, and
FDE$=0.447$ m.

For Digit to achieve a socially acceptable path, it needs to track the path
produced by ESN, by assuming that ESN’s ADE $=0.218<\epsilon$ as defined in
Definition. 2. Fig. 11(b) shows that we are able to reduce the social
acceptability cost when running SZN-MPC (19) with the social running cost Eq.
(18), thus, promoting social acceptability by tracking the path of ESN. Fig.
12 shows the results of a planned trajectory of SZN-MPC with and without
social acceptability cost (18).

####  VIII-B2 Locomotion Feasibility

Figure 13: Violin plots of ESN test results with and without STL locomotion
losses are shown in blue and red, respectively. The shaded green region is
where the heading change specification (a), and locomotion velocity
specification (b) are satisfied, i.e.,
$\rho(s^{\Delta\theta},\phi_{\Delta\theta}))<0$ and $-\rho((s^{v_{\rm
sag}},s^{v_{\rm lat}}),\phi_{\rm vel})<0$. The solid vertical line represents
the mean value of $-\rho$, while the dot represents the median. The data is
collected based on the UNIV dataset with $7831$ unique frames. (c) Shows the
social acceptability metric when ESN is integrated with SZN-MPC with and
without the locomotion losses.

Integrating the locomotion losses into ESN training limits the violation of
the locomotion safety specifications (9) and (10) as seen in Fig. 13(a)-(b).
The reduction in the violation allows SZN-MPC to generate a trajectory that
achieves an improved tracking of the socially acceptable path as shown in Fig.
13(c). These results indicate that the locomotion losses shape ESN output to
comply with the capabilities of bipedal locomotion.

###  VIII-C Experiment 2: Benchmarking

Figure 14: Block diagram of (a) coupled SZN-MPC and (b) decoupled SZN-MPC.

Figure 15: Storyboard snapshots of SZN-MPC trajectory at different walking
steps. The ego-agent (cyan) successfully reaches the goal (yellow $\star$)
while avoiding pedestrians (green). The dashed circle is the sensory radius of
the ego-agent. Blue dots represent the CoM of ROM, and black $\times$ is the
desired foot placement. Green dots are unobserved pedestrians.

Figure 16: Benchmarking results for the ego-agent’s (a) velocity, (b)
optimality: number of walking steps to reach within $1$ m of the final goal,
(c) frequency, and (d) safety: minimum distance to pedestrians. The data
consists of 20 different trials with random initial conditions and a fixed
goal location (see Sec. VIII-A3). Each trial is limited up to $100$ walking
steps. The velocity data is collected before reaching the goal, to avoid
collecting a stopping velocity. The frequency is calculated based on a data
collection of $300$ walking steps, and the green shaded area in (c) is the
minimum required computation time of SZN-MPC for Digit hardware
implementation. (d) the dashed red line is $0.5$ m which is $r$ in (22), and
what is considered as $d$ in navigation safety in Definition. III.2.

We compare a coupled and decoupled version of our method with a baseline
approach for LIP-based MPC with a discrete-time control barrier function
(DCBF-MPC) for collision avoidance of dynamic obstacles [6, 53]. We compare
the conservativeness of the produced trajectory, social acceptability, safety
and optimality, and finally the computational cost. Snapshots of SZN-MPC
results at different walking steps are shown in Fig. 15.

####  VIII-C1 Models

##### Coupled SZN-MPC

In this setup, the prediction (PPN) and planning (ESN) networks are coupled
during the trajectory optimization, where the MPC reasons about the effect of
the ego-agent’s control on the future prediction of the pedestrians. A block
diagram of this model is shown in Fig. 14(a).

##### Decoupled SZN-MPC (dec.)

In this setup, PPN and ESN are decoupled. Before each solve of (19), the
pedestrians’ future prediction is conditioned on the optimal solution from the
previous MPC solve for the ego-agent and is fixed throughout the optimization
for the current solve. Namely, the PPN module is queried only once for each
MPC solve. A block diagram of this model is shown in Fig. 14(b).

######  Remark 4.

For clarification, our decoupled SZN-MPC is not considered as the category of
the decoupled social navigation literature as explained in Sec. II-A, since
the PPN module is conditioned on the ego-agent next planned walking step.
Thus, our decoupled SZN-MPC can be viewed as a coupled social navigation
method in the literature.

##### DCBF-MPC

We compare our path plan to that generated by a LIP-based MPC with a DCBF for
navigation safety, where we substitute (19d) and (19e) with a DCBF:

$h(\bm{p}^{\rm ego}_{q+1},\bm{p}_{k_{q+1}})\geq(1-\gamma)h(\bm{p}^{\rm
ego}_{q},\bm{p}_{k_{q}}),\;\forall\;k_{q}$ (21)

where $(1-\gamma)$ is a class $\mathcal{K}_{\infty,e}$, $0<\gamma\leq 1$ [54,
73], and $h(\bm{p}^{\rm ego}_{q},\bm{p}_{k_{q}})$ is a distance metric defined
as:

$h(\bm{p}^{\rm ego}_{q},\bm{p}_{k_{q}})=\|\frac{1}{r}(\bm{p}^{\rm
ego}_{q}-\bm{p}_{k_{q}})\|-1$ (22)

and the optimization is initialized with $h(\bm{p}^{\rm
ego}_{0},\bm{p}_{k_{0}})\geq 0$. For this model, we also use PPN as a
prediction module for the surrounding pedestrians, and SGAN as the pedestrian
simulator.

Figure 17: A storyboard snapshots of the hardware experiments with $5$
pedestrians. In this experiment, the first group consists of three stationary
pedestrians standing in place, while the other group consists of two
pedestrians walking towards Digit.

####  VIII-C2 Conservativeness

In Fig. 16(a), all three models produce relatively similar median velocities
over the testing data. However, coupled SZN-MPC and decoupled SZN-MPC produce
more consistent velocities. Our proposed method is more efficient as shown by
the number of steps taken to reach the goal Fig. 16(b), with less variation in
velocity and similar safety performance (see Fig. 16(a) and (d)). The high
variability in velocity that DCBF-MPC produces indicates adaptability in a
dynamic environment, however, it does not translate to a safer path in a
densely crowded environment (instead, it generates the same level of safety as
SZN-MPC) as it produces a similar minimum distance to the surrounding
pedestrians (See Fig. 16(a) and (d)).

####  VIII-C3 Social Acceptability

SZN-MPC produces a more consistent and predictable behavior for the ego-agent
when compared to DCBF-MPC as indicated by the the smaller interquartile range
in Fig. 16(a). Predictability of the ego-agent behavior in a social context is
desirable by pedestrians as it is perceived to be less disruptive.

######  Remark 5.

The coupled SZN-MPC and decoupled SZN-MPC produce similar results. The reason
is as follows: Although the decoupled SZN-MPC only inquires the PPN module
once for each MPC solve and then fixes the pedestrian prediction during the
optimization, this MPC is solved at a frequency of individual walking steps
and each solve will couple the pedestrian prediction with the ego-agent
planning. Thus, the decoupled SZN-MPC presents a similar level of coupling
between the pedestrian prediction and ego-agent planning, and produces
comparable results to the coupled SZN-MPC.

####  VIII-C4 Safety and Optimality

All three methods produce comparable safety performance by maintaining a
similar minimum distance to the pedestrians as shown in Fig. 16(d). However,
SZN-MPC consistently generates a more optimal path, as indicated by the lower
number of steps taken to reach the goal (see Fig. 16(b)). Thus, we are not
sacrificing safety by moving faster. SZN-MPC generates optimal paths for the
ego-agent due to its proactive approach, i.e., the SZN-MPC generates future
reachable sets that take into account not only the collision avoidance but
also social influence from all surrounding pedestrians. On the other hand,
DCBF-MPC is reactive, as it only reacts to the PPN predictions of the
pedestrians to avoid collisions. This reactivity leads to the “freezing robot
problem” [35], as evidenced by the low velocities that DCBF-MPC generates,
specifically in environments with high crowd densities 16(a).

######  Remark 6.

The pedestrian simulation lacks assurance of generating non-adversarial paths.
Despite considering the ego-agent’s position, there are occasional cases where
the simulator generates paths for pedestrians that intersect with the ego-
agent and other pedestrians. In our real-world experiments, we operate under
the assumption of pedestrian collaboration and non-adversarial behavior
towards the ego-agent, following social norms.

####  VIII-C5 Computational Cost

The three models’ computational costs are different by orders of magnitude,
where DCBF-MPC is $10^{2}$ times faster than the coupled SZN-MPC and $10$
times faster than the decoupled SZN-MPC as shown in Fig. 16(c). Note that, the
MPC is used as a step planner for Digit, and it only needs to be solved once
before the swing phase of the walking motion ends. Since the commanded step
duration is $0.4$ s, which is the minimum computational time required for
hardware implementation, the SZN-MPC needs to be solved at a rate of at least
$2.5$ Hz (Sec.VIII-D1). Based on the testing parameters (Sec.VIII-A), the
coupled SZN-MPC is not viable for hardware implementation. Therefore, we
conclude the decoupled SZN-MPC outperforms the coupled version, and will use
the decoupled SZN-MPC for our hardware implementation. Computational
efficiency can be increased with a smaller number of the planning horizon $N$,
and the number of pedestrians in the environment.

Figure 18: Snapshots of three different testing scenarios. (a) depicts
pedestrians walking in different directions. (b) illustrates a group of
pedestrians walking alongside Digit in a row while the other group walks
towards Digit. (c) demonstrates two groups walking in the direction opposite
to Digit. In the first column, green arrows indicate the direction of the
pedestrians’ movement, while a cyan arrow indicates Digit’s direction, and the
yellow star is the final goal position for Digit.

###  VIII-D Experiment 3: Hardware Validation in A Human Crowd

We conduct a series of hardware experiments to achieve the following
objectives: First, to showcase the versatility of our proposed method in
reliably navigating social settings with pedestrians across various scenarios.
Second, to validate our network architecture choices, specifically learning
the pedestrians’ future path prediction and the ego-agent’s future path
directly as zonotopes, which facilitates real-time implementation of the full
framework on Digit’s onboard PC.

######  Remark 7.

The personal space modulation and modeling error compensation in Sec. VI, are
included in this hardware experiment for their practical use. Without such
refinements, Digit’s trajectory was relatively close to the surrounding
pedestrians. A key feature of the zonotope parametrization is that it allows
us to compensate for desired behaviors downstream without the need to retrain
the SZN.

####  VIII-D1 Low-level Full-Body Control

At the low level we use the Angular momentum LIP planner introduced in [1],
and a Digit’s passivity controller with ankle actuation which we have
previously shown to exhibit desirable ROM tracking results [3]. Here we set
the desired walking step time and the desired lateral step width to be $0.4$ s
and $0.4$ m, respectively.

####  VIII-D2 Experiment Setup

Our experimental setup uses an indoor VICON motion capture system [74] to
measure the current position of the surrounding pedestrians $\bm{p}_{k_{0}}$.
The coordinates of pedestrians are sent over to SZN-MPC every $0.4$ s to match
the human crowd dataset SZN is trained on. SZN-MPC then solves for the ego-
agent’s social trajectory and sends the next desired CoM velocity and heading
change $(v^{\rm loc}_{q+1},\theta_{q+1})$ to the low-level controller. The
prediction, planning, and low-level control are executed on Digit’s onboard PC
and in real-time, with a walking step horizon $N=4$. The experimental space in
$6\times 3$ m2, Digit’s starting position is $\bm{p}^{\rm ego}_{0}=(0,0)$ m
with $\theta_{0}=0^{\circ}$, and the goal location is diagonally across the
experiment space at $\mathcal{G}=(5.72,1.18)$ m (see Fig. 17).

####  VIII-D3 Experiments

Our framework is demonstrated in various mock social scenarios999videos of the
experiments are found here https://youtu.be/bUSbsj3cCH4?si=wPDV0ezDONasbZNm. .
In Fig. 17, we depict a social scenario involving two groups of pedestrians.
The first group consists of three stationary pedestrians, while the other
group consists of two pedestrians walking towards Digit. Our results show that
SZN accurately predicts the two groups’ behaviors and safely navigates toward
the goal location marked by the yellow star. Digit adjusts its position closer
to the stationary group to avoid collision with the approaching pedestrians.
Three different scenarios are also shown in Fig. 18. In Fig. 18(a),
pedestrians move towards the center of the space from different directions and
continue their path out of the experimental space. In Fig. 18(b), one group
walks alongside Digit in a row while the other group of pedestrians walks in
the opposite direction. In Fig. 18(c), two groups of pedestrians walk towards
Digit but with a slightly different lateral direction. In all scenarios, Digit
successfully generate socially acceptable and safe navigation paths while
maintaining a proactive forward motion.

Figure 19: Hardware experiment results: in (a) we show the torso heading
tracking performance, and CoM sagittal velocity tracking performance in (b),
and in (c) we show the experimental CoM position with superimposed Digit
illustration. The vertical gray lines indicate the foot contact switching
instants of each walking step.

Fig. 19 illustrates the tracking performance comparisons between the desired
$(v^{\rm loc},\theta)$ from SZN-MPC and the actual hardware responses of
Digit. Fig. 19(a) shows the tracking performance of the heading for Digit
$\theta$, and Fig. 19(b) shows the CoM sagittal velocity $v^{\rm loc}$
tracking performance. The continuous desired velocity profile is the desired
continuous ROM velocity based on $v^{\rm loc}$ [3, 1]. SZN-MPC updates the
target parameters $(v^{\rm loc},\theta)$ at every walking step, with vertical
gray lines marking the foot contact events. Discrepancies are bound to occur
when using a ROM plan to control a full-order system. These issues become
especially noticeable with elements like body orientation (see Fig. 19(a)),
which the ROM does not explicitly account for.

The hardware experiments demonstrate the efficacy of our proposed approach.
The design choices made for the SZN architecture, the ROM-based motion
planner, and the reachable-based collision avoidance collectively enable safe
and real-time experimentation with the hardware.

##  IX Limitations and Discussions

In this section, we discuss the limitations of our proposed framework,
including the assumptions underpinning our social acceptability evaluation,
the computational demands of our approach, and the constraints related to the
locomotion gait of our bipedal robot.

###  IX-A Social Acceptability Evaluation and Datasets

The core assumption underlying our framework is that the paths recorded in
human crowd datasets [66, 67] represent socially acceptable trajectories.
Although this assumption may hold true, we acknowledge that in practical,
real-world implementations, human crowds may behave differently in the
presence of a bipedal robot. This presents a significant challenge. Collecting
data on human’s social reaction with Digit does not ensure that pedestrians
will behave naturally. Since the presence of bipedal robots in social settings
is still relatively new and potentially intimidating, it may influence
pedestrian’s social navigation behavior. One way to potentially address this
issue is using a human pedestrian simulation in which the simulated
pedestrians perceive the robot as just another pedestrian within the scene,
similar to the approach used in this work. However, accurately simulating
pedestrian interactions remains a challenging task.

We base the design of socially acceptable paths for the ego-agent on the past
paths of pedestrians. To achieve a more comprehensive socially acceptable
behavior for the ego-agent, further considerations could include (1) the
design of a natural walking gait for the robot such that the pedestrians are
more at ease around the robot ego-agent; (2) an analysis of the facial
expressions of surrounding pedestrians and adjusting the ego-agent’s path
accordingly; and (3) incorporation of the social scenario context such as
shopping mall, airport, and hospital.

###  IX-B Computational Cost and Outdoor Implementation

We opted for a relatively straightforward neural network architecture to
enable real-time implementation. However, processing large crowds in real-time
can be challenging. This issue could be mitigated by employing a more advanced
filtering scheme (rather than the simple circle threshold used in our study)
to exclude pedestrians that do not impact the ego-agent’s path. Ideally, this
new filtering scheme could enable an optimized neural network design that does
not increase the computational burden during the trajectory optimization
phase. Additionally, outdoor deployments may increase computational demands
due to the complexity of the environment, which includes not only pedestrians
but also static and dynamic obstacles and other environmental components.
Using sensors to track pedestrians and processing this data on the fly for the
SZN-MPC could extend the solve time beyond acceptable limits.

###  IX-C Locomotion Gait

In our implementation, we constrain the walking gait to maintain stable torso
Euler angles and fixed arm motion. In human locomotion, particularly in narrow
spaces such as corridors, socially acceptable behaviors often involve
adjusting the torso yaw angle while continuing straight center-of-mass motion.
By adjusting the torso, a person can reduce their effective width, making it
easier to pass by others without direct contact. This subtle change in body
orientation helps to communicate intent and awareness of social norms,
facilitating smoother and more courteous interactions in confined areas. In
scenarios where forward movement is obstructed by standing pedestrians, the
capability to walk sideways could provide a more efficient and socially
acceptable solution to navigate around other pedestrians. Without this
capability, the robot is forced to either come to a complete stop or walk
backward.

###  IX-D Environment and Task

Our framework only considers an open space environment with no static
obstacles and a simple reach-avoid task. To expand this capability, more
complex navigation tasks in an environment with both dynamic and static
obstacles can be implemented by using high-level planners hierarchically
connected as a layer on top of the SZN-MPC, e.g., formal task planning methods
as in [3]. This hierarchical approach allows for better handling of more
sophisticated tasks and possibly adversarial environmental events.
Furthermore, the modeling error GP in Sec. VI-B can be expanded to include
terrain profile uncertainty, enabling the robot to navigate complex and rough
terrain [64, 65].

##  X Conclusion

Integration of bipedal robotic systems in real-world environments is still an
open problem, specifically in human-oriented environments. These environments
are less predictable and require a nuanced level of social interaction between
the robot and humans.

We present a novel approach for socially acceptable bipedal navigation. At the
core of our framework is SZN, a learning architecture for reachability-based
prediction of future pedestrian reachable sets and planning a socially
acceptable reachable set for the robot parameterized as zonotopes. Zonotopes
allow for efficient modulation of the reachable set based on modeling
uncertainty and personal space preferences. Our integration of SZN into MPC
allows for real-time pedestrian trajectory prediction with bipedal motion
planning. SZN-MPC optimizes over the output of the neural network, with a
novel cost function designed to encourage the generation of socially
acceptable trajectories, striking a balance between efficient navigation and
adherence to social norms. The extensive validation through simulations and
hardware experiments solidifies the effectiveness of the proposed framework.

##  Appendix A Derivation of Linear Inverted Pendulum model

In a manner similar to the derivation of the step-to-step discrete Linear
Inverted Pendulum (LIP) dynamics as in [53, 6], the continuous motion of the
LIP dynamics in the sagittal direction is governed by $\ddot{x}^{\rm
loc}=-(g/H)u^{f}$, where $g$ is the gravitational acceleration, $H$ is CoM
height, and $u^{f}$ is the sagittal distance of the stance foot from CoM. The
closed-form solution is given by

$\begin{bmatrix}x^{\rm loc}(t)\\\ v^{\rm
loc}(t)\end{bmatrix}=\begin{bmatrix}1&\sinh(\omega t)/\omega\\\ 0&\cosh(\omega
t)\end{bmatrix}\begin{bmatrix}x^{\rm loc}(0)\\\ v^{\rm
loc}(0)\end{bmatrix}+\begin{bmatrix}1-\cosh(\omega t)\\\ -\omega\sinh(\omega
t)\end{bmatrix}u^{f}$

where $\omega=\sqrt{g/H}$. Setting each step duration to a constant $T$, such
that the state at the $(q+1)^{\rm th}$ step is $x_{q+1}=x_{q}(T)$, we obtain
the step-to-step discrete LIP model as:

$\begin{bmatrix}x^{\rm loc}_{q+1}\\\ v^{\rm
loc}_{q+1}\end{bmatrix}=\begin{bmatrix}1&\sinh(\omega T)/\omega\\\
0&\cosh(\omega T)\end{bmatrix}\begin{bmatrix}x^{\rm loc}_{q}\\\ v^{\rm
loc}_{q}\end{bmatrix}+\begin{bmatrix}1-\cosh(\omega T)\\\ -\omega\sinh(\omega
T)\end{bmatrix}u^{f}_{q}$

Therefore the sagittal CoM position change in one walking step can be
expressed as:

$\Delta x^{\rm loc}=x^{\rm loc}_{q+1}-x^{\rm loc}_{q}=v^{\rm loc}\sinh(\omega
T)/\omega+(1-\cosh(\omega T))u^{f}_{q}$

To obtain the dynamics $\bm{x}=(\bm{p},v^{\rm loc},\theta)$, where
$\bm{p}=(x,y)$, we introduce the heading change based on
$u^{\Delta\theta}_{q}=\theta_{q+1}-\theta_{q}$ to the sagittal dynamics and
obtain the following set of dynamics as in (3):

$\displaystyle x_{q+1}$ $\displaystyle=x_{q}+\Delta x^{\rm
loc}\cos(\theta_{q})$ $\displaystyle y_{q+1}$ $\displaystyle=y_{q}+\Delta
x^{\rm loc}\sin(\theta_{q})$ $\displaystyle v^{\rm loc}_{q+1}$
$\displaystyle=\cosh(\omega T)v^{\rm loc}_{q}-\omega\sinh(\omega T)u^{f}_{q}$
$\displaystyle\theta_{q+1}$ $\displaystyle=\theta_{q}+u^{\Delta\theta}_{q}$

##  Appendix B Network Architecture Parameters

TABLE II: Network architecture parameters Pedestrian Prediction Network
---
$E_{\rm ped}$ | $16\rightarrow 32\rightarrow 16$
$E_{\rm end}$ | $2\rightarrow 8\rightarrow 16$
$E_{\rm nxt}$ | $2\rightarrow 32\rightarrow 16$
$P_{\rm future}$ | $50\rightarrow 32\rightarrow 16\rightarrow 32\rightarrow 70$
$E_{\rm latent}$ | $48\rightarrow 8\rightarrow 16\rightarrow 32$
$D_{\rm latent}$ | $48\rightarrow 32\rightarrow 16\rightarrow 32\rightarrow 2$
Ego-agent Social Network
$E_{\rm goal}$ | $2\rightarrow 8\rightarrow 16\rightarrow 2$
$E_{\rm future}$ | $16\rightarrow 64\rightarrow 32\rightarrow 16$
$E_{\rm nxt}$ | $2\rightarrow 64\rightarrow 32\rightarrow 2$
$E_{\rm traj}$ | $16\rightarrow 64\rightarrow 32\rightarrow 16$
$E_{\rm latent}$ | $36\rightarrow 8\rightarrow 50\rightarrow 16$
$D_{\rm latent}$ | $36\rightarrow 128\rightarrow 64\rightarrow 128\rightarrow 70$

## Acknowledgment

The authors would like to express their gratitude to Dr. Seth Hutchinson and
Dr. Sehoon Ha for allowing us to use their lab space and motion capture
system. Special thanks to Nitish Sontakke for helping with the motion capture
system setup and to Vignesh Subramanian, Abivishaq Balasubramanian, Meghna
Narayanan, Hemanth Sai Surya Kumar Tammana, Harini Mudradi, Venkat Sai Chinta,
Manthan Joshi for their participation in the hardware experiments.

## References

  * [1] Y. Gong and J. W. Grizzle, “Zero Dynamics, Pendulum Models, and Angular Momentum in Feedback Control of Bipedal Locomotion,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 144, no. 12, 10 2022, 121006.
  * [2] H. Sadeghian, C. Ott, G. Garofalo, and G. Cheng, “Passivity-based control of underactuated biped robots within hybrid zero dynamics approach,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2017, pp. 4096–4101.
  * [3] A. Shamsah, Z. Gu, J. Warnke, S. Hutchinson, and Y. Zhao, “Integrated task and motion planning for safe legged navigation in partially observable environments,” _IEEE Transactions on Robotics_ , pp. 1–22, 2023.
  * [4] Z. Li, J. Zeng, S. Chen, and K. Sreenath, “Autonomous navigation of underactuated bipedal robots in height-constrained environments,” _The International Journal of Robotics Research_ , vol. 42, no. 8, pp. 565–585, 2023.
  * [5] J.-K. Huang and J. W. Grizzle, “Efficient anytime clf reactive planning system for a bipedal robot on undulating terrain,” _IEEE Transactions on Robotics_ , 2023.
  * [6] K. S. Narkhede, A. M. Kulkarni, D. A. Thanki, and I. Poulakakis, “A sequential mpc approach to reactive planning for bipedal robots using safe corridors in highly cluttered environments,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 11 831–11 838, 2022.
  * [7] G. Gibson, O. Dosunmu-Ogunbi, Y. Gong, and J. Grizzle, “Terrain-adaptive, alip-based bipedal locomotion controller via model predictive control and virtual constraints,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2022, pp. 6724–6731.
  * [8] 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_ , vol. 41, no. 8, pp. 812–847, 2022.
  * [9] 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_. IEEE, 2020.
  * [10] 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_ , 2020, pp. 958–965.
  * [11] 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.
  * [12] C. Mavrogiannis, F. Baldini, A. Wang, D. Zhao, P. Trautman, A. Steinfeld, and J. Oh, “Core challenges of social robot navigation: A survey,” _ACM Transactions on Human-Robot Interaction_ , vol. 12, no. 3, pp. 1–39, 2023.
  * [13] Y. Che, A. M. Okamura, and D. Sadigh, “Efficient and trustworthy social navigation via explicit and implicit robot–human communication,” _IEEE Transactions on Robotics_ , vol. 36, no. 3, pp. 692–707, 2020.
  * [14] A. Bera, T. Randhavane, and D. Manocha, “The emotionally intelligent robot: Improving socially-aware human prediction in crowded environments,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition Workshops_ , 2019, pp. 0–0.
  * [15] D. Paez-Granados, V. Gupta, and A. Billard, “Unfreezing social navigation: Dynamical systems based compliance for contact control in robot navigation,” in _International Conference on Robotics and Automation_. IEEE, 2022, pp. 8368–8374.
  * [16] H. Nishimura, B. Ivanovic, A. Gaidon, M. Pavone, and M. Schwager, “Risk-sensitive sequential action control with multi-modal human trajectory forecasting for safe crowd-robot interaction,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2020, pp. 11 205–11 212.
  * [17] S. Schaefer, K. Leung, B. Ivanovic, and M. Pavone, “Leveraging neural network gradients within trajectory optimization for proactive human-robot interactions,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2021, pp. 9673–9679.
  * [18] K. Majd, S. Yaghoubi, T. Yamaguchi, B. Hoxha, D. Prokhorov, and G. Fainekos, “Safe navigation in human occupied environments using sampling and control barrier functions,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2021, pp. 5794–5800.
  * [19] C. Cathcart, M. Santos, S. Park, and N. E. Leonard, “Proactive opinion-driven robot navigation around human movers,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2023, pp. 4052–4058.
  * [20] Z. Gu, R. Guo, W. Yates, Y. Chen, Y. Zhao, and Y. Zhao, “Walking-by-logic: Signal temporal logic-guided model predictive control for bipedal locomotion resilient to external perturbations,” in _International Conference on Robotics and Automation_ , 2024.
  * [21] Z. Gu, N. Boyd, and Y. Zhao, “Reactive locomotion decision-making and robust motion planning for real-time perturbation recovery,” in _International Conference on Robotics and Automation_. IEEE, 2022, pp. 1896–1902.
  * [22] L. Yang, Z. Li, J. Zeng, and K. Sreenath, “Bayesian optimization meets hybrid zero dynamics: Safe parameter learning for bipedal locomotion control,” _arXiv preprint arXiv:2203.02570_ , 2022.
  * [23] M. Dai, X. Xiong, and A. D. Ames, “Data-driven step-to-step dynamics based adaptive control for robust and versatile underactuated bipedal robotic walking,” _arXiv preprint arXiv:2209.08458_ , 2022.
  * [24] A. Robotics, “Digit robot.” [Online]. Available: https://agilityrobotics.com/robots
  * [25] L. Paparusso, S. Kousik, E. Schmerling, F. Braghin, and M. Pavone, “Zapp! zonotope agreement of prediction and planning for continuous-time collision avoidance with discrete-time dynamics,” _arXiv preprint arXiv:2406.01814_ , 2024.
  * [26] M. Selim, A. Alanwar, S. Kousik, G. Gao, M. Pavone, and K. H. Johansson, “Safe reinforcement learning using black-box reachability analysis,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 10 665–10 672, 2022.
  * [27] S. Kousik, P. Holmes, and R. Vasudevan, “Safe, aggressive quadrotor flight via reachability-based trajectory design,” in _Dynamic Systems and Control Conference_ , vol. 59162. American Society of Mechanical Engineers, 2019, p. V003T19A010.
  * [28] M. Althoff, “Reachability analysis and its application to the safety assessment of autonomous cars,” Ph.D. dissertation, Technische Universität München, 2010.
  * [29] A. Shamsah, K. Agarwal, S. Kousik, and Y. Zhao, “Real-time model predictive control with zonotope-based neural networks for bipedal social navigation,” _arXiv preprint arXiv:2403.16485_ , 2024.
  * [30] M. Moder and J. Pauli, “Proactive robot movements in a crowd by predicting and considering the social influence,” in _31st IEEE International Conference on Robot and Human Interactive Communication_. IEEE, 2022, pp. 644–651.
  * [31] X.-T. Truong and T. D. Ngo, “Toward socially aware robot navigation in dynamic and crowded environments: A proactive social motion model,” _IEEE Transactions on Automation Science and Engineering_ , vol. 14, no. 4, pp. 1743–1760, 2017.
  * [32] H. Kretzschmar, M. Spies, C. Sprunk, and W. Burgard, “Socially compliant mobile robot navigation via inverse reinforcement learning,” _The International Journal of Robotics Research_ , vol. 35, no. 11, pp. 1289–1307, 2016.
  * [33] B. Brito, B. Floor, L. Ferranti, and J. Alonso-Mora, “Model predictive contouring control for collision avoidance in unstructured dynamic environments,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 4459–4466, 2019.
  * [34] F. Feurtey, “Simulating the collision avoidance behavior of pedestrians,” _The University of Tokyo, School of Engineering. Department of Electronic Engineering_ , 2000.
  * [35] P. Trautman, J. Ma, R. M. Murray, and A. Krause, “Robot navigation in dense human crowds: Statistical models and experimental studies of human–robot cooperation,” _The International Journal of Robotics Research_ , vol. 34, no. 3, pp. 335–356, 2015.
  * [36] T. Salzmann, B. Ivanovic, P. Chakravarty, and M. Pavone, “Trajectron++: Dynamically-feasible trajectory forecasting with heterogeneous data,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, Proceedings, Part XVIII 16_. Springer, 2020, pp. 683–700.
  * [37] A. Gupta, J. Johnson, L. Fei-Fei, S. Savarese, and A. Alahi, “Social gan: Socially acceptable trajectories with generative adversarial networks,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2018, pp. 2255–2264.
  * [38] K. Mangalam, H. Girase, S. Agarwal, K.-H. Lee, E. Adeli, J. Malik, and A. Gaidon, “It is not the journey but the destination: Endpoint conditioned trajectory prediction,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, Proceedings, Part II 16_. Springer, 2020, pp. 759–776.
  * [39] K. Mangalam, Y. An, H. Girase, and J. Malik, “From goals, waypoints & paths to long term human trajectory forecasting,” in _Proceedings of the IEEE/CVF International Conference on Computer Vision_ , 2021, pp. 15 233–15 242.
  * [40] A. Sadeghian, V. Kosaraju, A. Sadeghian, N. Hirose, H. Rezatofighi, and S. Savarese, “Sophie: An attentive gan for predicting paths compliant to social and physical constraints,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 1349–1358.
  * [41] C. Yu, X. Ma, J. Ren, H. Zhao, and S. Yi, “Spatio-temporal graph transformer networks for pedestrian trajectory prediction,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, Proceedings, Part XII 16_. Springer, 2020, pp. 507–523.
  * [42] Y. Hong, Z. Ding, Y. Yuan, W. Chi, and L. Sun, “Obstacle avoidance learning for robot motion planning in human-robot integration environments,” _IEEE Transactions on Cognitive and Developmental Systems_ , 2023.
  * [43] P. Zaytsev, W. Wolfslag, and A. Ruina, “The boundaries of walking stability: Viability and controllability of simple models,” _IEEE Transactions on Robotics_ , vol. 34, no. 2, pp. 336–352, 2018.
  * [44] Y. Ding, C. Khazoom, M. Chignoli, and S. Kim, “Orientation-aware model predictive control with footstep adaptation for dynamic humanoid walking,” in _IEEE-RAS 21st International Conference on Humanoid Robots_ , 2022, pp. 299–305.
  * [45] G. Romualdi, S. Dafarra, G. L’Erario, I. Sorrentino, S. Traversaro, and D. Pucci, “Online non-linear centroidal mpc for humanoid robot locomotion with step adjustment,” pp. 10 412–10 419, 2022.
  * [46] N. Smit-Anseeuw, C. D. Remy, and R. Vasudevan, “Walking with confidence: Safety regulation for full order biped models,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 4177–4184, 2019.
  * [47] G. A. Castillo, B. Weng, S. Yang, W. Zhang, and A. Hereid, “Template model inspired task space learning for robust bipedal locomotion,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2023, pp. 8582–8589.
  * [48] Z. Li, X. Cheng, X. B. Peng, P. Abbeel, S. Levine, G. Berseth, and K. Sreenath, “Reinforcement learning for robust parameterized locomotion control of bipedal robots,” in _International Conference on Robotics and Automation_. IEEE, 2021, pp. 2811–2817.
  * [49] I. Radosavovic, T. Xiao, B. Zhang, T. Darrell, J. Malik, and K. Sreenath, “Learning humanoid locomotion with transformers,” _arXiv preprint arXiv:2303.03381_ , 2023.
  * [50] A. Hereid, C. M. Hubicki, E. A. Cousineau, and A. D. Ames, “Dynamic humanoid locomotion: A scalable formulation for hzd gait optimization,” _IEEE Transactions on Robotics_ , vol. 34, no. 2, pp. 370–387, 2018.
  * [51] J. W. Grizzle, C. Chevallereau, R. W. Sinnet, and A. D. Ames, “Models, feedback control, and open problems of 3d bipedal robotic walking,” _Automatica_ , vol. 50, no. 8, pp. 1955–1988, 2014.
  * [52] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The 3d linear inverted pendulum mode: A simple modeling for a biped walking pattern generation,” in _Proceedings IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No. 01CH37180)_ , vol. 1. IEEE, 2001, pp. 239–246.
  * [53] S. Teng, Y. Gong, J. W. Grizzle, and M. Ghaffari, “Toward safety-aware informative motion planning for legged robots,” _arXiv preprint arXiv:2103.14252_ , 2021.
  * [54] A. Agrawal and K. Sreenath, “Discrete control barrier functions for safety-critical control of discrete systems with application to bipedal robot navigation.” in _Robotics: Science and Systems_ , vol. 13. Cambridge, MA, USA, 2017, pp. 1–10.
  * [55] L. J. Guibas, A. T. Nguyen, and L. Zhang, “Zonotopes as bounding volumes.” in _SODA_ , vol. 3. Citeseer, 2003, pp. 803–812.
  * [56] B. Ivanovic, E. Schmerling, K. Leung, and M. Pavone, “Generative modeling of multimodal multi-human behavior,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2018, pp. 3088–3095.
  * [57] A. Jain, A. R. Zamir, S. Savarese, and A. Saxena, “Structural-rnn: Deep learning on spatio-temporal graphs,” in _Proceedings of the ieee conference on computer vision and pattern recognition_ , 2016, pp. 5308–5317.
  * [58] O. Maler and D. Nickovic, “Monitoring temporal properties of continuous signals,” in _International Symposium on Formal Techniques in Real-Time and Fault-Tolerant Systems_. Springer, 2004, pp. 152–166.
  * [59] K. Leung, N. Aréchiga, and M. Pavone, “Back-propagation through signal temporal logic specifications: Infusing logical structure into gradient-based methods,” in _Algorithmic Foundations of Robotics XIV: Proceedings of the Fourteenth Workshop on the Algorithmic Foundations of Robotics 14_. Springer, 2021, pp. 432–449.
  * [60] X. Li, G. Rosman, I. Gilitschenski, C.-I. Vasile, J. A. DeCastro, S. Karaman, and D. Rus, “Vehicle trajectory prediction using generative adversarial network with temporal logic syntax tree features,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 3459–3466, 2021.
  * [61] E. T. Hall, “The hidden dimension: man’s use of space in public and private the bodley head,” _London, Sydney, Toronto_ , vol. 121, 1969.
  * [62] P. Patompak, S. Jeong, I. Nilkhamhang, and N. Y. Chong, “Learning proxemics for personalized human–robot social interaction,” _International Journal of Social Robotics_ , vol. 12, pp. 267–280, 2020.
  * [63] J. Rios-Martinez, A. Spalanzani, and C. Laugier, “From proxemics theory to socially-aware navigation: A survey,” _International Journal of Social Robotics_ , vol. 7, pp. 137–153, 2015.
  * [64] J. Jiang, S. Coogan, and Y. Zhao, “Abstraction-based planning for uncertainty-aware legged navigation,” _IEEE Open Journal of Control Systems_ , 2023.
  * [65] K. Muenprasitivej, J. Jiang, A. Shamsah, S. Coogan, and Y. Zhao, “Bipedal safe navigation over uncertain rough terrain: Unifying terrain mapping and locomotion stability,” _arXiv preprint arXiv:2403.16356_ , 2024.
  * [66] A. Lerner, Y. Chrysanthou, and D. Lischinski, “Crowds by example,” in _Computer graphics forum_ , vol. 26, no. 3. Wiley Online Library, 2007, pp. 655–664.
  * [67] S. Pellegrini, A. Ess, K. Schindler, and L. van Gool, “You’ll never walk alone: Modeling social behavior for multi-target tracking,” in _IEEE 12th International Conference on Computer Vision_ , 2009, pp. 261–268.
  * [68] J. Li, H. Ma, and M. Tomizuka, “Conditional generative neural system for probabilistic trajectory prediction,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2019, pp. 6150–6156.
  * [69] A. Paszke, S. Gross, F. Massa, A. Lerer, J. Bradbury, G. Chanan, T. Killeen, Z. Lin, N. Gimelshein, L. Antiga _et al._ , “Pytorch: An imperative style, high-performance deep learning library,” _Advances in neural information processing systems_ , vol. 32, 2019.
  * [70] F. Fiedler, B. Karg, L. Lüken, D. Brandner, M. Heinlein, F. Brabender, and S. Lucia, “do-mpc: Towards fair nonlinear and robust model predictive control,” _Control Engineering Practice_ , vol. 140, p. 105676, 2023.
  * [71] J. A. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “Casadi: a software framework for nonlinear optimization and optimal control,” _Mathematical Programming Computation_ , vol. 11, pp. 1–36, 2019.
  * [72] V. Makoviychuk, L. Wawrzyniak, Y. Guo, M. Lu, K. Storey, M. Macklin, D. Hoeller, N. Rudin, A. Allshire, A. Handa _et al._ , “Isaac gym: High performance gpu-based physics simulation for robot learning,” _arXiv preprint arXiv:2108.10470_ , 2021.
  * [73] J. Zeng, B. Zhang, and K. Sreenath, “Safety-critical model predictive control with discrete-time control barrier function,” in _American Control Conference_. IEEE, 2021, pp. 3882–3889.
  * [74] V. M. Systems, “Vicon vero.” [Online]. Available: https://www.vicon.com/