On Terrain-Aware Locomotion
for Legged Robots Shamel Fahmi Istituto Italiano di Tecnologia, Italy
Università degli Studi di Genova, Italy Thesis submitted for the degree of:
Doctor of Philosophy (PhD) April, 2021

Shamel Fahmi

On Terrain-Aware Locomotion for Legged Robots

Doctor of Philosophy (PhD) in Bioengineering and Robotics

Curriculum: Advanced and Humanoid Robotics

Dynamic Legged Systems (DLS) lab, Italian Institute of Technology (IIT), Italy

Tutors:

Dr. Victor Barasuol, Dr. Michele Focchi, and Dr. Andreea Radulescu

Dynamic Legged Systems (DLS) lab, Italian Institute of Technology (IIT), Italy

Principle Advisor:

Dr. Claudio Semini

Dynamic Legged Systems (DLS) lab, Italian Institute of Technology (IIT), Italy

Annual Evaluation Committee:

Prof. Roy Featherstone

Advanced Robotics (ADVR) department, Italian Institute of Technology (IIT),
Italy

Dr. Enrico Mingo

Humanoids & Human Centered Mechatronics (HHCM) lab, Italian Institute of
Technology (IIT), Italy

Dr. Geoff Fink

Dynamic Legged Systems (DLS) lab, Italian Institute of Technology (IIT), Italy

External Examination Committee:

Prof. Auke Ijspeert

Institute of Bioengineering, the Swiss Federal Institute of Technology at
Lausanne (EPFL), Switzerland

Prof. Luis Sentis

Department of Aerospace Engineering and Engineering Mechanics, University of
Texas, USA

Prof. Fabio Ruggiero

Department of Electrical Engineering and Information Technology, University of
Naples Federico II, Italy

© 2021 Shamel Fahmi. All rights reserved.

To my parents Amal and Prof. Shamel,
to my grandmother Hagougi,
to my siblings Menna and Mohamed,
to our family’s latest member Selim,
and to my partner Ilef.

“What I hate is ignorance, smallness of imagination, the eye that sees no
farther than its own lashes. All things are possible … Who you are is limited
only by who you think you are.” - The Egyptian Book of the Dead.

### Abstract

Legged robots are advancing towards being fully autonomous as can be seen by
the recent developments in academia and industry. To accomplish breakthroughs
in dynamic whole-body locomotion, and to be robust while traversing unexplored
complex environments, legged robots have to be terrain aware.

Terrain-Aware Locomotion (TAL) implies that the robot can perceive the terrain
with its sensors, and can take decisions based on this information. The
decisions can either be in planning, control, or in state estimation, and the
terrain may vary in geometry or in its physical properties. TAL can be
categorized into Proprioceptive Terrain-Aware Locomotion (PTAL), which relies
on the internal robot measurements to negotiate the terrain, and Exteroceptive
Terrain-Aware Locomotion (ETAL) that relies on the robot’s vision to perceive
the terrain. This thesis presents TAL strategies both from a proprioceptive
and an exteroceptive perspective. The strategies are implemented at the level
of locomotion planning, control, and state estimation, and are using
optimization and learning techniques.

The first part of this thesis focuses on PTAL strategies that help the robot
adapt to the terrain geometry and properties. At the Whole-Body Control (WBC)
level, achieving dynamic TAL requires reasoning about the robot dynamics,
actuation and kinematic limits as well as the terrain interaction. For that,
we introduce a Passive Whole-Body Control (pWBC) framework that allows the
robot to stabilize and walk over challenging terrain while taking into account
the terrain geometry (inclination) and friction properties. The pWBC relies on
rigid contact assumptions which makes it suitable only for stiff terrain. As a
consequence, we introduce Soft Terrain Adaptation aNd Compliance Estimation
(STANCE) which is a soft terrain adaptation algorithm that generalizes beyond
rigid terrain. STANCE consists of a Compliant Contact Consistent Whole-Body
Control (c3WBC) that adapts the locomotion strategies based on the terrain
impedance, and an online Terrain Compliance Estimator (TCE) that senses and
learns the terrain impedance properties to provide it to the c3WBC.
Additionally, we demonstrate the effects of terrains with different impedances
on state estimation for legged robots.

The second part of the thesis focuses on ETAL strategies that makes the robot
aware of the terrain geometry using visual (exteroceptive) information. To do
so, we present Vision-Based Terrain-Aware Locomotion (ViTAL) which is a
locomotion planning strategy. ViTAL consists of a Vision-Based Pose Adaptation
(VPA) algorithm to plan the robot’s body pose, and a Vision-Based Foothold
Adaptation (VFA) algorithm to select the robot’s footholds. The VFA is an
extension to the state of the art in foothold selection planning strategies.
Most importantly, the VPA algorithm introduces a different paradigm for
vision-based pose adaptation. ViTAL relies on a set of robot skills that
characterizes the capabilities of the robot and its legs. These skills are
then learned via self-supervised learning using Convolutional Neural Networks
(CNNs). The skills include (but are not limited to) the robot’s ability to
assess the terrain’s geometry, avoid leg collisions, and to avoid reaching
kinematic limits. As a result, we contribute with an online vision-based
locomotion planning strategy that selects the footholds based on the robot
capabilities, and the robot pose that maximizes the chances of the robot
succeeding in reaching these footholds.

Our strategies are based on optimization and learning methods, and are
extensively validated on the quadruped robots HyQ and HyQReal in simulation
and experiment. We show that with the help of these strategies, we can push
dynamic legged robots one step closer towards being fully autonomous and
terrain aware.

### Acknowledgments

I would like to thank Claudio for giving me the chance to work at the DLS lab,
and for his continuous support during my PhD. Claudio creates an exceptional,
motivating, and comfortable environment for everyone in the lab. He was always
available when I needed him, and his suggestions were always critical.

During my PhD, I worked under the supervision of Andreea, Michele, and Victor
whom I truly enjoyed working with and learning from. Andreea, thank you for
being patient with me. I know I was hard to get along in the beginning. Thank
you for covering up on how bad I am at making pretzels. Michele, thank you for
saving me when Andreea left. I truly enjoyed working with you, and writing
down proofs on the white board. You always have this motivation and spark
towards research that I am sure it will never fade. Victor, first of all,
thank you for the barbecues; no one beats Victor when it comes to barbecues.
Thank you for always pushing me to do my best. I will never forget the night
we stayed late to submit a paper. That night you stayed with us till we
submitted, and you kept waking me up to support the team.

Working at the DLS lab is of one of the greatest experience that I have had.
My experience would not have been the same without all the current and the
past members of the DLS lab. Particularly, I would like to thank Geoff whom I
learned a lot from on the personal and professional level. Thanks for giving
me your time albeit having loads of work on your shoulders, and for teaching
me that the world frame does not exist in reality. Thanks for being there when
I wanted someone to talk to when I had a mental breakdown. Yet, none of that
matters because you did not name your daughter Shamela! Special thanks to
Octavio who was my first close friend in Genoa. Thank you for being a part of
my family, and for being my official interpreter. Another special mention to
Chundri. The lab has improved dramatically since Chundri stepped foot in the
lab. I still owe him a Margarita. I would also like to thank Angelo, Abdo,
Letizia, Domingo, and Salvatore (the perfect Italian model).

Getting a PhD was a great achievement to me. But equally important was to get
a work-life balance, and to live a healthy life. That would have never been
possible without my friends. Particularly, Abril. Abril, thank you for being
my coach at the gym and for taking it slow with me. Thank you for teaching me
how to be fit, eat healthy, and enjoy my life outside work. You truly made me
a different (better) person. Yet, none of that matters because you did not
name your daughter Shamela! I would also like to thank Carlos G. and Marial
(you still need to take me to that Jazz club!), Andrea B. (I will not forgive
you for leaving the house early without waking me up), Romeo (what happened in
Freiburg stays in Freiburg), Eamon (you know we love you because of your
awesome parents!), and Fabrizia (for the paste di mandorla). My thoughts are
also dedicated to Kristina (for the best birthday gift that I have ever had),
Juliet (my wing woman), Lidia (for being in her top 10 list), Maria (I am
still available if you need a model), Sep (for his Christmas parties, and for
never giving up on me going out for apperitivo), Olmo (for his yearly New
Year’s Eve parties that never fails to impress), Francesca (you still did not
take me out for pizza), Mihail, Anthony, Dimitrious, Mieke, Edwin, Aida, and
Wiebke. I would also like to thank my friends that albeit living far from each
other, we’re still close: Anwar, Adel, Essam, Samer, Fay, Pavel, Ena, Eva, and
Vassilina

I would have never reached this point without my parents, Amal and Shamel
(yes, my dad is the true Shamel). Thank you for your infinite love and
support. Thank you for putting us first and for doing everything possible to
make us feel happy, supported, and appreciated. Thank you for believing in me,
and for encouraging me to follow my dreams and passion. I would like to thank
Menna, Mohamed, and our latest family member, Selim. I live by your support
and love.

Last but not least, I would like to thank my creative, smart, and loving
partner Ilef. Thank you for drawing a smile on my face from the moment I saw
you. Thank you for your continuous love and support.

Shamel Fahmi

### Preface

  * •

This doctoral thesis is building upon decades of research and development in
robotics, dynamics, controls, and machine learning. We expect that the reader
has a basic knowledge about legged robotics before reading this thesis.

  * •

This doctoral thesis is styled as a cumulative thesis. The main contents are
based on publications from peer-reviewed journals, with an exception of one
chapter that contains yet unpublished material. Chapter 1 gives a brief
introduction to the problem and the state of the art, and it lists the
contributions and the outline of the thesis. Chapters 2, 3, and 4 include the
peer-reviewed publications while Chapter 5 includes the yet to be published
one. Finally, Chapter 7 concludes this thesis with a discussion and a summary
of this work and its future directions.

  * •

The articles that Chapters 2-5 are based on are my original work as a first
author. However, these articles are also the fruit of the effort of the
supervisors and co-authors that assisted me during the period of my PhD. For
this reason, I decided to use the active plural voice (we and our) instead of
the singular voice (I and my) throughout the text.

  * •

Chapter 2 has been published in [1]. The concept and theory of this work has
been developed by myself and M. Focchi. The formulation and implementation has
been developed by myself with the support of M. Focchi. The experiments were
conducted and analyzed by M. Focchi with the support of C. Mastalli and
myself. The manuscript was written by myself with the support of M. Focchi and
C. Mastalli, and was reviewed by C. Semini.

  * •

Chapter 3 has been published in [2]. The concept and theory of this work has
been developed by myself and M. Focchi. The formulation and implementation has
been developed by myself with the support of M. Focchi and A. Radulescu. The
experiments were conducted and analyzed by myself with the support of M.
Focchi and G. Fink. The manuscript was written by myself with the support of
M. Focchi and G. Fink, and it was reviewed by A. Radulescu, V. Barasuol, and
C. Semini.

  * •

Chapter 4 has been published in [3]. The concept and theory of this work has
been developed by myself with the support of G. Fink. The formulation and
implementation has been developed by myself with the support of G. Fink. The
experiments were conducted and analyzed by myself and G. Fink. The manuscript
was written by myself and G. Fink and was reviewed by C. Semini.

  * •

Chapters 5 and 6 have been published in [4]. The concept and theory of this
work has been developed by myself and V. Barasuol. The formulation and
implementation has been developed by myself with the support of D. Esteban, O.
Villarreal, and V. Barasuol. The experiments were conducted and analyzed by
myself with the support of V. Barasuol. The manuscript was written by myself
with the support of V. Barasuol, and it was reviewed by D. Esteban, O.
Villarreal, and C. Semini. Note that, part of this chapter has been revised
after the defense date.

  * •

The style of this dissertation has been adopted from A. Winkler’s dissertation
[5].

### Contents

toc

### List of Figures

lof

### List of Tables

lot

### Acronyms

AtI

    Athletic Intelligence
c3WBC

    Compliant Contact Consistent Whole-Body Control
c3

    compliant contact consistent
CI

    Cognitive Intelligence
CNN

    Convolutional Neural Network
CoM

    Center of Mass
DoFs

    Degrees of Freedom
RL

    Reinforcement Learning
EKF

    Extended Kalman Filter
ETAL

    Exteroceptive Terrain-Aware Locomotion
FC

    Foot Trajectory Collision
$\mathcal{F}$

    Set of Safe Footholds
FEC

    Foothold Evaluation Criteria
GES

    Globally Exponentially Stable
GRFs

    Ground Reaction Forces
HAA

    Hip Adduction-Abduction
HC

    Hunt and Crossley’s
HFE

    Hip Flexion-Extension
HyQ

    Hydraulically actuated Quadruped
HyQReal

IMU

    Inertial Measurement Unit
KFE

    Knee Flexion-Extension
KF

    Kinematic Feasibility
KV

    Kelvin-Voigt’s
LC

    Leg Collision
LF

    Left-Front
LH

    Left-Hind
LTV

    Linear Time-Varying
MAE

    Mean Absolute Tracking Error
MCS

    Motion Capture System
MPC

    Model Predictive Control
NLO

    Non-Linear Observer
$\mathrm{n_{sf}}$

    Number of Safe Footholds
ODE

    Open Dynamics Engine
PE

    Persistency of Excitation
PTAL

    Proprioceptive Terrain-Aware Locomotion
pWBC

    Passive Whole-Body Control
QP

    Quadratic Program
RCF

    Reactive Controller Framework
RF

    Right-Front
RH

    Right-Hind
STANCE

     Soft Terrain Adaptation aNd Compliance Estimation
TCE

    Terrain Compliance Estimator
sWBC

    Standard Whole-Body Control
TAL

    Terrain-Aware Locomotion
TBR

    Terrain-Based Body Reference
TO

    Trajectory Optimization
TR

    Terrain Roughness
VFA

    Vision-Based Foothold Adaptation
ViTAL

    Vision-Based Terrain-Aware Locomotion
VPA

    Vision-Based Pose Adaptation
WBC

    Whole-Body Control
WBOpt

    Whole-Body Optimization
XKF

    eXogeneous Kalman Filter
ZMP

    Zero Moment Point

###  Chapter 1 Introduction

Marc Raibert gave a broad definition of intelligence in a recent talk, and
divided it into: Cognitive Intelligence (CI) and Athletic Intelligence (AtI)
[6]. CI allows us to make abstract plans, and to understand and solve broader
problems. AtI on the other hand, allows us to operate our bodies in such a way
that we can balance, stand, walk, climb, etc. AtI also lets us do real-time
perception so that we can interact with the world around us. Marc also noted
that although not all of us are athletes, we still have a great amount of AtI
in us. This thesis is about AtI for legged robots; it can perhaps be one step
towards reaching animal-level AtI.

#### 1.1 Motivation

Legged robots have been around for decades. Recently however, they have shown
remarkable agile capabilities thanks to the research efforts of academia and
industry. For this reason, legged robots are moving out of research labs into
the real world with the promise of being athletically intelligent. The promise
is that legged robots are to aid humans in various applications. The
applications include (but are not limited to) warehouse logistics, inspection
at industrial plants and construction sites, search and rescue, agriculture,
package delivery, space exploration, etc. In all of these applications, there
is perhaps one thing in common: none of the terrains that the robots traverse
are the same. In fact, these terrains are usually dynamic, unexplored, and
uncertain. As a result, the core problem is that the terrain that robots
traverse introduces a large amount of uncertainty. Therefore, for legged
robots to achieve AtI and accomplish breakthroughs in dynamic whole-body
locomotion, they have to be terrain aware.

Terrain-Aware Locomotion (TAL) means that the robot is able to perceive and
understand the surrounding terrain, and is able to take decisions based on
that. In other words, the robot has to have a good knowledge of its
surroundings and use whatever sensors it has to perceive these surroundings
and act upon them. The terrain itself may vary in its geometry or in its
physical properties, and the decisions can either be in planning, control, or
in state estimation. To clarify, let us raise the following questions:

  * •

Can the robot sense (see and feel) the world around it, and the terrain it is
traversing?

  * •

Can the robot understand the differences between the geometrical and physical
properties of the terrain it is traversing?

  * •

Can the robot plan its motion based on its understanding of the terrain and
its own limitations?

  * •

Can the robot quickly adapt this planned motion in case something goes wrong
with it (such as falling, slipping, external pushes, etc.)?

If the answer is yes, then the robot is terrain aware.

Figure 1.1: The Bigger Picture of Locomotion for Legged Robots. (a) an
overview of the pipeline used in locomotion strategies for legged robots. (b)
an illustration of the two categories of Terrain-Aware Locomotion (TAL).

#### 1.2 The Bigger Picture

This section provides a broad overview of the pipeline used in locomotion
strategies for legged robots, and details on each block. As shown in Fig.
1.1(a), the pipeline has four main modules: sensing, planning, adaptation and
execution.

Sensing: This is the perception module that encapsulates all of the robot’s
ability to perceive itself and its surroundings. For that, the robot relies on
its onboard sensors to measure its base and joint states, and build a map of
its surroundings. These onboard sensors include IMUs, joint encoders, torque
sensors, as well as its onboard LIDARs and cameras.

Planning: This is the trajectory generation module that plans the motion of
the robot. The goal of the planning module is to understand the perceived
information from the sensing module, and plan a motion for the robot to take
accordingly. This motion tends to be a long term horizon motion.

Adaptation: The adaption module acts as an intermediate module between the
planning and execution modules. This module tends to be usually merged with
the planning module. However, it is important to understand the core
differences between both. The adaptation module has a re-planning nature. This
means that if the planned motion is not executed as programmed, or if
something goes wrong during execution (such as if the robots falls, slips or
gets disturbed or pushed), the adaptation module can be able to sense this
right away, and adapt the robot’s motion accordingly.

Execution: After perceiving the sensed information, and after planning and
adapting the robot’s motion, the robot finally gets to execute this motion at
the joint level. Hence, the robot has to track its desired whole-body states
while reasoning about its own dynamics and limits, and about the surrounding
terrain.

#### 1.3 Terrain-Aware Locomotion (TAL)

TAL can be categorize into Proprioceptive Terrain-Aware Locomotion (PTAL) and
Exteroceptive Terrain-Aware Locomotion (ETAL) as shown in Fig. 1.1(b). PTAL
relies on the internal robot measurements (mainly its whole body states) to
acquire the terrain information that is surrounding the robot. ETAL relies on
directly acquiring this information using the robot’s visual sensing.

An early work on PTAL was on reflex actions that reactively adapt the swinging
legs trajectory to overcome obstacles if a collision is detected [7]. Since
proprioceptive sensors measures the internal robot states, detecting and
localizing contacts on the robot is possible. For instance, some PTAL
strategies rely on the joint position, velocity and/or torque measurements to
detect and localize contacts [8, 9, 10], and to detect slippage [11, 12]. In
addition to the terrain’s geometry, PTAL strategies are also used to infer and
adapt to the physical properties of the terrain. For instance, several works
have adopted PTAL strategies in locomotion planning and control over different
terrain impedance parameters [13, 2, 14]. In these works, the robot was able
to detect changes in the terrain impedance, and act upon it online.

PTAL strategies are useful in many scenarios when visual feedback is denied
(such as smoky areas, or areas with thick vegetation) or when the terrain map
is unreliable. However, based on their proprioceptive nature, the actions from
PTAL strategies are limited to corrective actions because predicting future
robot-terrain interactions using only the robot’s internal states is
insufficient. This means that PTAL strategies do not act on what is ahead of
the robot. Hence, an action has to happen first before triggering a reactive
strategy; the foot has to collide before triggering a step reflex, or touch
the terrain before inferring its physical properties.

Unlike PTAL, ETAL relies mainly on visual information. This gives ETAL
strategies the advantage of looking ahead of the robot. One famous ETAL
strategy is in selecting the best footholds based on the terrain information
and the capabilities of the legs. This is often referred to as foothold
selection [15, 16, 17, 18]. Apart from foothold selection and similar to PTAL,
ETAL strategies have also been used to infer the terrain properties from
images using deep learning [19, 20, 21].

#### 1.4 Contributions

This thesis summaries our work done on TAL for legged robots. Our work
includes strategies implemented for both PTAL and ETAL, and is applied at the
levels of planning, control, and state estimation. This thesis is divided into
two parts. The first part focuses on PTAL and the second part focuses on ETAL.
This thesis is based on four main articles papers [1, 2, 3, 4]. Each article
is self-contained and is included in a stand-alone chapter. The remainder of
this section summarizes the motivation and contributions of each of these
articles.

##### C1: Passive Whole-Body Control for Quadruped Robots

© 2019 IEEE. Reprinted, with permission. S. Fahmi, C. Mastalli, M. Focchi and
C. Semini, "Passive Whole-Body Control for Quadruped Robots: Experimental
Validation Over Challenging Terrain," in IEEE Robotics and Automation Letters
(RA-L), vol. 4, no. 3, pp. 2553-2560, July 2019, doi:
10.1109/LRA.2019.2908502.

To achieve AtI as explained earlier in this chapter, the locomotion strategy
should be able to reason about the robot’s capabilities, and to be terrain
aware. Thus, as a first step, the first paper contributes to PTAL strategies
by presenting a Whole-Body Control (WBC) framework.

This paper presents a Passive Whole-Body Control (pWBC) framework for
quadruped robots, and focuses on the experimental validation. The pWBC is
aware of the terrain geometry and friction properties. Additionally, the pWBC
achieves dynamic locomotion while compliantly balancing the robot’s trunk. To
do so, we formulate the motion tracking as a Quadratic Program (QP) that takes
into account the full robot rigid body dynamics, the actuation limits, the
joint limits and the contact interaction. To be terrain aware, we encode the
terrain geometry (inclination), and frictional properties in the QP
formulation. To maintain contact consistency with the rigid terrain, we also
encode the rigid contact interaction in the QP formulation.

To validate the approach used in this paper, we analyze the pWBC’s robustness
against inaccurate terrain friction properties, and the robot’s ability to
adapt to any sudden change in the actuation limits. We also present extensive
experimental trials on the HyQ robot, and validate the PTAL capabilities of
the pWBC under various terrain conditions and gaits. The paper also includes
extensive implementation details gained from the experience with the real
platform.

##### C2: STANCE: Locomotion Adaptation over Soft Terrain

© 2020 IEEE. Reprinted, with permission. S. Fahmi, M. Focchi, A. Radulescu, G.
Fink, V. Barasuol and C. Semini, "STANCE: Locomotion Adaptation Over Soft
Terrain," in IEEE Transactions on Robotics (T-RO), vol. 36, no. 2, pp.
443-457, April 2020, doi: 10.1109/TRO.2019.2954670.

###### Remark 1.1

This work has been selected as a finalist for the IEEE RAS Italian Chapter
Young Author Best Paper Award 2020, and for the IEEE RAS Technical Committee
on Model-Based Optimization for Robotics Best Paper Award 2020.

The previous paper presented a pWBC framework that was rigid contact
consistent. In other words, the pWBC was terrain aware with respect to rigid
terrain. In fact, most of WBC frameworks fail to generalize beyond rigid
terrains. To be terrain aware, the robot should be able to adapt to terrains
with different impedances. For that, we focused on extending the PTAL
capabilities of the previously presented pWBC, and adapting it to multiple
terrains with different impedances (such as soft terrain). We study compliant
terrain since it is an unsolved issue for legged locomotion. Legged locomotion
over soft terrain is difficult because of the presence of unmodeled contact
dynamics that standard WBCs do not account for. This introduces uncertainty in
locomotion and affects the stability and performance of the system.

Therefore, this paper proposes a novel soft terrain adaptation algorithm
called Soft Terrain Adaptation aNd Compliance Estimation (STANCE). From its
name, STANCE consists of a Compliant Contact Consistent Whole-Body Control
(c3WBC) that is aware of the terrain impedance, and an online Terrain
Compliance Estimator (TCE) that senses and estimates the terrain impedance.
The c3WBC exploits the knowledge of the terrain to generate an optimal
solution that is contact consistent. This terrain knowledge is provided to the
c3WBC by the TCE.

In this paper, we show that STANCE can adapt online to any type of terrain
compliance (stiff or soft). To do so, we evaluated STANCE both in simulation
and experiment on HyQ, and we compared it with the state of the art pWBC from
the previous paper. We demonstrated the capabilities of STANCE with multiple
terrains of different compliances, with aggressive maneuvers, different
forward velocities, and external disturbances. STANCE allowed HyQ to adapt
online to terrains with different compliances (rigid and soft) without pre-
tuning. HyQ was able to successfully deal with the transition between
different terrains and showed the ability to differentiate between compliances
under each foot.

##### C3: State Estimation for Legged Locomotion over Soft Terrain

© 2021 IEEE. Reprinted, with permission. S. Fahmi, G. Fink and C. Semini, "On
State Estimation for Legged Locomotion over Soft Terrain," in IEEE Sensors
Letters (L-SENS), vol. 5, no. 1, pp. 1–4, January 2021, doi:
10.1109/LSENS.2021.3049954.

The previous STANCE paper presented a PTAL strategy to adapt to soft terrain.
One of the limitations of that paper was in state estimation for legged robots
over soft terrain. This is a limitation because most of the work done on state
estimation for legged robots is designed for rigid contacts, and does not take
into account the physical parameters of the terrain. Thus, this paper is a
step towards extending the PTAL capabilities of legged robots to state
estimation. In detail, this paper answers the following questions: how and why
does soft terrain affect state estimation for legged robots? To do so, we
utilize a state estimator that fuses IMU measurements with leg odometry that
is designed with rigid contact assumptions. We experimentally validate the
state estimator with HyQ trotting over both soft and rigid terrain. Then, we
demonstrate that soft terrain negatively affects state estimation for legged
robots, and that the state estimates have a noticeable drift over soft terrain
compared to rigid terrain.

##### C4: ViTAL: Vision-Based Terrain-Aware Locomotion

© 2022 IEEE. Reprinted, with permission. S. Fahmi, V. Barasuol, D. Esteban, O.
Villarreal, and C. Semini, "ViTAL: Vision-Based Terrain-Aware Locomotion for
Legged Robots," in IEEE Transactions on Robotics (T-RO), vol. 0, no. 0, pp.
1–20, November 2022, doi: 10.1109/TRO.2022.3222958.

Unlike the previous contributions that were PTAL strategies, the second part
of this thesis (and the fourth contribution) is an ETAL strategy. This work
focuses particularly on vision-based planning strategies that decouple
locomotion planning into foothold selection and pose adaptation. Despite the
work done for foothold selection, pose adaptation strategies lag behind. The
core problem of the current pose adaptation strategies is that they focus on
finding one optimal solution based on given selected footholds. This is a
problem because there are no guarantees on what would happen if the selected
footholds are not reached, or if the robot gets disturbed. If any of these
cases happen, the robot may end up in a pose that makes the feet reach
kinematic limits, or collide with the terrain. This would in turn compromise
the robot’s performance and safety. To solve this problem, we should not find
body poses that are optimal with respect to a given foothold, but rather find
body poses that maximize the chances of reaching safe footholds.

With this in mind, we present a locomotion planning strategy called Vision-
Based Terrain-Aware Locomotion (ViTAL). ViTAL consists of a pose adaptation
algorithm called Vision-Based Pose Adaptation (VPA), and a foothold selection
algorithm called Vision-Based Foothold Adaptation (VFA). The VFA is an
extension of state of the art foothold selection strategies. The VPA
introduces a different paradigm for pose adaptation strategies. The VPA is a
pose adaptation algorithm that finds the body pose that maximizes the number
of safe footholds based on a set of skills. The skills represent the
capabilities of the robot and its legs including the ability to assess the
terrain’s geometry, avoid leg collisions, and to avoid reaching kinematic
limits during the swing and stance phases. These skills are then learned via
self-supervised learning using Convolutional Neural Networks (CNNs).
Therefore, ViTAL is an online strategy that simultaneously plans the robot’s
body pose and footholds based on the robot capabilities.

To validate ViTAL, we use the HyQ and HyQReal robots. Thanks to ViTAL, our
robots are able to climb various obstacles including stairs and gaps at
different speeds. We also compare the VPA with a baseline strategy that
selects the robot pose based on given selected footholds, and show that it is
indeed not robust enough to select robot poses based only on given footholds.

##  Part I Proprioceptive Terrain-Aware Locomotion

###  Chapter 2 Passive Whole-Body Control
for Quadruped Robots

© 2019 IEEE. Reprinted, with permission. S. Fahmi, C. Mastalli, M. Focchi and
C. Semini, "Passive Whole-Body Control for Quadruped Robots: Experimental
Validation Over Challenging Terrain," in IEEE Robotics and Automation Letters
(RA-L), vol. 4, no. 3, pp. 2553-2560, July 2019, doi:
10.1109/LRA.2019.2908502.

Abstract. We present experimental results using a passive whole-body control
approach for quadruped robots that achieves dynamic locomotion while
compliantly balancing the robot’s trunk. We formulate the motion tracking as a
Quadratic Program (QP) that takes into account the full robot rigid body
dynamics, the actuation limits, the joint limits and the contact interaction.
We analyze the controller’s robustness against inaccurate friction coefficient
estimates and unstable footholds, as well as its capability to redistribute
the load as a consequence of enforcing actuation limits. Additionally, we
present practical implementation details gained from the experience with the
real platform. Extensive experimental trials on the 90 kg Hydraulically
actuated Quadruped (HyQ) robot validate the capabilities of this controller
under various terrain conditions and gaits. The proposed approach is superior
for accurate execution of highly dynamic motions with respect to the current
state of the art.

Accompanying Video. https://youtu.be/Lg3V_juoE1w

#### 2.1 Introduction

Achieving dynamic locomotion requires reasoning about the robot’s dynamics,
actuation limits and interaction with the environment while traversing
challenging terrain (such as rough or sloped terrain). Optimization-based
techniques can be exploited to attain these objectives in locomotion planning
and control of legged robots. For instance, one approach is to use non-linear
Model Predictive Control (MPC) while taking into consideration the full
dynamics of the robot. Yet, it is often challenging to meet real-time
requirements because the solver can get stuck in local minima, unless proper
warm-starting is used [22]. Thus, current research often relies on low
dimensional models or constraint relaxation approaches to meet such
requirements (e.g. [23]). Other approaches rely on decoupling the motion
planning from the motion control [24, 25, 26]. Along this line, an
optimization-based motion planner could rely on low dimensional models to
compute Center of Mass (CoM) trajectories and footholds while a locomotion
controller tracks these trajectories.

Many recent contributions in locomotion control have been proposed in the
literature that were successfully tested on bipeds and quadrupeds (e.g. [27,
28, 29, 30, 26, 31]). Some of them are based on quasi-static assumptions or
lower dimensional models [32, 33, 34]. This often limits the dynamic
locomotion capabilities of the robot [27]. Consequently, another approach,
that is preferable for dynamic motion, is based on Whole-Body Control (WBC).
WBC facilitates such decoupling between the motion planning and control in
such a way that it is easy to accomplish multiple tasks while respecting the
robot’s behavior [30]. These tasks might include motion tasks for the robot’s
end effectors (legs and feet) [29, 30], but also could be utilized for
contacts anywhere on the robot’s body [35] or for a cooperative manipulation
task between robots [36]. WBC casts the locomotion controller as an
optimization problem, in which, by incorporating the full dynamics of the
legged robot, all of its Degrees of Freedom (DoFs) are exploited in order to
spread the desired motion tasks globally to all the joints. This allows us to
reason about multiple tasks and solve them in an optimization fashion while
respecting the full system dynamics and the actuation and interaction
constraints. WBC relies on the fact that robot dynamics and constraints could
be formulated, at each loop, as linear constraints with a convex cost function
(i.e., a Quadratic Program (QP)) [23]. This allows us to solve the
optimization problem in real-time.

Passivity theory is proven to guarantee a certain degree of robustness during
interaction with the environment [37]. For that reason, such tool is commonly
exploited in the design of locomotion controllers to ensure a passive contact
interaction. Passivity based WBC in humanoids was introduced first by [38] to
effectively balance the robot when experiencing contacts. By providing
compliant tracking and gravity compensation, the humanoid was able to adapt to
unknown disturbances. The same approach was further extended first by [33] and
later by [29]. The former extended [38] to posture control, while the latter
analyzed the passivity of a humanoid robot in multi-contact scenarios (by
exploiting the similarity with PD+ control [39]).

Figure 2.1: Overview of the whole-body controller as part of our locomotion
framework. The dashed black box (cartouche) presents an overview of the
robot’s joints, feet and generated wrenches and forces. LH, LF, RH and RF are
Left-Hind, Left-Front, Right-Hind and Righ-Front legs, respectively. HAA, HFE
and KFE are Hip Abduction/Adduction, Hip Flexion/Extension and Knee
Flexion/Extension, respectively.

In our previous work [34], the locomotion controller was designed for quasi-
static motions using only the robot’s centroidal dynamics. Under that
assumption, we noticed that during dynamic motions, the effect of the leg
dynamics no longer negligible; and thus, it becomes necessary to abandon the
quasi-static assumption to achieve good tracking. Second, since the robot is
constantly interacting with the environment (especially during walking and
running), it is crucial to ensure a compliant and passive interaction. For
these reasons, in this paper, we improve our previous work [34] by
implementing a passivity based WBC that incorporates the full robot dynamics
and interacts compliantly with the environment, while satisfying the kinematic
and torque limits. Our WBC implementation is capable of achieving faster
dynamic motions than our previous work. We also integrate terrain mapping and
state estimation on-board and present some practical implementation details
gained from the experience with the real platform.

Contributions: In this paper, we mainly present experimental contributions in
which we demonstrate the effectiveness of the controller both in simulation
and experiments on Hydraulically actuated Quadruped (HyQ). Compared to
previous work on passivity-based WBC [29, 33], in which experiments were
conducted on the robot while standing (not walking or running), we tested our
controller on HyQ during crawling and trotting. Similar to the recent
successful work of [26] and [40] in quadrupedal locomotion over rough terrain,
we used similar terrain templates to present experiments of our passive WBC on
HyQ using multiple gaits over slopes and rough terrain of different heights.

The rest of this paper is structured as follows: In Section 2.2 we present the
detailed formulation and design of our WBC followed by its passivity analysis
in Section 2.3. Section 2.4 presents further crucial implementation details.
Finally we present our simulation and experimental results in Section 2.5
followed by our conclusions in Section 2.6.

#### 2.2 Whole-Body Controller (WBC)

In this section we present and formulate our WBC. Figure 2.1 depicts the main
components of our locomotion framework. Given high-level user velocity
commands, the planner generates a reference motion online [41] or offline
[25], and provides it to the WBC. Such references include the desired
trajectories for CoM, trunk orientation and swing legs. The state estimator
supplies the controller with an estimate of the actual state of the robot, by
fusing leg odometry, inertial sensing, visual odometry and LIDAR while, the
terrain estimator, provides an estimate of the terrain inclination (i.e.
surface normal). Finally, there is a momentum-based observer that estimates
external disturbances [41] and a lower-level torque controller. The goal of
the designed WBC is to keep the quadruped robot balanced (during running,
walking or standing) while interacting passively with the environment. The
motion tasks of a quadruped robot can be categorized into a trunk task and a
swing task. The trunk task regulates the position of the CoM and the
orientation of the trunk111Since HyQ is not equipped with arms, it suffices
for us to control the trunk orientation instead of the whole robot angular
momentum. and is achieved by implementing a Cartesian-based impedance
controller with a feed-forward term222This is similar to a PD+ controller
[39].. The swing task regulates the swing foot trajectory in order to place it
in the desired location while achieving enough clearance from the terrain.
Similar to the trunk task, the swing task is achieved by implementing a
Cartesian-based impedance controller with a feed-forward term. The WBC
realizes these tasks by computing the optimal generalized accelerations and
contact forces [27] via QP and mapping them to the desired joint torques while
taking into account the full dynamics of the robot, the properties of the
terrain (friction constraints), the unilaterality of the contacts (e.g. the
legs can only push and not pull) (unilateral constraints), and the actuator’s
torque/kinematic limits. The desired torques, will be sent to the lower-level
(torque) controller.

##### 2.2.1 Robot Model

For a legged robot with $n$ DoFs and $c$ feet, the forward kinematics of each
foot is defined by $n_{{a}}$ coordinates333Without the loss of generality, we
consider a quadruped robot with $n=12$ DoFs with point feet, where $c=4$ and
$n_{{a}}=3$.. The total dimension of the feet operational space is
$n_{{f}}=n_{{a}}c$. This can be separated into stance
($n_{\mathrm{st}}=n_{{a}}c_{\mathrm{st}}$) and swing feet
($n_{\mathrm{sw}}=n_{{a}}c_{\mathrm{sw}}$). Since we are interested in
regulating the position of the CoM, we formulate the dynamics in terms of the
CoM, using its velocity rather than the base velocity444In this coordinate
system, the inertia matrix is block diagonal [33]. For the detailed
implementation of the dynamics using the base velocity, see [38]. [33].
Assuming that all the external forces are exerted on the stance feet, we write
the equation of motion that describes the full dynamics of the robot as:

$\underbrace{\begin{bmatrix}M_{\mathrm{com}}&0_{6\times n}\\\ 0_{n\times
6}&\bar{M}_{j}\end{bmatrix}}_{M(q)}\\!\\!\underbrace{\begin{bmatrix}\dot{v}_{\mathrm{com}}\\\
\ddot{q}_{j}\end{bmatrix}}_{\ddot{q}}\\!+\\!\underbrace{\begin{bmatrix}h_{com}\\\
\bar{h}_{j}\end{bmatrix}}_{h}\\!=\\!\begin{bmatrix}0_{6\times n}\\\
\tau_{j}\end{bmatrix}+\underbrace{\begin{bmatrix}J_{\mathrm{st,com}}^{T}\\\
J_{\mathrm{st,j}}^{T}\end{bmatrix}}_{J_{\mathrm{st}}(q)^{T}}F_{\mathrm{grf}}$
(2.1)

where the first 6 rows represent the (un-actuated) floating base part and the
remaining $n$ rows represent the actuated part. $q\in
SE(3)\times\mathbb{R}^{n}$ represents the pose of the whole floating-base
system while
$\dot{q}=\begin{bmatrix}v_{\mathrm{com}}^{T}&\dot{q}_{j}^{T}\end{bmatrix}^{T}\in\mathbb{R}^{6+n}$
and $\ddot{q}=[\dot{v}_{\mathrm{com}}^{T}\
\ddot{q}_{j}^{T}]^{T}\in\mathbb{R}^{6+n}$ are the vectors of generalized
velocities and accelerations, respectively.
$v_{\mathrm{com}}=[\dot{x}_{\mathrm{com}}^{T}\ \omega_{b}^{T}]^{T}$
$\in\mathbb{R}^{6}$ and $\dot{v}_{\mathrm{com}}=[\ddot{x}_{\mathrm{com}}^{T}\
\dot{\omega}_{b}^{T}]^{T}$ $\in\mathbb{R}^{6}$ are the spatial velocity and
acceleration of the floating-base expressed at the CoM.
$M(q)\in\mathbb{R}^{(6+n)\times(6+n)}$ is the inertia matrix, where
$M_{\mathrm{com}}(q)$ $\in\mathbb{R}^{6\times 6}$ is the composite rigid body
inertia matrix of the robot expressed at the CoM. $h\in\mathbb{R}^{6+n}$ is
the force vector that accounts for Coriolis, centrifugal, and gravitational
forces555Note that $h_{com}=-mg+v_{com}\times^{*}M_{com}v_{com}$ according to
the spatial algebra notation, where $m$ is the total robot mass..
$\tau\in\mathbb{R}^{n}$ are the actuated joint torques while
$F_{\mathrm{grf}}\in\mathbb{R}^{n_{\mathrm{st}}}$ is the vector of Ground
Reaction Forces (GRFs) (contact forces). In this context, the floating base
Jacobian $J$ $\in\mathbb{R}^{n_{\mathrm{f}}\times(6+n)}$ is separated into
swing Jacobian $J_{\mathrm{sw}}$ $\in\mathbb{R}^{n_{\mathrm{sw}}\times(6+n)}$
and stance Jacobian $J_{\mathrm{st}}$
$\in\mathbb{R}^{n_{\mathrm{st}}\times(6+n)}$ which could be further expanded
into $J_{\mathrm{st,com}}$ $\in\mathbb{R}^{n_{\mathrm{st}}\times 6}$,
$J_{\mathrm{st,j}}$ $\in\mathbb{R}^{n_{\mathrm{st}}\times n}$,
$J_{\mathrm{sw,com}}$ $\in\mathbb{R}^{n_{\mathrm{sw}}\times 6}$ and
$J_{\mathrm{sw,j}}$ $\in\mathbb{R}^{n_{\mathrm{sw}}\times n}$. The operator
$\bar{[\cdot]}$ denotes the matrices/vectors recomputed after the coordinate
transform to the CoM [38]. Following the sign convention in Fig. 2.1,
recalling the first 6 rows in (2.1), and by defining the gravito-inertial CoM
wrench as $W_{\mathrm{com}}=M_{\mathrm{com}}\dot{v}_{\mathrm{com}}+h_{com}$
$\in\mathbb{R}^{6}$, we can write the floating-base dynamics as:

$\displaystyle W_{\mathrm{com}}=J_{\mathrm{st,com}}^{T}F_{\mathrm{grf}}$ (2.2)

such that $J_{\mathrm{st,com}}^{T}$ maps $F_{\mathrm{grf}}$ to the CoM wrench
space.

The feet velocities $v=[v_{\mathrm{st}}^{T}\ v_{\mathrm{sw}}^{T}]^{T}$
$\in\mathbb{R}^{n_{\mathrm{f}}}$ could be separated into stance
$v_{\mathrm{st}}$ $\in\mathbb{R}^{n_{\mathrm{st}}}$ and swing
$v_{\mathrm{sw}}$ $\in\mathbb{R}^{n_{\mathrm{sw}}}$ feet velocities. The
mapping between $v$ and the generalized velocities $\dot{q}$ is:

$\displaystyle v$ $\displaystyle=$ $\displaystyle J\dot{q}$ (2.3a)
$\displaystyle
v=\begin{bmatrix}J_{\mathrm{com}}&J_{{j}}\end{bmatrix}\begin{bmatrix}v_{\mathrm{com}}\\\
\dot{q}_{j}\end{bmatrix}$ $\displaystyle=$ $\displaystyle
J_{\mathrm{com}}v_{\mathrm{com}}+J_{{j}}\dot{q}_{j}$ (2.3b)

such that $J_{\mathrm{com}}$ $\in\mathbb{R}^{n_{\mathrm{f}}\times 6}$ and
$J_{{j}}$ $\in\mathbb{R}^{n_{\mathrm{f}}\times n}$. Similar to the feet
velocities, we split the feet force vector $F=[F_{\mathrm{st}}^{T}\
F_{\mathrm{sw}}^{T}]^{T}$ $\in\mathbb{R}^{n_{\mathrm{f}}}$, into
$F_{\mathrm{st}}$ $\in\mathbb{R}^{n_{\mathrm{st}}}$ and $F_{\mathrm{sw}}$
$\in\mathbb{R}^{n_{\mathrm{sw}}}$.

###### Assumption 2.1

The robot is walking over rigid terrain in which the stance feet do not move
(i.e., $v_{\mathrm{st}}=\dot{v}_{\mathrm{st}}=0$).

##### 2.2.2 Trunk and Swing Leg Control Tasks

To compliantly achieve a desired motion of the trunk, we define the desired
wrench at the CoM $W_{\mathrm{com,d}}$ using the following: 1) a Cartesian
impedance at the CoM $W_{\mathrm{imp}}$ that is represented by a stiffness
term ($\nabla V_{\mathrm{com,K}}=K_{\mathrm{com}}\Delta x_{\mathrm{com}}$)
with positive definite stiffness matrix $K_{\mathrm{com}}\in R^{6\times 6}$
and a damping term ($D_{\mathrm{com}}\Delta v_{\mathrm{com}}$) with positive
definite damping matrix $D_{\mathrm{com}}\in R^{6\times 6}$, 2) a virtual
gravitational potential gradient to render gravity compensation $(\nabla
V_{\mathrm{com,\bar{g}}}=mg)$666$\nabla V_{[.]}$ denotes the gradient of a
potential function $V_{[.]}$. For more information regarding the Cartesian
stiffness and gravitational potentials, see [29]., 3) a feedforward term to
improve tracking ($W_{\mathrm{ff}}=M_{\mathrm{com}}\dot{v}_{\mathrm{com,d}}$)
and a compensation term for external disturbances $-W_{\mathrm{ext}}$ [41]:

$\displaystyle W_{\mathrm{com,d}}$ $\displaystyle=$ $\displaystyle
W_{\mathrm{imp}}+\nabla
V_{\mathrm{com,\bar{g}}}+W_{\mathrm{ff}}-W_{\mathrm{ext}}$ (2.4a)
$\displaystyle W_{\mathrm{imp}}$ $\displaystyle=$ $\displaystyle\nabla
V_{\mathrm{com,K}}+D_{\mathrm{com}}\Delta v_{\mathrm{com}}$ (2.4b)

such that $\Delta x_{\mathrm{com}}=x_{\mathrm{com,d}}-x_{\mathrm{com}}$,
$\Delta v_{\mathrm{com}}=v_{\mathrm{com,d}}-v_{\mathrm{com}}$ are the tracking
errors $\in\mathbb{R}^{6}$ of the position and velocity, respectively.

Similarly, the tracking of the swing task is obtained by the virtual force
$F_{\mathrm{sw,d}}\in\mathbb{R}^{n_{\mathrm{sw}}}$. This is generated by 1) a
Cartesian impedance at the swing foot that is represented by a stiffness term
($\nabla V_{\mathrm{sw}}=K_{\mathrm{sw}}\Delta x_{\mathrm{sw}}$) with positive
definite stiffness matrix $K_{\mathrm{sw}}\in R^{n_{\mathrm{sw}}\times
n_{\mathrm{sw}}}$ and a damping term ($D_{\mathrm{sw}}\Delta v_{\mathrm{sw}}$)
with positive definite damping matrix $D_{\mathrm{sw}}\in
R^{n_{\mathrm{sw}}\times n_{\mathrm{sw}}}$, and 2) a feedforward term to
improve tracking
($F_{\mathrm{sw,ff}}=M_{\mathrm{sw}}\dot{v}_{\mathrm{sw,ff}}$):

$\displaystyle F_{\mathrm{sw,d}}$ $\displaystyle=$ $\displaystyle\nabla
V_{\mathrm{sw}}+D_{\mathrm{sw}}\Delta v_{\mathrm{sw}}+F_{\mathrm{sw,ff}}$
(2.5)

such that $\Delta x_{\mathrm{sw}}=x_{\mathrm{sw,d}}-x_{\mathrm{sw}}$ and
$\Delta v_{\mathrm{sw}}=v_{\mathrm{sw,d}}-v_{\mathrm{sw}}$ are the tracking
errors of the swing feet positions and velocities respectively. Alternatively,
it is possible to write this task at the acceleration level, with the
difference that the gains $\mathrm{K}_{\mathrm{sw}}$ and
$\mathrm{D}_{\mathrm{sw}}$ have no physical meaning:

$\displaystyle\dot{v}_{\mathrm{sw,d}}=\dot{v}_{\mathrm{sw,ff}}+\mathrm{K}_{\mathrm{sw}}\Delta
x_{\mathrm{sw}}+\mathrm{D}_{\mathrm{sw}}\Delta v_{\mathrm{sw}}$ (2.6)

##### 2.2.3 Optimization

To fulfill the motion tasks in Section 2.2.2 and to distribute the load on the
stance feet, while respecting the mentioned constraints, we formulate the QP:

$\displaystyle\underset{u=[\ddot{q}^{T}\hskip
5.69046ptF_{\mathrm{grf}}^{T}]^{T}}{\min}\|W_{\mathrm{com}}-W_{\mathrm{com,d}}\|^{2}_{Q}+\|u\|^{2}_{R}$
(2.7a) $\displaystyle\text{s.t.}\hskip 14.22636pt\quad Au=b$ (2.7b)
$\displaystyle\underline{d}<Cu<\bar{d}$ (2.7c)

such that our decision variables $u=[\ddot{q}^{T}\
F_{\mathrm{grf}}^{T}]^{T}\in\mathbb{R}^{6+n+n_{\mathrm{st}}}$ are the
generalized accelerations $\ddot{q}$ and the contact forces
$F_{\mathrm{grf}}$. The cost function (2.7a) is designed to minimize the trunk
task and to regularize the solution. The equality constraints (2.7b) encode
dynamic consistency, stance constraints and swing tasks. The inequality
constraints (2.7c) encode friction constraints, joint kinematic and torque
limits. All constraints are stacked in the matrix
$A^{T}=\begin{bmatrix}A_{p}^{T}&A_{\mathrm{st}}^{T}&A_{\mathrm{sw}}^{T}\end{bmatrix}$
and
$C^{T}=\begin{bmatrix}C_{\mathrm{fr}}^{T}&C_{j}^{T}&C^{T}_{\tau}\end{bmatrix}$
and detailed in the following sections.

###### 2.2.3.1 Cost

The first term of the cost in (2.7a) represents the tracking error between the
actual $W_{\mathrm{com}}$ and the desired $W_{\mathrm{com,d}}$ CoM wrenches
from (2.2) and (2.4a) respectively. Since $W_{\mathrm{com}}$ is not a decision
variable, we compute it from the contact forces (see (2.2)) and re-write
$\|W_{\mathrm{com}}-W_{\mathrm{com,d}}\|_{Q}^{2}$ in the form of $\|Gu-
g_{0}\|_{Q}^{2}$ with:

$\displaystyle
G=\begin{bmatrix}0_{6\times(6+n)}&J^{T}_{\mathrm{st,com}}\end{bmatrix},\quad
g_{0}=W_{\mathrm{com,d}}$ (2.8)

###### 2.2.3.2 Physical consistency

To enforce physical consistency between $F_{\mathrm{grf}}$ and $\ddot{q}$, we
impose the dynamics of the unactuated part of the robot (the trunk dynamics in
(2.2)) as an equality constraint:

$A_{p}=\begin{bmatrix}M_{\mathrm{com}}&0_{6\times
n}&-J^{T}_{\mathrm{st,com}}\end{bmatrix},\quad b_{p}=-h_{com}$ (2.9)

###### 2.2.3.3 Stance condition

We can encode the stance feet constraints by re-writing them at the
acceleration level in order to be compatible with the decision variables.
Since $v_{\mathrm{st}}=J_{\mathrm{st}}\dot{q}$, differentiating once in time,
yields to
$\dot{v}_{\mathrm{st}}=J_{\mathrm{st}}\ddot{q}+\dot{J}_{\mathrm{st}}\dot{q}$.
Recalling Assumption 2.1 yields
$J_{\mathrm{st}}\ddot{q}+\dot{J}_{\mathrm{st}}\dot{q}=0$ which is encoded as:

$A_{\mathrm{st}}=\begin{bmatrix}J_{\mathrm{st}}&0_{n_{\mathrm{st}}\times
n_{\mathrm{st}}}\end{bmatrix},\quad
b_{\mathrm{st}}=-\dot{J}_{\mathrm{st}}\dot{q}$ (2.10)

such that $\dot{J}_{\mathrm{st}}$ is the time derivative of $J_{\mathrm{st}}$.
For numerical precision, we compute the product $\dot{J}_{\mathrm{st}}\dot{q}$
using spatial algebra.

###### 2.2.3.4 Swing task

Similar to Section 2.2.3.3, we can encode the swing task directly as an
equality constraint, i.e. by enforcing the swing feet to follow a desired
swing acceleration
$\dot{v}_{\mathrm{sw}}(q)=\dot{v}_{\mathrm{sw,d}}\in\mathbb{R}^{n_{\mathrm{sw}}}$
yielding:

$J_{\mathrm{sw}}\ddot{q}+\dot{J}_{\mathrm{sw}}\dot{q}=\dot{v}_{{\mathrm{sw,d}}}$
(2.11)

that in matrix form becomes777Alternatively, it is possible to write the swing
task at the joint space rather than in the operational space by changing the
matrix $A_{\mathrm{sw}},b_{\mathrm{sw}}.$:

$A_{\mathrm{sw}}=\begin{bmatrix}J_{\mathrm{sw}}&0_{n_{\mathrm{sw}}\times
n_{\mathrm{st}}}\end{bmatrix},\quad
b_{\mathrm{sw}}=\dot{v}_{{\mathrm{sw,d}}}-\dot{J}_{\mathrm{sw}}\dot{q}$ (2.12)

where we computed $\dot{v}_{{\mathrm{sw,d}}}$ as in (2.6). Note that this
implementation is analogous to the trunk task in Section 2.2.2. The difference
is that this implementation is at the acceleration level while the other is at
the force level. However, without any loss of generality, the formulation
(2.5) could also be used. In Section 2.4.2 we incorporate slacks in the
optimization to allow temporary violation of the swing tasks (e.g. useful when
the kinematic limits are reached).

###### 2.2.3.5 Friction cone constraints

To avoid slippage and obtain a smooth loading/unloading of the legs, we
incorporate friction/unilaterality constraints. For that, we ensure that the
contact forces lie inside the friction cones and their normal components stay
within some user-defined values (i.e. maximum and minimum force magnitudes).
We approximate the friction cones with square pyramids to express them with
linear constraints. The fact that the ground contacts are unilateral, can be
naturally encoded by setting an “almost-zero” lower bound on the normal
component, while the upper bound allows us to regulate the amount of “loading”
for each leg. We define the friction inequality constraints as:

$\displaystyle\underline{d}_{\mathrm{fr}}<C_{\mathrm{fr}}u<\bar{d}_{\mathrm{fr}},\quad
C_{\mathrm{fr}}=\begin{bmatrix}0_{p\times(6+n)}&F_{\mathrm{fr}}\end{bmatrix}$
(2.13)

with:

$F_{\mathrm{fr}}=\begin{bmatrix}F_{0}&\dots&0\\\ \vdots&\ddots&\vdots\\\
0&\dots&F_{c}\end{bmatrix},\ \ \
\underline{d}_{\mathrm{fr}}=\begin{bmatrix}\underline{f}_{0}\\\ \vdots\\\
\underline{f}_{c}\end{bmatrix},\ \ \
\bar{d}_{\mathrm{fr}}=\begin{bmatrix}\bar{f}_{0}\\\ \vdots\\\
\bar{f}_{c}\end{bmatrix}$ (2.14)

where $F_{\mathrm{fr}}\in\mathbb{R}^{p\times n_{\mathrm{st}}}$ is a block
diagonal matrix that encodes the friction cone boundaries and select the
normals, for each stance leg and
$\underline{d}_{\mathrm{fr}},\bar{d}_{\mathrm{fr}}\in\mathbb{R}^{p}$ are the
lower/upper bounds respectively. For the detailed implementation of the
friction constraints refer to [34].

###### 2.2.3.6 Torque limits

We notice that the torques be obtained from the decision variables since they
can be expressed as a bi-linear function of $\ddot{q}_{j}$ and
$F_{\mathrm{grf}}$. Therefore, the constraint on the joint torques (i.e., the
actuation limits $\tau_{\min}<\tau_{j}<\tau_{\max}$) can be encoded by
exploiting the actuated part of the dynamics (2.1):

$\displaystyle\underline{d}_{\tau}<C_{\tau}u<\bar{d}_{\tau},\quad
C_{\tau}=\begin{bmatrix}0_{n\times
6}&\bar{M}_{j}&-J_{\mathrm{st,j}}^{T}\end{bmatrix}$ (2.15)
$\displaystyle\underline{d}_{\tau}=-\bar{h}_{j}+\tau_{\min}(q_{j}),\quad\bar{d}_{\tau}=-\bar{h}_{j}+\tau_{\max}(q_{j})$

where $\tau_{\min}(q_{j}),\tau_{\max}(q_{j})\in\mathbb{R}^{n}$ are the
lower/upper bounds on the torques. In the case of our quadruped robot, these
bounds must be recomputed at each control loop because they depend on the
joint positions. This is due to the presence of linkages on the sagittal
joints (HFE and KFE), that set a joint-dependent profile on the maximum torque
(non-linear in the joint range).

###### 2.2.3.7 Joint kinematic limits

We enforce joint kinematic constraints as function of the joint accelerations
(i.e. $\ddot{q}_{j_{\min}}<\ddot{q}_{j}<\ddot{q}_{j_{\max}}$). We select them
via the matrix $C_{j}$:

$\displaystyle\underline{d}_{j}<C_{j}u<\bar{d}_{j},\quad{C}_{j}=\begin{bmatrix}0_{n\times
6}&I_{n\times n}&0_{n\times n_{\mathrm{st}}}\end{bmatrix}$ (2.16)
$\displaystyle\underline{d}_{j}=\ddot{q}_{j_{\min}}(q_{j}),\quad\quad\bar{d}_{j}=\ddot{q}_{j_{\max}}(q_{j})$
(2.17)

such that $\ddot{q}_{j_{\min}}(q_{j})$ and $\ddot{q}_{j_{\max}}(q_{j})$ are
the upper/lower bounds on accelerations. These bounds should be recomputed at
each control loop. They are set in order to make the joint to reach the end-
stop at a zero velocity in a time interval $\Delta t=10dt$, where $dt$ is the
loop duration. For instance, if the joint is at a distance
$q_{j_{\max}}-q_{j}$ from the end-stop with a velocity $\dot{q}_{j}$, the
deceleration to cover this distance in a time interval $\Delta t$, and
approach the end-stop with zero velocity, will be:

$\ddot{q}_{j_{\min,\max}}=-\frac{2}{\Delta
t^{2}}(q_{j_{\min,\max}}-q_{j}-\Delta t\,\dot{q}_{j})$ (2.18)

##### 2.2.4 Torque computation

We map the optimal solution
$u^{*}=\begin{bmatrix}\ddot{q}^{*}&F_{\mathrm{grf}}^{*}\end{bmatrix}$ obtained
by solving (2.7), into desired joint torques $\tau_{d}^{*}\in\mathbb{R}^{n}$
using the actuated part of the dynamics equation of the robot as:

$\tau_{d}^{*}=\bar{M}_{j}\ddot{q_{j}}^{*}+\bar{h}_{j}-J_{\mathrm{st,j}}^{T}F_{\mathrm{grf}}^{*}$
(2.19)

#### 2.3 Passivity Analysis

The overall system consists of the WBC, the robot and the environment. This
system is said to be passive if all these components, and their
interconnections are passive [37]. If the robot and the environment are
passive, and the controller is proven to be passive, then the overall system
is passive [42]. A system (with input $u$ and output $y$) is said to be
passive if there exists a storage function $S$ that is bounded from below and
its derivative $\dot{S}$ is less than or equal to its supply rate
($s=y^{T}u$). In this context, we define the total energy stored in the
controller to be the candidate storage function for the controller $S=V$. The
rest of this section is devoted to analyze the passivity of the overall
system.

###### Assumption 2.2

A feasible solution exists for the QP in (2.7) in which the motion tasks are
achieved. Moreover, we do not consider the feed-forward terms in (2.4a) and
(2.5) leaving this to future developments.

We start by defining the velocity error at the joints and at the stance feet
to be $\Delta\dot{q}_{j}=\dot{q}_{j,d}-\dot{q}_{j}$, and $\Delta
v_{\mathrm{st}}=v_{\mathrm{st,d}}-v_{\mathrm{st}}$, respectively. We also
define the desired feet forces $F_{\mathrm{d}}=[F_{\mathrm{st,d}}^{T}\
F_{\mathrm{sw,d}}^{T}]^{T}$ such that, by following the sign convention in
Fig. 2.1, the mapping between $F_{\mathrm{d}}$ and $W_{\mathrm{com,d}}$ is
expressed as888Since we are analyzing the passivity of the controller, we are
interested in the forces exerted by the robot on the environment rather than
the forces exerted by the environment on the robot. Hence the mapping in
(2.20) is negative.:

$W_{\mathrm{com,d}}=-J_{\mathrm{com}}^{T}F_{\mathrm{d}},$ (2.20)

while mapping between $F_{d}$ and $\tau_{j}$ is expressed as999Assuming a
perfect low level torque control tracking (i.e., $\tau_{\mathrm{d}}=\tau$).

$\displaystyle\tau$ $\displaystyle=$ $\displaystyle J_{{j}}^{T}F_{d}.$ (2.21)

By defining $\nabla V_{\mathrm{com}}=\nabla V_{\mathrm{com,K}}+\nabla
V_{\mathrm{com,\bar{g}}}$ and recalling (2.20), we rewrite (2.4a) under
Assumption 2.2 as:

$\displaystyle\nabla V_{\mathrm{com}}$ $\displaystyle=$ $\displaystyle
W_{\mathrm{com,d}}-D_{\mathrm{com}}\Delta v_{\mathrm{com}}$ (2.22a)
$\displaystyle=$ $\displaystyle-
J_{\mathrm{com}}^{T}F_{\mathrm{d}}-D_{\mathrm{com}}\Delta v_{\mathrm{com}}.$
(2.22b)

##### 2.3.1 Analysis

The overall energy in the whole-body controller is the one stored in the
virtual impedance at the CoM and the potential energy due to gravity
compensation ($V_{\mathrm{com}}$), and the energy stored in the virtual
impedances at the swing feet ($V_{\mathrm{sw}}$)101010In this analysis we use
the formulation (2.5).:

$V=V_{\mathrm{com}}+V_{\mathrm{sw}}.$ (2.23)

The time derivatives are111111The time derivative of an arbitrary storage
function $\dot{V}(\Delta x(t))$ that is a function of $\Delta x(t)$ could be
written as $\dot{V}=\frac{d}{dt}\Delta
x^{T}(t)\cdot\frac{\partial}{\partial\Delta x(t)}V$ that is written for short
as $\dot{V}=\Delta v^{T}\nabla V$.:

$\dot{V}=\dot{V}_{\mathrm{com}}+\dot{V}_{\mathrm{sw}}=\Delta
v_{\mathrm{com}}^{T}\nabla V_{\mathrm{com}}+\Delta v_{\mathrm{sw}}^{T}\nabla
V_{\mathrm{sw}}.$ (2.24)

Recalling (2.5) and (2.22b), (2.24) yields:

$\displaystyle\dot{V}$ $\displaystyle=$ $\displaystyle\Delta
v_{\mathrm{com}}^{T}(-J_{\mathrm{com}}^{T}F_{\mathrm{d}}-D_{\mathrm{com}}\Delta
v_{\mathrm{com}})+$ (2.25) $\displaystyle\Delta
v_{\mathrm{sw}}^{T}(F_{\mathrm{sw,d}}-D_{\mathrm{sw}}\Delta
v_{\mathrm{sw}})).$

We regroup $\dot{V}$ in terms of the non-damping terms $\dot{V}_{1}$ and
damping terms $\dot{V}_{2}$ yielding:

$\displaystyle\dot{V_{1}}$ $\displaystyle=$ $\displaystyle-\Delta
v_{\mathrm{com}}^{T}J_{\mathrm{com}}^{T}F_{\mathrm{d}}+\Delta
v_{\mathrm{sw}}^{T}F_{\mathrm{sw,d}}$ (2.26a) $\displaystyle\dot{V_{2}}$
$\displaystyle=$ $\displaystyle-\Delta
v_{\mathrm{com}}^{T}D_{\mathrm{com}}\Delta v_{\mathrm{com}}-\Delta
v_{\mathrm{sw}}^{T}D_{\mathrm{sw}}\Delta v_{\mathrm{sw}}.$ (2.26b)

We rewrite (2.3b) in terms of $\Delta v_{\mathrm{com}}$, $\Delta v$ and
$\Delta\dot{q}_{\mathrm{j}}$ as:

$\displaystyle\Delta v$ $\displaystyle=$ $\displaystyle J_{\mathrm{com}}\Delta
v_{\mathrm{com}}+J_{\mathrm{j}}\Delta\dot{q}_{\mathrm{j}}$ (2.27a)
$\displaystyle\Delta v_{\mathrm{com}}^{T}J_{\mathrm{com}}^{T}$
$\displaystyle=$
$\displaystyle-\Delta\dot{q}_{\mathrm{j}}^{T}J_{\mathrm{j}}^{T}+\Delta v^{T}.$
(2.27b)

Plugging, (2.27b) in (2.26a) yields121212From the definition of $v$ and
$F_{d}$, we get
$v^{T}F_{d}=v_{\mathrm{st}}^{T}F_{\mathrm{st,d}}+v_{\mathrm{sw}}^{T}F_{\mathrm{sw,d}}$.:

$\displaystyle\dot{V_{1}}$ $\displaystyle=$
$\displaystyle\Delta\dot{q}_{\mathrm{j}}^{T}J_{\mathrm{j}}^{T}F_{\mathrm{d}}-\Delta
v^{T}F_{\mathrm{d}}+\Delta v_{\mathrm{sw}}^{T}F_{\mathrm{sw,d}}$ (2.28a)
$\displaystyle=$
$\displaystyle\Delta\dot{q}_{\mathrm{j}}^{T}J_{\mathrm{j}}^{T}F_{\mathrm{d}}-\Delta
v_{\mathrm{st}}^{T}F_{\mathrm{st,d}}.$ (2.28b)

Plugging (2.21) and into (2.28b) yields:

$\displaystyle\dot{V_{1}}$ $\displaystyle=$
$\displaystyle\Delta\dot{q}_{\mathrm{j}}^{T}\tau-\Delta
v_{\mathrm{st}}^{T}F_{\mathrm{st,d}}.$ (2.29)

Under Assumption 2.1, (2.29) yields:

$\displaystyle\dot{V_{1}}$ $\displaystyle=$
$\displaystyle\Delta\dot{q}_{\mathrm{j}}^{T}\tau.$ (2.30)

Thus, $\dot{V}$ could be rewritten as:

$\displaystyle\dot{V}=\Delta\dot{q}_{\mathrm{j}}^{T}\tau-\Delta
v_{\mathrm{com}}^{T}D_{\mathrm{com}}\Delta v_{\mathrm{com}}-\Delta
v_{\mathrm{sw}}^{T}D_{\mathrm{sw}}\Delta v_{\mathrm{sw}}.$ (2.31)

##### 2.3.2 Proof

Under Assumption 2.2, the designed WBC is an impedance control with gravity
compensation that, similar to a PD+ [39], defines a map of
$(\dot{q}_{\mathrm{j}}-\dot{q}_{\mathrm{j,d}})\mapsto-\tau$131313Note that
$\dot{q}_{\mathrm{j}}-\dot{q}_{\mathrm{j,d}}=-\Delta\dot{q}_{\mathrm{j}}$.
Thus, the controller with the map
$(\dot{q}_{\mathrm{j}}-\dot{q}_{\mathrm{j,d}})\mapsto-\tau$ has a supply rate
of $\Delta\dot{q}_{\mathrm{j}}^{T}\tau$.. This controller is passive if $V$ is
bounded from below and $\dot{V}\leq\Delta\dot{q}_{\mathrm{j}}^{T}\tau$. Since
$V$ consists of positive definite potentials that resemble Cartesian
stiffnesses at the CoM and the swing leg(s), and under the assumption the
gravitational potential is bounded from below (see [43]), $V$ is also bounded
from below. Additionally, recalling (2.31) proves that the controller is
indeed passive; thus, the overall system is passive.

#### 2.4 Implementation Details

This section describes some pragmatic details that we found crucial in the
implementation on the real platform.

##### 2.4.1 Stance task

Uncertainties in estimating the terrain’s normal direction and friction
coefficient could result in slippage. This can lead to considerable motion of
the stance feet with possible loss of stability. To avoid this, a joint
impedance feedback loop could be run in parallel to the WBC, at the price of
losing the capability of optimizing the GRFs. A cleaner solution is to
incorporate, in the optimization, Cartesian impedances specifically designed
to keep the relative distance among the stance feet constant (we denote it
stance task).

The stance task has an influence only when there is an anomalous motion in the
stance feet, retaining the possibility to freely optimize for GRFs in normal
situations. This can be achieved by re-formulating the stance condition in
(2.10) as a desired stance feet acceleration $\dot{v}_{\mathrm{st,d}}$ as:

$\dot{v}_{\mathrm{st,d}}=K_{\mathrm{st}}(\hat{x}_{\mathrm{st}}-x_{\mathrm{st}})-D_{\mathrm{st}}v_{\mathrm{st}}.$
(2.32)

This term is added to $b_{st}$ (in (2.10)) as
$b_{st}=-\dot{J}_{st}\dot{q}+\ddot{x}^{d}_{f_{st}}$ where
$\hat{x}_{\mathrm{st}}$ is a sample of the foot position at the touchdown
(expressed in the world frame).

##### 2.4.2 Constraint Softening

Adding slack variables to an optimization problem is commonly done to avoid
infeasible solutions, by allowing a certain degree of constraint violation.
Infeasibility can occur when hard constraints are conflicting with each other,
which can be the case in our QP. Hence, some of the equalities/inequalities in
(2.7) should be relaxed if they are in conflict.

We decided not to introduce slacks in the torque constraints (Section 2.2.3.6)
or the dynamics (Section 2.2.3.2) keeping them as hard constraints, since
torque constraints and physical consistency should never be violated. On the
other hand it is important to allow a certain level of relaxation for the
swing tasks in Section 2.2.3.4 that could be violated if the joint kinematic
limits are reached141414Using slacks in friction constraints did not result in
significant improvements..

To relax the constraints of the swing task, first, we augment the decision
variables $\tilde{u}=[u^{T}\ \epsilon^{T}]^{T}\in\mathbb{R}^{6+n+k+n_{sw}}$
with the vector of slack variables $\epsilon\in\mathbb{R}^{n_{sw}}$ where we
introduce a slack variable for each direction of the swing tasks. Then, we
replace the equality constraint $A_{sw}u+b_{sw}=0$ of the swing tasks, by two
inequality constraints:

$\displaystyle-\epsilon\leq A_{sw}\tilde{u}$
$\displaystyle+b_{sw}\leq\epsilon$ $\displaystyle\epsilon$ $\displaystyle\geq
0.$ (2.33)

The first inequality in (2.4.2) restricts the solution to a bounded region
around the original constraint while the second one ensures that the slack
variables remain non-negative. To make sure that there is a constraint
violation only when the constraints are conflicting, we minimize the norm of
the slack vector adding a regularization term $\alpha\|\epsilon\|^{2}$ to
(2.7a) with a high weight $\alpha$.

To reduce the computational complexity, we could have introduced a single
slack for each swing task (rather than one for each direction). However, this
could create coupling errors in the tracking. For instance, since the Hip
Flexion-Extension (HFE) joint (see Fig. 2.1) mainly acts in the $XZ$ plane, if
it reaches its joint limit, only that plane should be affected leaving the $Y$
direction unaffected. A single slack couples the three directions causing
tracking errors also in the $Y$ direction. Conversely, using multiple slacks,
only the directions the HFE acts upon, will be affected.

#### 2.5 Results

In this section we validate the capabilities of the controller under various
terrain conditions and locomotion gaits. The WBC and torque control loops run
in real-time threads at 250 Hz and 1 kHz, respectively. We set the gains for
the swing tasks to $K_{sw}=\mathrm{diag}({2000,2000,2000})$ and
$D_{sw}=\mathrm{diag}({20,20,20})$, while for the trunk task we set
$K_{x}=\mathrm{diag}({2000,2000,2000})$ $D_{x}=\mathrm{diag}({400,400,400})$
and $K_{\theta}=\mathrm{diag}({1000,1000,1000})$
$D_{\theta}=\mathrm{diag}({200,200,200})$. These values proved to be working
in both simulation and real experiments. The results are collected in the
accompanying video151515Link: https://youtu.be/Lg3V_juoE1w. Additionally, in
experimental trials, we also included a low gain joint-space attractor (PD
controller) for the swing task, since imprecise torque tracking of the knee
joints (due to the low inertia) produce control instabilities in an
operational space implementation (e.g. the one in Section 2.2.3.4).

##### 2.5.1 Constraint Softening through Slack Variables

Figure 2.2:  Simulation. Effect of (kinematic limits) slacks variables on foot
tracking. The upper-left/right and bottom-left plots show the tracking of the
desired foot position (LH leg) in $X$, $Y$ and $Z$, respectively. Bottom-right
plot depicts the joint limits (black line) and the actual position (blue line)
of the HFE joint. The red shaded area underlines that when the slack
increases, the HFE joint is properly clamped.

In Fig. 2.2 we artificially incremented the lower limit of the LH-HFE joint.
When the limit is hit, the bound on the joint acceleration (2.18) produces a
desired torque command that stops its motion. This “naturally” clamps the
actual joint position to the limit (bottom-left plot) and influences the foot
tracking mainly along the $X$ and $Z$ directions.

Computational time: the solution of the QP takes between 90-110 $\mu s$ on a
Intel i5 machine without the slacks variables. After augmenting the problem
with the slack variables and its constraints, it increases 30 % on average
(120-150 $\mu s$). However it still remains suitable for real-time
implementation (250 $Hz$).

##### 2.5.2 Friction Constraints and Bounded Slippage

We evaluated in simulation the controller performance against inaccurate
friction coefficient estimates $\mu$, which define incorrectly the friction
cone constraints in the WBC. In the accompanying video, we show an example
where the robot crawls at 0.11 m/s on a slippery floor ($\mu=0.4$) while we
set the friction coefficient to $\mu=1.0$ in the WBC, to emulate an estimation
error. Simulation results support the fact that foot slippage remains bounded
by the action of the stance task (Section 2.4.1). If we gradually correct
$\mu$ the slippage events completely disappear; allowing an increase of
forward velocity up to 0.16 m/s.

##### 2.5.3 Torque Limits and Load Redistribution

We analyzed in simulation the effect of adding an artificial torque limit, in
our WBC. This helps us to derive controllers that are robust against joint
damages. Figure 2.3 shows a reduction of the torque limits down to 26 Nm in
the Left-Front (LF)-KFE joint and the load redistribution among the other
joints (HAA and HFE) of the LF leg. Indeed, while the KFE joint torque is
clamped, the HFE is loaded more (lower plot). This load redistribution did not
affect the trunk motion and it demonstrates how the controller exploits the
torque redundancy by finding a new load distribution.

Figure 2.3: Simulation. Effect of introducing an artificial torque limit on
the KFE joint of the LF leg during a typical crawl. The shaded area represents
the swing phase of the leg while the unshaded part is the stance phase. We
reduced the torque limit down to 26 Nm (black line, upper plot), and as a
consequence, the HFE torque is increased by the controller (bottom plot).

We carried out also intensive experimental validation in various challenging
terrains. Slopes increase the probability of reaching torque limits because of
the more demanding kinematic configurations. Indeed, in Fig. 2.4, the robot
reached three times the torque limits (red shaded areas). Crossing this
terrain would not be possible without enforcing the torque limits as hard
constraints.

Figure 2.4: Real experiments. Reaching the torque limits on the RF-HFE joint
while climbing up and down two ramps. The HyQ robot reached three times its
torque limits (red shaded areas). The real torque (blue line) is tracking the
desired one (not shown) computed from our whole-body controller while
satisfying the joint limits (black line). The torque limits are time-varying
due to the joint mechanism.

##### 2.5.4 Different Torque Regularization Schemes

By setting different regularizations in (2.7a) [34], we can either choose to
maximize the robustness to uncertainties in the friction parameters (e.g. GRFs
closer to the friction cone normals) or to minimize the joint
torques161616Setting the weighting matrix
$R_{kk}=J_{st}S^{T}R_{\tau}SJ_{st}^{T}$ where: $R_{kk}$ is the sub-block of
$R$ that regularizes for GRFs variables in (2.7) and $S$ selects the actuation
joints.. In the latter case, for instance, we could encourage the controller
to use a particular joint by increasing its corresponding weight. If we
gradually increase the weight of the Knee Flexion-Extension (KFE) joints (see
accompanying video), the effect of torque regularization becomes visible
because the GRFs are no longer vertical. Indeed the GRFs start to point toward
the knee axis in order to reduce its torque command.

##### 2.5.5 Comparison with Previous Controller (Quasi-Static)

We compare our whole-body controller (dynamic) against a centroidal-based
controller (quasi-static) [34]. As metric we use the $l^{2}$-norm for the
linear $e_{x}$ and angular $e_{\theta}$ tracking errors of the trunk task. If
we increase linearly the forward speed from 0.04 m/s to 0.15 m/s, the tracking
error is reduced approximately by 50 % in comparison to the quasi-static
controller (Fig. 2.5). This is due to the fact that our WBC computes both
joint accelerations and contact forces, which allows a proper mapping of
torque commands (inverse dynamics). Indeed this results in better accuracy in
the execution of more dynamic motions.

Figure 2.5: Simulation. Comparison of tracking errors for the trunk task of a
quasi-static controller against our whole-body controller (dynamic).
$l^{2}$-norms of linear and angular errors are shown in the top and bottom
figures. Note that the errors are reset to zero at each step due to re-
planning.

##### 2.5.6 Disturbance Rejection against Unstable Foothold

We encoded compliance tracking of the CoM task through a virtual impedance.
Friction cone constraints help to instantaneously keep the robot’s balance
whenever a tracking error happens due to, for instance, an unstable foothold.
Furthermore joint constraints (positions and torques) guarantee feasibility of
the computed torque commands. Figure 2.6 shows how the controller compliantly
tracks the desired CoM trajectory during an unstable footstep (a rolling
stepping-stone) that occurs at $t=6.5$ s (experiments results from [44]). This
creates tracking errors on the CoM height, yet, good tracking performance is
kept for the horizontal CoM motion, due to the friction cone constraints that
maintained the robot’s balance along the entire locomotion.

Figure 2.6: Real experiments. Disturbance rejection against unstable foothold
that occurs when a stepping-stone rolled under the RF leg at $t=6.5$ s. The
controller lost tracking of the CoM height, however, the friction cone
constraints keep instantaneously the robot’s balance. Indeed a good tracking
of the horizontal motion of the CoM is obtained. Note that the red shaded area
depicts the moments of the disturbance rejection.

##### 2.5.7 Locomotion over Slopes

These experiments have been performed with online terrain mapping [41]. Both
the terrain mapping and the whole-body controller make use of a drift-free
state estimation algorithm to obtain the body state. The friction cone
constraints of the controller are described given the real terrain normals
provided by an onboard mapping algorithm171717The controller action can be
greatly improved by setting the real terrain normal (under each foot) rather
than using a default value for all the feet.. The friction coefficient has
been conservatively set to 0.7 for all the experiments. Figure 2.7 shows
different snapshots of various challenging terrain used to evaluate our
controller. The centroidal trajectory, gait and footholds are computed
simultaneously as described in [25].

Figure 2.7: Snapshots of experimental trials to evaluate our whole-body
control and the online terrain mapping. Left column: crossing a 22 cm gap with
a 7 cm step. Right column: traversing two ramps with a 15 cm gap between them.

##### 2.5.8 Tracking Performance with Different Gaits

The quadrupedal trotting gait is difficult to control because the robot uses
only two legs at the time to achieve the tracking of the desired CoM motion
and of the trunk orientation. Figure 2.8 depicts the roll and pitch tracking
for climbing up a ramp during a trotting gait. Although a trot is an under-
actuated gait, our controller can still track the desired orientation.
Moreover, in these cases, the orientation error is always below 0.2 rad.

Figure 2.8: Real experiments. Roll and pitch tracking performance while
climbing up a ramp with a trotting gait. Although there is under-actuation the
controller can still track roll and pitch motions.

#### 2.6 Conclusion

This paper presented an experimental validation of our passive WBC. Compared
to our previous work [34], the presented WBC enables higher dynamic motions
thanks to the use of the full dynamics of the robot. Although similar
controllers have been proposed in the literature (e.g. [27, 29, 26]), we
validated our locomotion controller on HyQ over a wide range of challenging
terrain (slopes, gaps, stairs, etc.), using different gaits (crawl and trot).
Additionally, we have analyzed the controller capabilities against 1)
inaccurate friction coefficient estimation, 2) unstable footholds, 3) changes
in the regularization scheme and 4) the load redistribution under restrictive
torque limits. Extensive experimental results validated the controller
performance together with the online terrain mapping and the state estimation.
Moreover, we demonstrated experimentally the superiority of our WBC compared
to a quasi-static control scheme [34].

###  Chapter 3 STANCE:
Locomotion Adaptation over Soft Terrain

© 2020 IEEE. Reprinted, with permission. S. Fahmi, M. Focchi, A. Radulescu, G.
Fink, V. Barasuol and C. Semini, "STANCE: Locomotion Adaptation Over Soft
Terrain," in IEEE Transactions on Robotics (T-RO), vol. 36, no. 2, pp.
443-457, April 2020, doi: 10.1109/TRO.2019.2954670.

Abstract. Whole-Body Control (WBC) has emerged as an important framework in
locomotion control for legged robots. However, most WBC frameworks fail to
generalize beyond rigid terrains. Legged locomotion over soft terrain is
difficult due to the presence of unmodeled contact dynamics that WBCs do not
account for. This introduces uncertainty in locomotion and affects the
stability and performance of the system. In this paper, we propose a novel
soft terrain adaptation algorithm called STANCE: Soft Terrain Adaptation and
Compliance Estimation. STANCE consists of a WBC that exploits the knowledge of
the terrain to generate an optimal solution that is contact consistent and an
online terrain compliance estimator that provides the WBC with terrain
knowledge. We validated STANCE both in simulation and experiment on the
Hydraulically actuated Quadruped (HyQ) robot, and we compared it against the
state of the art WBC. We demonstrated the capabilities of STANCE with multiple
terrains of different compliances, aggressive maneuvers, different forward
velocities, and external disturbances. STANCE allowed HyQ to adapt online to
terrains with different compliances (rigid and soft) without pre-tuning. HyQ
was able to successfully deal with the transition between different terrains
and showed the ability to differentiate between compliances under each foot.

Accompanying Video. https://youtu.be/0BI4581DFjY

#### 3.1 Introduction

Whole-Body Control (WBC) frameworks have achieved remarkable results in legged
locomotion control [30, 45, 1]. Their main feature is that they use
optimization techniques to solve the locomotion control problem. WBC can
achieve multiple tasks in an optimal fashion by exploiting the robot’s full
dynamics and reasoning about both the actuation constraints and the contact
interaction. These tasks include balancing, interacting with the environment,
and performing dynamic locomotion over a wide variety of terrains [1]. The
tasks are executed at the robot’s end effectors, but can also be utilized for
contacts anywhere on the robot’s body [35] or for a cooperative manipulation
task between robots [36].

To date, most of the work done on WBC assumes that the ground is rigid (i.e.,
rigid contact consistent). However, if the robot traverses soft terrain (as
shown in Fig. 3.1), the mismatch between the rigid assumption and the soft
contact interaction can significantly affect the robot’s performance and
locomotion stability. This mismatch is due to the unmodeled contact dynamics
between the robot and the terrain. In fact, under the rigid ground assumption,
the controller can generate instantaneous changes to the Ground Reaction
Forces (GRFs). This is equivalent to thinking that the terrain will respond
with an infinite bandwidth.

In order to robustly traverse a wide variety of terrains of different
compliances, the WBC must become compliant contact consistent (c3). Namely,
the WBC should be terrain-aware. That said, a more general WBC approach should
be developed that can adapt online to the changes in the terrain compliance.

Figure 3.1: HyQ traversing multiple terrains of different compliances.

##### 3.1.1 Related Work:
Soft Terrain Adaptation for Legged Robots

Locomotion over soft terrain can be tackled either from a control or a
planning perspective. In the context of locomotion control, Henze et al. [29]
presented the first experimental attempt using a WBC over soft terrain. Their
WBC is based on the rigid ground assumption, but it allows for constraint
relaxation. This allowed the humanoid robot TORO to adapt to a compliant
surface. Their approach was further extended in [46] by dropping the rigid
contact assumption and using an energy-tank approach. Despite balancing on
compliant terrain, both approaches were only tested for one type of soft
terrain when the robot was standing still.

Similarly, other works explicitly adapt to soft terrain by incorporating
terrain knowledge (i.e., contact model) into their balancing controllers. For
example, Azad et al. [47] proposed a momentum based controller for balancing
on soft terrain by relying on a nonlinear soft contact model. Vasilopoulos et
al. [48] proposed a similar hopping controller that models the terrain using a
viscoplastic contact model. However, these approaches were only tested in
simulation and for monopods.

In the context of locomotion planning, Grandia et al. [49] indirectly adapted
to soft terrain by shaping the frequency of the cost function of their Model
Predictive Control (MPC) formulation. By penalizing high frequencies, they
generated optimal motion plans that respect the bandwidth limitations due to
soft terrain. This approach was tested over three types of terrain
compliances. However, it was not tested during transitions from one terrain to
another. This approach showed an improvement in the performance of the
quadruped robot in simulation and experiment. However, the authors did not
offer the possibility to change their tuning parameters online. Thus, they
were not able to adapt the locomotion strategy based on the compliance of the
terrain.

In contrast to the aforementioned work, other approaches relax the rigid
ground assumption (hard contact constraint) but not for soft terrain
adaptation purposes. For instance, Kim et al. [50] implemented an approach to
handle sudden changes in the rigid contact interaction. This approach relaxed
the hard contact assumption in their WBC formulation by penalizing the contact
interaction in the cost function rather than incorporating it as a hard
constraint. For computational purposes, Neunert et al. [51] and Doshi et al.
[52] proposed relaxing the rigid ground assumption. Neunert et al. used a soft
contact model in their nonlinear MPC formulation to provide smooth gradients
of the contact dynamics to be more efficiently solved by their gradient based
solver. The soft contact model did not have a physical meaning and the contact
parameters were empirically chosen. Doshi et al. proposed a similar approach
which incorporates a slack variable that expands the feasibility region of the
hard constraint.

Despite the improvement in performance of the legged robots over soft terrain
in the aforementioned works, none of them offered the possibility to adapt to
the terrain online. Most of the aforementioned works lack a general approach
that can deal with multiple terrain compliances or with transitions between
them. Perhaps, one noticeable work (to date) in online soft terrain adaptation
was proposed by Chang et al. [53]. In that work, an iterative soft terrain
adaptation approach was proposed. The approach relies on a non-parametric
contact model that is simultaneously updated alongside an optimization based
hopping controller. The approach was capable of iteratively learning the
terrain interaction and supplying that knowledge to the optimal controller.
However, because the learning module was exploiting Gaussian process
regression, which is computationally expensive, the approach did not reach
real-time performance and was only tested in simulation, for one leg, under
one experimental condition (one terrain).

##### 3.1.2 Related Work:
Contact Compliance Estimation in Robotics

For contact compliance estimation, we need to accurately model the contact
dynamics and estimate the contact parameters online. In contact modeling,
Alves et al. [54] presented a detailed overview of the types of parametric
soft contact models used in the literature. In compliance estimation,
Schindeler et al. [55] used a two stage polynomial identification approach to
estimate the parameters of the Hunt and Crossley’s (HC) contact model online.
Differently, Azad et al. [56] used a least square-based estimation algorithm
and compared multiple contact models (including the Kelvin-Voigt’s (KV) and
the HC models). Other approaches that are not based on soft contact models use
force observers [57] or neural networks [58]. These aforementioned approaches
in compliance estimation were designed for robotic manipulation tasks.

To date, the only work on compliance estimation in legged locomotion was the
one by Bosworth et al. [13]. The authors presented two online (in-situ)
approaches to estimate the ground properties (stiffness and friction). The
results were promising and the approaches were validated on a quadruped robot
while hopping over rigid and soft terrain. However, the estimated stiffness
showed a trend, but was not accurate; the lab measurements of the terrain
stiffness did not match the in-situ ones. Although the estimation algorithms
could be implemented online, the robot had to stop to perform the estimation.

Figure 3.2: An overview of the STANCE algorithm.

##### 3.1.3 Proposed Approach and Contribution

In this work, we propose an online soft terrain adaptation algorithm called:
Soft Terrain Adaptation aNd Compliance Estimation (STANCE). As shown in Fig.
3.2, STANCE consists of

  * •

A Compliant Contact Consistent Whole-Body Control (c3WBC) that is contact
consistent to any type of terrain given the terrain compliance. This is done
by extending the state-of-the-art WBC in [1], hereafter denoted as the
Standard Whole-Body Control (sWBC). In particular, c3WBC incorporates a soft
contact model into the WBC formulation.

  * •

A Terrain Compliance Estimator (TCE) which is an online learning algorithm
that provides the c3WBC with an estimate of the terrain compliance. It is
based on the same contact model that is incorporated in the c3WBC.

The main contribution of STANCE is that it can adapt to any type of terrain
(stiff or soft) online without pre-tuning. This is done by closing the loop of
the c3WBC with the TCE. To our knowledge, this is the first implementation of
such an approach in legged locomotion.

STANCE is meant to overcome the limitations of the aforementioned approaches
in soft terrain adaptation for legged robots. Compared to previous works on
WBC that tested their approach only during standing [29, 46], we test our
STANCE approach during locomotion. Compared to other approaches [47, 48] that
were tested on monopods in simulation, STANCE is implemented and tested in
experiment on Hydraulically actuated Quadruped (HyQ). Compared to previous
work on soft terrain adaptation [49], STANCE can adapt to soft terrain online
and was tested on multiple terrains with different compliances and with
transitions between them. Compared to [53], our TCE is computationally
inexpensive, which allows STANCE to run real-time in experiments and
simulations. Compared to the previous work done on compliance estimation, we
implemented our TCE on a legged robot which is, to the best of our knowledge,
the first experimental validation of this approach. Differently from [13], our
TCE approach could be implemented in parallel with any gait or task. We also
achieved a more accurate estimation of the terrain compliance compared to
[13].

As additional contributions, we discussed the benefits (and the limitations)
of exploiting the knowledge of the terrain in WBC based on the experience
gained during extensive experimental trials. To our knowledge, STANCE is the
first work to present legged locomotion experiments crossing multiple terrains
of different compliances.

#### 3.2 Robot model

Consider a legged robot with $n$ Degrees of Freedom (DoFs) and $c$ feet. The
total dimension of the feet operational space $n_{{f}}$ can be separated into
stance ($n_{\mathrm{st}}=3c_{\mathrm{st}}$) and swing feet
($n_{\mathrm{sw}}=3c_{\mathrm{sw}}$) where $c_{\mathrm{st}}$ and
$c_{\mathrm{sw}}$ are the number of stance and swing legs respectively.
Assuming that all external forces are exerted on the stance feet, the robot
dynamics is written as

$\displaystyle\underbrace{\begin{bmatrix}M_{\mathrm{com}}&0_{3\times
3}&0_{3\times n}\\\ 0_{3\times 3}&M_{\theta}&M_{\theta j}\\\ 0_{n\times
3}&M_{\theta
j}^{T}&M_{j}\end{bmatrix}}_{M(q)}\underbrace{\begin{bmatrix}\ddot{x}_{\mathrm{com}}\\\
\dot{\omega}_{b}\\\
\ddot{q}_{j}\end{bmatrix}}_{\ddot{q}}+\underbrace{\begin{bmatrix}h_{\mathrm{com}}\\\
h_{\theta}\\\ h_{j}\end{bmatrix}}_{h(q,\dot{q})}$
$\displaystyle=\begin{bmatrix}0_{3\times 1}\\\ 0_{3\times 1}\\\
\tau_{j}\end{bmatrix}+\underbrace{\begin{bmatrix}J_{\mathrm{st,com}}^{T}\\\
J_{\mathrm{st},\theta}^{T}\\\
J_{\mathrm{st},j}^{T}\end{bmatrix}}_{J_{\mathrm{st}}(q)^{T}}F_{\mathrm{grf}}$
(3.1)

where $q~{}\in~{}SE(3)~{}\times~{}\mathbb{R}^{n}$ denotes the generalized
robot states consisting of the Center of Mass (CoM) position
$x_{\mathrm{com}}$ $\in~{}\mathbb{R}^{3}$, the base orientation $R_{b}$
$\in~{}SO(3)$, and the joint positions $q_{j}$ $\in~{}\mathbb{R}^{n}$. The
vector
$\dot{q}~{}=~{}[\dot{x}_{\mathrm{com}}^{T}~{}\omega_{b}^{T}~{}\dot{q}_{j}^{T}]^{T}~{}\in~{}\mathbb{R}^{6+n}$
denotes the generalized velocities consisting of the velocity of the CoM
$\dot{x}_{\mathrm{com}}$ $\in~{}\mathbb{R}^{3}$, the angular velocity of the
base $\omega_{b}$ $\in~{}\mathbb{R}^{3}$, and the joint velocities
$\dot{q}_{j}$ $\in~{}\mathbb{R}^{n}$. The vector
$\ddot{q}~{}=~{}[\ddot{x}_{\mathrm{com}}^{T}~{}\dot{\omega}_{b}^{T}~{}\ddot{q}_{j}^{T}]^{T}~{}\in~{}\mathbb{R}^{6+n}$
denotes the corresponding generalized accelerations. All Cartesian vectors are
expressed in the world frame $\Psi_{W}$ unless mentioned otherwise.
$M~{}\in~{}\mathbb{R}^{(6+n)\times(6+n)}$ is the inertia matrix.
$h\in\mathbb{R}^{6+n}$ is the force vector that accounts for Coriolis,
centrifugal, and gravitational forces. $\tau_{j}\in\mathbb{R}^{n}$ are the
actuated joint torques, $F_{\mathrm{grf}}\in\mathbb{R}^{n_{\mathrm{st}}}$ is
the vector of GRFs (contact forces). The Jacobian matrix $J$
$\in\mathbb{R}^{n_{\mathrm{f}}\times(6+n)}$ is separated into swing Jacobian
$J_{\mathrm{sw}}$ $\in\mathbb{R}^{n_{\mathrm{sw}}~{}\times~{}(6+n)}$ and
stance Jacobian
$J_{\mathrm{st}}~{}\in~{}\mathbb{R}^{n_{\mathrm{st}}~{}\times~{}(6+n)}$ which
can be further expanded into $J_{\mathrm{st,com}}$
$\in\mathbb{R}^{n_{\mathrm{st}}\times 3}$, $J_{\mathrm{st,}\theta}$
$\in\mathbb{R}^{n_{\mathrm{st}}\times 3}$, and $J_{\mathrm{st},j}$
$\in\mathbb{R}^{n_{\mathrm{st}}\times n}$. The feet velocities
$v~{}\in\mathbb{R}^{n_{\mathrm{f}}}$ are separated into stance
$v_{\mathrm{st}}~{}\in~{}\mathbb{R}^{n_{\mathrm{st}}}$ and swing
$v_{\mathrm{sw}}~{}\in~{}\mathbb{R}^{n_{\mathrm{sw}}}$ feet velocities.
Similarly, the feet accelerations $\dot{v}~{}\in\mathbb{R}^{n_{\mathrm{f}}}$
are separated into stance
$\dot{v}_{\mathrm{st}}~{}\in~{}\mathbb{R}^{n_{\mathrm{st}}}$ and swing
$\dot{v}_{\mathrm{sw}}~{}\in~{}\mathbb{R}^{n_{\mathrm{sw}}}$ feet
accelerations. The feet forces $F~{}=~{}[F_{\mathrm{st}}^{T}\
F_{\mathrm{sw}}^{T}]^{T}~{}\in~{}\mathbb{R}^{n_{\mathrm{f}}}$ are also
separated into stance $F_{\mathrm{st}}~{}\in~{}\mathbb{R}^{n_{\mathrm{st}}}$
and swing $F_{\mathrm{sw}}~{}\in\mathbb{R}^{n_{\mathrm{sw}}}$ feet forces. We
split the robot dynamics (3.1) into an unactuated floating base part (the
first 6 rows) and an actuated part (the remaining $n$ rows) as

$\displaystyle M_{u}(q)\ddot{q}+h_{u}(q,\dot{q})$ $\displaystyle=$
$\displaystyle J_{\mathrm{st},u}(q)^{T}F_{\mathrm{grf}}$ (3.2a) $\displaystyle
M_{a}(q)\ddot{q}+h_{j}(q,\dot{q})$ $\displaystyle=$
$\displaystyle\tau_{j}+J_{\mathrm{st},j}(q)^{T}F_{\mathrm{grf}}$ (3.2b)

where $M_{u}$ $\in~{}\mathbb{R}^{6\times 6+n}$ and $M_{a}$
$\in~{}\mathbb{R}^{n\times 6+n}$ are sub matrices of $M$,
$h_{u}=[h_{\mathrm{com}}^{T}~{}h_{\theta}^{T}]^{T}$ $\in~{}\mathbb{R}^{6}$ and
$h_{j}$ $\in~{}\mathbb{R}^{n}$ are sub vectors of $h$, and
$J_{\mathrm{st},u}=[J_{\mathrm{st,com}}^{T}~{}J_{\mathrm{st},\theta}^{T}]^{T}$.
Finally, we define the gravito-inertial wrench as
$W_{\mathrm{com}}~{}=~{}M_{u}(q)\ddot{q}+h_{u}(q,\dot{q})$
$\in\mathbb{R}^{6}$.

Figure 3.3: Overview of the WBC in our locomotion framework. The dashed black
box presents the definition of the HyQ’s legs (LF, RF, LH and RH) and the
generated wrenches. The solid orange box presents the terminologies used for
the soft contact model. $\Psi_{\mathrm{W}}$ is the world frame and
$\Psi_{\mathrm{C}_{i}}$ is the local contact frame for the leg $i$ fixed at
the touch down position.

#### 3.3 Standard Whole-Body Controller (sWBC)

This section summarizes the sWBC as detailed in [1]. Besides the WBC, our
locomotion framework includes a locomotion planner, state estimator and a low-
level torque controller as shown in Fig. 3.3. Given high-level user inputs,
the planner generates the desired trajectories for the CoM, trunk orientation
and swing legs, and provides them to the WBC. The state estimation provides
the WBC with the estimated states of the robot.

The objective of the sWBC is to ensure the execution of the trajectories
provided by the planner while keeping the robot balanced and reasoning about
the robot’s dynamics, actuation limits and the contact constraints [1]. We
denote the execution of the trajectories provided by the planner as control
tasks. These control tasks alongside the aforementioned constraints define the
WBC problem. The control problem is casted as a Whole-Body Optimization
(WBOpt) problem via a Quadratic Program (QP) which solves for the optimal
generalized accelerations and contact forces at each iteration of the control
loop [27]. The optimal solution of the WBC is then mapped into joint torques
that are sent to the low-level torque controller.

##### 3.3.1 Control Tasks

We categorize the sWBC control tasks into: 1) a trunk task that tracks the
desired trajectories of the CoM position and trunk orientation, and 2) a swing
task that tracks the swing feet trajectories [1]. Similar to a PD+ controller
[39], both tasks are achieved by a Cartesian-based impedance controller with a
feed-forward term. The feedforward terms are added in order to improve the
tracking performance of the tasks when following the trajectories from the
planner [29, 34]. The tracking of the trunk task is obtained by the desired
wrench at the CoM $W_{\mathrm{com,d}}\in\mathbb{R}^{6}$. This is generated by
a Cartesian impedance at the CoM, a gravity compensation term, and a feed-
forward term. Similarly, the tracking of the swing task can be obtained by the
virtual force $F_{\mathrm{sw,d}}\in\mathbb{R}^{n_{\mathrm{sw}}}$. This is
generated by a Cartesian impedance at the swing foot and a feed-forward term.
As in [1], we can also write the swing task at the acceleration level by
defining the desired swing feet velocities $\dot{v}_{\mathrm{sw,d}}$
$\in\mathbb{R}^{n_{\mathrm{sw}}}$ as

$\displaystyle\dot{v}_{\mathrm{sw,d}}=\dot{v}_{\mathrm{sw,ff}}+\mathrm{K}_{\mathrm{sw}}\Delta
x_{\mathrm{sw}}+\mathrm{D}_{\mathrm{sw}}\Delta v_{\mathrm{sw}}$ (3.3)

where
$\mathrm{K}_{\mathrm{sw}},\mathrm{D}_{\mathrm{sw}}\in\mathbb{R}^{n_{\mathrm{sw}}\times
n_{\mathrm{sw}}}$ are positive definite PD gains, $\Delta
x_{\mathrm{sw}}=x_{\mathrm{sw,d}}-x_{\mathrm{sw}}$
$\in\mathbb{R}^{n_{\mathrm{sw}}}$ and $\Delta
v_{\mathrm{sw}}=v_{\mathrm{sw,d}}-v_{\mathrm{sw}}$
$\in\mathbb{R}^{n_{\mathrm{sw}}}$ are tracking errors of the swing foot
position and velocity, respectively, and $\dot{v}_{\mathrm{sw,ff}}$ is a feed-
forward term.

##### 3.3.2 Whole-Body Optimization

To accomplish the sWBC objective (the control tasks in Section 3.3.1 and
constraints), we formulate the WBOpt problem presented in Algorithm 1 and
detailed in [1].

###### 3.3.2.1 Decision Variables

As shown in Algorithm 1, we choose the generalized accelerations $\ddot{q}$
and the contact forces $F_{\mathrm{grf}}$ as the decision variables
$u~{}=~{}[\ddot{q}^{T}~{}F_{\mathrm{grf}}^{T}]^{T}~{}\in~{}\mathbb{R}^{6+n+n_{\mathrm{st}}}$.
Later in this subsection, we will augment the vector of decision variables
with a slack term $\eta\in\mathbb{R}^{n_{\mathrm{sw}}}$.

###### 3.3.2.2 Cost

The cost function (3.5) consists of two terms. The first term ensures the
tracking of the trunk task by minimizing the two-norm of the tracking error
between the actual $W_{\mathrm{com}}$ and desired $W_{\mathrm{com,d}}$ CoM
wrenches. The second term in (3.5) regularizes the solution and penalizes the
slack variable.

###### 3.3.2.3 Physical Consistency

The equality constraint (3.6) enforces the physical consistency between
$F_{\mathrm{grf}}$ and $\ddot{q}$ by ensuring that the contact wrenches due to
$F_{\mathrm{grf}}$ will sum up to $W_{\mathrm{com}}$. This is done by imposing
the unactuated dynamics (3.2a) as an equality constraint.

###### 3.3.2.4 Stance Task

To remain contact consistent, we incorporate the stance task that enforces the
stance legs to remain in contact with the terrain. Since the sWBC is assuming
a rigid terrain, the stance feet are forced to remain stationary in the world
frame, i.e., $v_{\mathrm{st}}=\dot{v}_{\mathrm{st}}=0$ (see [1]). As a result,
we incorporate the rigid contact model in the sWBC formulation as an equality
constraint at the acceleration level (3.7) in order to have a direct
dependency on the decision variables. In detail, since
$v_{\mathrm{st}}=J_{\mathrm{st}}\dot{q}=0$, differentiating once with respect
to time yields
$\dot{v}_{\mathrm{st}}=J_{\mathrm{st}}\ddot{q}+\dot{J}_{\mathrm{st}}\dot{q}=0$.

###### 3.3.2.5 Friction and Normal Contact Force

The inequality constraint (3.11) enforces the friction constraints by ensuring
that the contact forces lie inside the friction cones. This is done by
limiting the tangential component of the GRFs $F_{\mathrm{grf,\parallel}}$.
The inequality constraint (3.12) enforces constraints on the normal component
of the GRFs $F_{\mathrm{grf,\perp}}$. This includes the unilaterality
constraints which encodes that the legs can only push on the ground by setting
an “almost-zero” lower bound $F_{\mathrm{\min}}$ to $F_{\mathrm{grf,\perp}}$.
They also allow a smooth loading/unloading of the legs, and set a varying
upper bound $F_{\mathrm{\max}}$ to $F_{\mathrm{grf,\perp}}$. For the detailed
implementation of the inequality constraints (3.11) and (3.12), refer to [34].

###### 3.3.2.6 Swing Task

We implement the tracking of the swing task (in Section 3.3.1) at the
acceleration level (3.3) rather than the force level since we can express the
swing feet velocities $\dot{v}_{\mathrm{sw}}$ as a function of $\ddot{q}$
which is a decision variable, i.e.,
$\dot{v}_{\mathrm{sw}}(q)=J_{\mathrm{sw}}\ddot{q}+\dot{J}_{\mathrm{sw}}\dot{q}$.
This task could be encoded as an equality constraint
$\dot{v}_{\mathrm{sw}}=\dot{v}_{\mathrm{sw,d}}$. Yet, it is important to relax
this hard constraint when the joint kinematic limits are reached (see [1]).
Hence, the swing task is encoded in (3.13) by an inequality constraint that
bounds the solution around the original hard constraint and a slack term
$\eta$ that is penalized for its non-zero values in the cost function (3.5)
and is constrained to remain non-negative in (3.13).

###### 3.3.2.7 Torque and Joint Limits

The torque and joint limits are enforced in the inequality constraints (3.14)
and (3.15), respectively.

###### 3.3.2.8 Torque Mapping

The WBOpt (3.5)-(3.7), (3.11)-(3.15) generates optimal joint accelerations
$\ddot{q}_{j}^{*}$ and contact forces $F_{\mathrm{grf}}^{*}$, that are mapped
into optimal joint torques $\tau^{*}$ and sent to the low-level controller
using the actuated dynamics (3.2b) as

$\tau^{*}=M_{a}\ddot{q}^{*}+h_{j}-J_{\mathrm{st},j}^{T}F_{\mathrm{grf}}^{*}$
(3.4)

Algorithm 1 Whole-Body Optimization: sWBC Vs. c3WBC (Trunk Task) (Decision
Variables) (Physical Consistency) ~~(Stance Task)~~ (c3-Stance Task)
(Friction) (Normal Contact Force) (Swing Task) (Torque Limits) (Joint Limits)
$\displaystyle\underset{u}{\text{min~{}}}\|W_{\mathrm{com}}-W_{\mathrm{com,d}}\|^{2}_{Q}+{\color[rgb]{0,0.5,1}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.5,1}\pgfsys@color@cmyk@stroke{1}{0.50}{0}{0}\pgfsys@color<EMAIL_ADDRESS>(3.5) $\displaystyle u=[\ddot{q}^{T}\ F_{\mathrm{grf}}^{T}\ \eta^{T}\
{\color[rgb]{0,0.5,1}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.5,1}\pgfsys@color@cmyk@stroke{1}{0.50}{0}{0}\pgfsys@color<EMAIL_ADDRESS>s.t.: $\displaystyle
M_{u}\ddot{q}+h_{u}=J^{T}_{\mathrm{st},u}F_{\mathrm{grf}}$ (3.6) (3.7)
$\displaystyle{\color[rgb]{0,0.5,1}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.5,1}\pgfsys@color@cmyk@stroke{1}{0.50}{0}{0}\pgfsys@color<EMAIL_ADDRESS>(3.8)
$\displaystyle{\color[rgb]{0,0.5,1}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.5,1}\pgfsys@color@cmyk@stroke{1}{0.50}{0}{0}\pgfsys@color<EMAIL_ADDRESS>(3.9)
$\displaystyle{\color[rgb]{0,0.5,1}\definecolor[named]{pgfstrokecolor}{rgb}{0,0.5,1}\pgfsys@color@cmyk@stroke{1}{0.50}{0}{0}\pgfsys@color<EMAIL_ADDRESS>0}}$ (3.10)
$\displaystyle|F_{\mathrm{grf,\parallel}}|\leq\mu|F_{\mathrm{grf,\perp}}|$
(3.11) $\displaystyle F_{\mathrm{\min}}\leq F_{\mathrm{grf,\perp}}\leq
F_{\mathrm{\max}}$ (3.12)
$\displaystyle-\eta\leq\dot{v}_{\mathrm{sw}}-\dot{v}_{{\mathrm{sw,d}}}\leq\eta,~{}\eta\geq
0$ (3.13) $\displaystyle\tau_{\min}\leq\tau_{j}\leq\tau_{\max}$ (3.14)
$\displaystyle\ddot{q}_{j_{\min}}\leq\ddot{q}_{j}\leq\ddot{q}_{j_{\max}}$
(3.15)

##### 3.3.3 Feedback Control

The computation of the optimal torques $\tau^{*}$ relies on the inverse
dynamics in (3.4) which might be prone to model inaccuracies [59]. In order to
tackle this issue, the desired torques $\tau_{d}$ sent to the lower level
control could combine the optimal torques $\tau^{*}$ in (3.4) with a feedback
controller $\tau_{\mathrm{fb}}$ as shown in Fig. 3.3. The feedback controller
improves the tracking performance if the dynamic model of the robot becomes
less accurate [59]. The feedback controller is a proportional-derivative (PD)
joint space impedance controller [34].

###### Remark 3.1

Throughout this work and similar to [1] and [27], we found it sufficient to
use only the inverse-dynamics term (the optimal torques $\tau^{*}$) and not
the joint feedback part. This is due to the fact that we can identify the
parameters of our dynamic model with sufficient accuracy as detailed in [60].
That said, we carried out the simulation and experiment without any need of
the feedback loop.

#### 3.4 C3 Whole-Body Controller

Over soft terrain, the feet positions are non-stationary and are allowed to
deform the terrain. Thus, the rigid contact assumption of the stance task
(3.7) in the sWBC does not hold anymore and should be dropped. To be c3, the
interaction between the stance feet and the soft terrain must be governed not
just by the robot dynamics but also by the soft contact dynamics. That said,
the c3WBC extends the sWBC by: 1) modeling the soft contact dynamics and
incorporating it as a stance task similar to the control tasks in Section
3.3.1, and 2) encoding the stance task in the WBOpt as a function of the
decision variables. The differences between the sWBC and the c3WBC are
highlighted in boldface in Algorithm 1.

###### Remark 3.2

The term contact consistent is a well-established term in the literature that
was initially introduced in [61]. It implies formulating the control structure
to account for the contact with the environment. The term c3 is an extension
of the term contact consistent. Hence, c3 implies formulating the control
structure to account for the compliant contact with the environment.

##### 3.4.1 c3-Stance Task

We model the soft contact dynamics with a simple explicit model (the KV
model). This consists of 3D linear springs and dampers normal and tangential
to the contact point [51]. The normal direction of this impedance emulates the
normal terrain deformation while the tangential ones emulate the shear
deformation. Although several models that accurately emulate contact dynamics
are available [62, 63, 54], we implemented the KV model for several reasons.
First, since the model is linear in the parameters, it fits our QP
formulation. Second, estimating the parameters of the KV model is
computationally efficient. As a result, using this model, we can run a
learning algorithm online which would be challenging if a model similar to
[53] is used. For a legged robot with point-like feet, for each stance leg
$i$, we formulate the contact model in the world frame as

$\displaystyle
F_{\mathrm{grf,i}}=k_{\mathrm{st},i}p_{i}+d_{\mathrm{st},i}\dot{p}_{i}$ (3.16)

where $k_{\mathrm{st},i}\in\mathbb{R}^{3\times 3}$,
$d_{\mathrm{st},i}\in\mathbb{R}^{3\times 3}$,
$F_{\mathrm{grf,i}}\in\mathbb{R}^{3}$, $p_{i}\in\mathbb{R}^{3}$, and
$\dot{p}_{i}\in\mathbb{R}^{3}$ are the terrain stiffness, the terrain damping,
the GRFs, the penetration and the penetration rate of the $i$-th stance leg,
all expressed in the world frame, respectively (see Fig. 3.3). We define
$p_{i}$ and $\dot{p}_{i}$ as

$\displaystyle p_{i}=x_{\mathrm{td},i}-x_{\mathrm{st},i}\text{,}\hskip
50.00008pt\dot{p}_{i}=0-v_{\mathrm{st},i}$ (3.17)

where $x_{\mathrm{td},i}\in\mathbb{R}^{3}$ denotes the position of the contact
point of foot $i$ at the touchdown in the world frame. By appending all of the
stance feet, the contact model can be re-written in a compact form as

$F_{\mathrm{grf}}=K_{\mathrm{st}}p+D_{\mathrm{st}}\dot{p}=K_{\mathrm{st}}(x_{\mathrm{td}}-x_{\mathrm{st}})-D_{\mathrm{st}}v_{st}$
(3.18)

where $K_{\mathrm{st}}$ $\in R^{n_{\mathrm{st}}\times n_{\mathrm{st}}}$ and
$D_{\mathrm{st}}$ $\in R^{n_{\mathrm{st}}\times n_{\mathrm{st}}}$ are the
block-diagonal stiffness and damping matrices of the terrain of all the stance
feet, respectively, and $x_{\mathrm{td}}$ $\in\mathbb{R}^{n_{st}}$ are the
touchdown positions of all the stance feet.

Similar to Section 3.3.1, we deal with the contact model (3.18) as another WBC
task (alongside the trunk and swing (3.3) tasks). We can think of (3.18) as a
desired stance task that keeps the WBC c3. This stance task is achieved by a
Cartesian impedance at the stance foot which is represented by the impedance
of the terrain ($K_{\mathrm{st}}$ and $D_{\mathrm{st}}$). This similarity
makes us encode the contact model in the WBOpt as a stance constraint similar
to what we did for the swing task in Section 3.3.2. Hereafter, we refer to
this stance task as the c3-stance task (see Fig. 3.3).

##### 3.4.2 Whole-Body Optimization Revisited

The c3-stance task is included in the WBOpt by writing the soft contact model
(3.18) as a function of the decision variables. Ideally, we can directly
reformulate (3.18) as a function of $F_{\mathrm{grf}}$ and
$\dot{v}_{\mathrm{st}}$. Indeed, $\dot{v}_{\mathrm{st}}$ can be expressed as a
function of the joint accelerations $\ddot{q}$ which is a decision variable
(as explained in [1]). By numerically integrating $\dot{v}_{\mathrm{st}}$
(once to obtain $v_{\mathrm{st}}$ and twice to obtain $x_{\mathrm{st}}$), we
can associate $F_{\mathrm{grf}}$ with $\dot{v}_{\mathrm{st}}$. This approach
requires the knowledge of $x_{\mathrm{td}}$ to compute $p$ which might be
prone to estimation errors and it requires a reset of the integrator at every
touchdown.

We choose a more convenient approach which is to add the desired foot
penetration $\epsilon$ as an extra decision variable in the WBOpt formulation.
The difference between $p$ and $\epsilon$ is that $p$ is the actual
penetration due to the interaction with the soft contact while $\epsilon$ is
the desired penetration in the world frame generated from the optimization
problem. Both variables imply the same physical phenomenon (the soft contact
deformation). That said, we can rewrite $F_{\mathrm{grf}}$ in (3.18) as a
function of $\epsilon$ and $\dot{\epsilon}$ (by numerically differentiating
$\epsilon$) without the previous knowledge of $x_{\mathrm{td}}$, which is
advantageous.

To do so, $\epsilon$ is appended to the vector of decision variables $u$ and
regularized in (3.5). Then, we incorporate (3.18) directly as a function of
$F_{\mathrm{grf}}$ and $\epsilon$ as in the equality constraint (3.8). We
numerically differentiate $\epsilon$ to obtain
$\dot{\epsilon}_{k}=\frac{\epsilon_{k}-\epsilon_{k-1}}{\Delta t}$. To maintain
physical consistency, we need to enforce an additional constraint between the
desired penetration $\epsilon$ and the contact acceleration
($\ddot{\epsilon}=-\dot{v}_{\mathrm{st}}$). This is encoded as an equality
constraint as shown in (3.9). To do so, we numerically differentiate
$\epsilon$ twice to obtain
$\ddot{\epsilon}_{k}=\frac{\epsilon_{k}-2\epsilon_{k-1}+\epsilon_{k-2}}{\Delta
t^{2}}$. We also ensure the consistency of the physical contact model
throughout the optimization problem by ensuring that the penetration is always
positive in (3.10).

We also consider the loading and unloading phase, explained in [1] and [34],
to be terrain-aware. We tune the loading and unloading phase period $T_{l/u}$
for each leg to follow the settling time of a second order system response
that is a function of the terrain compliance and the robot’s mass [64]. Hence,
$T_{l/u}$ is

$T_{l/u}=4.6/\sqrt{\frac{k_{\mathrm{st,i}}}{m_{e}}}$ (3.19)

where $m_{e}$ is the equivalent mass felt at the robot’s feet (i.e., the
weight of the robot $m_{R}$ spread across its stance feet
$m_{e}~{}=~{}m_{R}/n_{\mathrm{st}}$) and the constant in the numerator
represents a $1\%$ steady-state error.

Finally, the WBOpt (3.5), (3.6), (3.8)-(3.15) generates optimal joint
accelerations $\ddot{q}_{j}^{*}$ and contact forces $F_{\mathrm{grf}}^{*}$,
that are mapped into optimal joint torques $\tau^{*}$ and sent to the low-
level controller using the actuated part of the robot’s dynamics as shown in
(3.4). Note that similar to the sWBC, we found it sufficient to use only the
inverse-dynamics term (the optimal torques $\tau^{*}$) and not the joint
feedback part.

As explained above, adding $\epsilon$ as a decision variable involved adding
two constraints in the optimization which increases the problem size and the
computation time. Yet, we are still able to run the c3WBC in real-time. The
advantage of our approach is that the knowledge of the touchdown position
$x_{\mathrm{td}}$ is not required. We only need the previous two time
instances of the penetration $\epsilon_{k-1}$ and $\epsilon_{k-2}$ that we
already computed in the previous control loops.

#### 3.5 Terrain Compliance Estimation

Figure 3.4: Overview of the TCE’s architecture inside the state estimator.

The purpose of the TCE is to estimate online the terrain parameters (namely
$K_{\mathrm{st}}$ and $D_{\mathrm{st}}$) based on the states of the robot. It
is a stand-alone algorithm that is decoupled from the c3WBC. The TCE uses the
contact model (3.18). Based on that, the current measurement of the contact
states (contact status $\alpha$, GRFs $F_{\mathrm{grf}}$, the penetration $p$,
and the penetration rate $\dot{p}$) of each leg $i$ at every time step are
required. Given the contact states, we use supervised learning to learn the
terrain parameters. As shown in Fig. 3.4, the TCE consists of two main
modules: contact state estimation (Section 3.5.1) and supervised learning
(Section 3.5.2). The contact state estimation module estimates the contact
states and provides it to the supervised learning module that collects these
data and computes the estimates of the terrain parameters.

##### 3.5.1 Contact State Estimation

The contact states are estimated solely from the current states of the robot
by the state estimator. The GRFs are estimated from the torques and the joint
states, and the penetration and its rate are estimated from the floating base
(trunk) states and the joint states.

###### 3.5.1.1 GRFs Estimation

To estimate the GRF, we use actuated part of the dynamics in (3.2b) as

$F_{\mathrm{grf},i}=\alpha_{i}J_{j,i}^{-T}(M_{a,i}\ddot{q}_{i}+h_{j,i}-\tau_{j,i})$
(3.20)

where $F_{\mathrm{grf},i}$, $J_{j,i}$, $M_{a,i}$, $\ddot{q}_{i}$, $h_{j,i}$,
and $\tau_{j,i}$ correspond to $F_{\mathrm{grf}}$, $J_{\mathrm{st},j}$,
$M_{a}$, $\ddot{q}$, $h_{j}$, and $\tau_{j}$ for the $i$-th leg, respectively.
Additionally, $\alpha_{i}$ is the contact status variable that detects if
there is a contact in the $i$-th leg or not. The contact is detected when the
GRFs exceed a certain threshold $F_{\min}$. Hence, $\alpha_{i}$ computed along
the normal direction of the $i$-th leg $n_{i}$ as:

$\alpha_{i}=\begin{cases}1,&\text{if
$n_{i}^{T}(J_{j,i}^{-T}(M_{a,i}\ddot{q}_{i}+h_{j,i}-\tau_{j,i})\geq
F_{\min}$}\\\ 0,&\text{otherwise}\end{cases}$ (3.21)

###### 3.5.1.2 Penetration Estimation

As shown in (3.17), we estimate the penetration and its rate using the stance
feet positions $x_{\mathrm{st},i}$ and velocities $v_{\mathrm{st},i}$, and the
touchdown position $x_{\mathrm{td},i}$ all in the world frame. To estimate the
feet states in the world frame, we use the forward kinematics and the base
state in the world frame. Thus, the penetration and its rate are written as

$\displaystyle p_{i}$ $\displaystyle=$ $\displaystyle
x_{\mathrm{td},i}-x_{\mathrm{st},i}=x_{\mathrm{td},i}-x_{b}-R_{B}^{W}x_{\mathrm{st},i}^{B}$
(3.22) $\displaystyle\dot{p}_{i}$ $\displaystyle=$ $\displaystyle-
v_{\mathrm{st},i}=-v_{b}-R_{B}^{W}v_{\mathrm{st},i}^{B}-(\omega_{b}\times
R_{B}^{W})x_{\mathrm{st},i}^{B}$ (3.23)

where $x_{b}$ $\in\mathbb{R}^{3}$ and $v_{b}$ $\in\mathbb{R}^{3}$ are the base
position and velocity in the world frame, respectively. The terms
$x_{\mathrm{st},i}^{B}$ and $v_{\mathrm{st},i}^{B}$ are the stance feet
position and velocity of the $i$-th leg in the base frame, respectively. The
terms $R_{B}^{W}$ $\in SO(3)$ and $\omega_{b}$ are the rotation matrix mapping
vectors from the base frame to the world frame and the base angular velocity,
respectively. The touch down positions are obtained using a height map.

###### 3.5.1.3 Contact States Mapping

Since, the KV model consists of 3D linear springs and dampers, normal and
tangential to the contact point, this makes the stiffness and damping matrices
diagonal with respect to the contact frame. However, if expressed in the world
frame, the stiffness and damping matrices become dense. Thus, if we formulate
the KV model in the contact frame rather than the world frame, we estimate
less number of elements per matrix per leg: three elements instead of nine.
Henceforth, the KV model in the TCE should be formulated with respect to the
contact frame rather than the world frame to reduce the computational
complexity. To do so, the GRFs (3.20), the penetration (3.22) and its rate
(3.23) of the $i$-th leg are transformed from the world frame $\Psi_{W}$ to
the contact frame $\Psi_{C_{i}}$ as (see Fig. 3.3)

$\displaystyle F_{\mathrm{grf},i}^{C}$ $\displaystyle=$ $\displaystyle
R_{W}^{C_{i}}F_{\mathrm{grf,i}}$ (3.24) $\displaystyle p_{i}^{C}$
$\displaystyle=$ $\displaystyle R_{W}^{C_{i}}p_{i}$ (3.25)
$\displaystyle\dot{p}_{i}^{C}$ $\displaystyle=$ $\displaystyle
R_{W}^{C_{i}}\dot{p}_{i}$ (3.26)

where the superscript $\bullet^{C}$ refers to the contact frame and
$R_{W}^{C_{i}}$ is the rotation matrix mapping from the world $\Psi_{W}$ to
the contact $\Psi_{C_{i}}$ frames for the $i$-th leg. Note that the
transformation (3.26) is linear since the contact frame is fixed with respect
to the world frame at the touch down position (i.e., $\dot{R}_{W}^{C_{i}}=0$).

##### 3.5.2 Supervised Learning

Considering the contact model in the contact frame and using the estimated
contact states (3.24)-(3.26), we learn the terrain parameters online via
supervised learning. In particular, we use weighted linear least squared
regression. The algorithm is treated as a batch algorithm with $m$-examples
such that, at every time instant $k$, we gather samples from the previous $m$
time instances and compute the terrain parameters [65].

For the $k$-th time instant, of the $i$-th leg in the $d\text{-th}$ direction
($d\in\\{n_{i},t_{1,i},t_{2,i}\\}$, see Fig. 3.3), the terms
$F_{\mathrm{grf},i}^{C,d}(k)$, $p_{i}^{C,d}(k)$, and $\dot{p}_{i}^{C,d}(k)$
are estimated as shown in Section 3.5.1 where $\bullet_{i}^{d}(k)$ refers to
the $k$-th time instance of the $i$-th leg in the $d$-th direction. That said,
we construct the following objects (buffers) with size $m$

$\displaystyle\mathcal{F}_{i}^{d}$ $\displaystyle=$
$\displaystyle\begin{bmatrix}F_{\mathrm{grf},i}^{C,d}(k)&\cdots&F_{\mathrm{grf},i}^{C,d}(k-m)\end{bmatrix}^{T}$
(3.27) $\displaystyle\mathrm{P}_{i}^{d}$ $\displaystyle=$
$\displaystyle\begin{bmatrix}p_{i}^{C,d}(k)&\cdots&p_{i}^{C,d}(k-m)\end{bmatrix}^{T}$
(3.28) $\displaystyle\dot{\mathrm{P}}_{i}^{d}$ $\displaystyle=$
$\displaystyle\begin{bmatrix}\dot{p}_{i}^{C,d}(k)&\cdots&\dot{p}_{i}^{C,d}(k-m)\end{bmatrix}^{T}$
(3.29) $\displaystyle\mathcal{P}_{i}^{d}$ $\displaystyle=$
$\displaystyle\begin{bmatrix}\mathrm{P}_{i}^{d}&\dot{\mathrm{P}}_{i}^{d}\end{bmatrix}$
(3.30)

where $\mathcal{F}_{i}^{d}\in\mathbb{R}^{m}$ is the GRFs buffer and
$\mathcal{P}_{i}^{d}\in\mathbb{R}^{m\times 2}$ is the penetration, and
penetration rate buffer. Given $\mathcal{F}_{i}^{d}$ and $\mathcal{P}_{i}^{d}$
as inputs and outputs of the learning algorithm respectively, we estimate the
terrain impedance parameters as
$I_{i}^{d}=\begin{bmatrix}k_{\mathrm{st},i}^{C,d}~{}d_{\mathrm{st},i}^{C,d}\end{bmatrix}^{T}\in\mathbb{R}^{2}$
using the analytical solution

$I_{i}^{d}=(\mathcal{P}_{i}^{dT}W\mathcal{P}_{i}^{d})^{-1}\mathcal{P}_{i}^{dT}W\mathcal{F}_{i}^{d}$
(3.31)

where $k_{\mathrm{st},i}^{C,d}\in\mathbb{R}$ and
$d_{\mathrm{st},i}^{C,d}\in\mathbb{R}$ are the terrain stiffness and damping
parameters expressed in the contact frame. The matrix $W\in\mathbb{R}^{m\times
m}$ is a weighting matrix used to penalize the error on most recent sample
compared to the less recent ones and thus, giving more importance to the most
recent samples.

All of the legs in the learning algorithm are decoupled. We found it
advantageous to treat each leg separately because the robot can be standing on
a different terrain at each foot.

##### 3.5.3 Implementation Details

Algorithm 2 sketches the entire TCE process. To initialize the buffers, we
acquire samples when the robot is at full stance and return the first estimate
of $I_{i}^{d}$ once the buffers are full. After initialization, we acquire
samples and update the buffers only when the leg is at stance.

The buffers are continuously updated in a sliding window fashion. When a leg
finishes the swing phase and is at a new touch down, it continues to use the
previous samples from the previous stance phase. This is advantageous since it
gives a smooth transition between terrains, but it adds a delay.

###### Remark 3.3

Since the c3WBC formulation is based in the world frame, it is essential to
map the estimated stiffness and damping matrices back to the world frame
before providing them to the c3WBC (see Fig. 3.4).

###### Remark 3.4

The TCE can be used with any arbitrary terrain geometry given the terrain
normal and thus $R_{W}^{C_{i}}$. The terrain normal $n_{i}$ at the contact
point $i$ can be provided by a height map that is generated via an RGBD
sensor.

Algorithm 2 Terrain Compliance Estimation

1:initialize the buffers ($\mathcal{F}_{i}^{d}$ and $\mathcal{P}_{i}^{d}$) and
$I_{i}^{d}$

2:for each iteration $k$ do

3: for each leg $i$ do

4: if leg is in contact ($\alpha_{i}==1$) then(3.21)

5: for each direction $d$ do

6: estimate $F_{\mathrm{grf,i}}^{d}(k)$ (3.20)

7: estimate $p_{i}^{d}(k)$ (3.22)

8: estimate $\dot{p}_{i}^{d}(k)$ (3.23)

9: transform $F_{\mathrm{grf,i}}^{d}(k)$ into $F_{\mathrm{grf},i}^{C,d}(k)$
(3.24)

10: transform $p_{i}^{d}(k)$ into $p_{i}^{C,d}(k)$ (3.25)

11: transform $\dot{p}_{i}^{d}(k)$ into $\dot{p}_{i}^{C,d}(k)$ (3.26)

12: update buffers $\mathcal{F}_{i}^{d}$ and $\mathcal{P}_{i}^{d}$
(3.27)-(3.30)

13: solve for $I_{i}^{d}$ (3.31)

14: end for

15: map the estimated parameters to $\Psi_{W}$

16: end if

17: end for

18:end for

#### 3.6 Experimental Setup

##### 3.6.1 State Estimation

We implemented our approach on HyQ [66] which is equipped with a variety of
sensors. Each leg contains two load-cells, one torque sensor, and three high-
resolution optical encoders. A tactical-grade Inertial Measurement Unit (IMU)
(KVH 1775) is mounted on its trunk. Of particular importance to this
experiment is the Vicon Motion Capture System (MCS). It is a multi-camera
infrared system capable of measuring the pose of an object with high accuracy.
During experiments, an accurate and non-drifting estimate of the position of
the feet in the world frame is required to calculate the real penetration for
the TCE. Typically, HyQ works independently of external sensors (e.g., MCS or
GPS), however, soft terrain presents problems for state estimators [29]. This
was re-affirmed in experiment.

The current state estimator [67] relies upon fusion of IMU and leg odometry
data at a high frequency and uses lower frequency feedback from cameras or
lidars to correct the drift. The leg odometry makes the assumption that the
ground is rigid. On soft terrain, the estimator has difficulties in
determining when a foot is in contact with the ground (i.e., is the foot in
the air, or compressing the surface?). These errors cause the leg odometry
signal to drift jeopardizing the estimation. Although, incorporating vision
information could be a possibility to correct for the drift in the estimation,
improving state estimation on soft terrain is an ongoing area of research and
is out of the scope of this paper.

Despite the drifting problem, we used the current state estimator [67] in our
WBC because the planner in Fig. 3.3 has a re-planning feature that makes our
WBC robust against a drifting state estimator [41]. However, the TCE still
requires an accurate and non drifting estimate of the feet position in the
world frame. Therefore, to validate the TCE, we used an external MCS that
completely eliminates the drift problem. The MCS measures the pose of a
special marker array placed on the head of the robot. Then the position of the
feet in the world frame was calculated online by using the MCS measurement and
the forward kinematics of the robot.

##### 3.6.2 Terrain Compliance Estimator (TCE) Settings

In this work we used a sliding window of $m=250$ samples (or 1 s for a control
loop running at 250 Hz). Despite the general formulation, in this paper we
estimate the terrain parameters only for the direction normal to the terrain,
and assume that the tangential directions are the same. We carried out the
simulation and experiment on a horizontal plane. Thus, the rotation matrix
$R_{W}^{C_{i}}$ is identity. Furthermore, we did not estimate the damping
parameter due to the inherent noise in the feet velocity signals that would
jeopardize the estimation. The damping term $D_{\mathrm{st}}v_{st}$ in (3.18)
is less dominant in computing the GRFs compared to the stiffness term. This is
because the feet velocities in the world frame $v_{st}$ are usually orders of
magnitude smaller than the penetration during stance, and the damping
parameter $D_{\mathrm{st}}$ is usually orders of magnitude smaller than the
stiffness parameter as shown in [63].

##### 3.6.3 Tuning of the Low Level Control

During experiments, we found that the low level torque loop creates system
instabilities when interacting with soft environments.

In particular, when we used the same set of (high) torque gains in the low
level control loop tuned for rigid terrain, we noticed joint instabilities
when walking over soft terrain. This is because interacting with soft terrain
reduces the stability margins of the system. Thus, keeping a high bandwidth in
the inner torque loop given the reduced stability margin will cause system
instability. In our previous work [68], we experimentally validated that
increasing the torque gain of the inner loop can indeed cause system
instabilities. In fact, this is a well know issue in haptics [69]. As a
result, reducing the bandwidth by decreasing the torque gains in the inner
torque loop was necessary to address these instabilities.

Our control design is a nested architecture consisting of the WBC and the low
level torque control in which, both control loops contribute to the system
stability [68, 70]. Over soft terrain, the dynamics of the environment also
plays a role and must be considered in analyzing the stability of the system.
That said, there is a nontrivial relationship between soft terrain and the
stability of a nested control loop architecture, and a formal and thorough
analysis is an ongoing work.

#### 3.7 Results

In this section, we evaluate the proposed approach on HyQ in simulation and
experiment. We compare three approaches: the sWBC which is the baseline, the
c3WBC which is our proposed WBC without the TCE, and STANCE which incorporates
both the c3WBC and TCE. We show the extent of improvement given by the c3WBC
controller with respect to the sWBC as well as the importance of the TCE
during locomotion over multiple terrains with different compliances. We set
the same parameters and gains throughout the entire simulations and
experiments, unless mentioned otherwise. The results are shown in the
accompanying video111Link: https://youtu.be/0BI4581DFjY.

##### 3.7.1 Simulations

To render soft terrain in simulation, we used the Open Dynamics Engine (ODE)
physics engine [71]. We used ODE because it is easily integrable with our
framework, and it is numerically fast and stable for stiff and soft contacts
[72]. Moreover, ODE can render soft contacts that emulates physical parameters
(using the SI units N/m and Ns/m for springs and dampers, respectively) unlike
other engines that uses non-physical ones [73]. ODE’s implicit solver uses
linear springs and dampers for their soft constraints which fits perfectly
with our contact model (3.18). In this way, we have a controlled simulation