environment where we can emulate any terrain compliance by manipulating its
stiffness $K_{t}$ and damping $D_{t}$ parameters similar to our contact model.
Throughout the simulation, we use four types of terrains with the following
parameters: soft $T_{1}$ ($K_{t}=3500$ N/m), moderate $T_{2}$ ($K_{t}=8000$
N/m), stiff $T_{3}$ ($K_{t}=10000$ N/m), and rigid $T_{4}$ ($K_{t}=2\times
10^{6}$ N/m) all with the same damping ($D_{t}=400$ Ns/m).

###### 3.7.1.1 Locomotion over Multiple Terrains

Figure 3.5: Simulation. Comparison of sWBC, c3WBC, and STANCE over three type
of terrains: soft $T_{1}$ ($K_{t}=3500$ N/m), moderate $T_{2}$ ($K_{t}=8000$
N/m), and rigid $T_{4}$ ($K_{t}=2\times 10^{6}$ N/m) all with the same damping
($D_{t}=400$ Ns/m). (a)-(c): The actual and desired contact forces in the
normal direction of the LF leg for one gait cycle. (d)-(f): The actual $p$ and
desired $\epsilon$ penetration in the normal direction of the LF leg for one
gait cycle. The red and green ellipses highlight the performance of the three
approaches in adapting to soft terrain.

Table 3.1: Mean Absolute Tracking Error (MAE) [N] of the GRFs in Simulation using sWBC, c3WBC and STANCE over Multiple Terrains.  Terrain | sWBC | c3WBC | STANCE
---|---|---|---
Soft | 7.7261 | 7.4419 | 6.3547
Moderate | 8.0594 | 7.4585 | 7.9889
Rigid | 4.889 | 6.6523 | 5.128

We evaluate the three approaches with the robot walking at $0.05$ m/s over the
terrains: $T_{1}$ (soft), $T_{2}$ (moderate), and $T_{4}$ (rigid). We provided
the c3WBC with the terrain parameters of the moderate terrain $T_{2}$ for all
the three simulations. We do that in order to test the performance of c3WBC if
given the real terrain parameters (in case of $T_{2}$) or inaccurate
parameters (in case of $T_{1}$ and $T_{4}$). In this simulation, we compare
the actual values of $F_{\mathrm{grf,\perp}}$ against the optimal values
$F_{\mathrm{grf,\perp}}^{*}$(solution of the WBOpt) as well as the actual
penetration $p$ against the desired penetration $\epsilon$ of the Left-Front
(LF) leg. We have omitted the other three feet for space as all four legs have
the same performance. The results are shown in Fig. 3.5. The Mean Absolute
Tracking Error (MAE) of the GRFs in these simulations are presented in Table
3.1. The MAE of the GRFs is defined as:
$\text{MAE}=\frac{1}{T}\int_{0}^{T}|F_{\mathrm{grf}}-F_{\mathrm{grf}}^{*}|dt$.

Fig. 3.5a captures the effect of the three approaches on the GRFs over soft
terrain. We can see that a WBC based on a rigid contact assumption (sWBC)
assumes that it can achieve an infinite bandwidth from the terrain and thus
supplying an instantaneous change in the GRFs as highlighted by the red
ellipses in Fig. 3.5a. On the other hand, STANCE and c3WBC were both capable
of attenuating this effect as highlighted by the green ellipses. For the
reasons explained earlier in this paper and in [49], instantaneous changes in
the GRFs are undesirable over soft terrain. This resulted in an improvement in
the tracking of the GRFs in STANCE and c3WBC compared to sWBC as shown in
Table 3.1. Moreover, by comparing c3WBC and STANCE over soft terrain $T_{1}$,
we can see that the shape of the GRFs did not differ. However, the tracking of
the GRFs in STANCE is better than the c3WBC. This shows that suppling the
c3WBC with the incorrect values of the terrain parameters deteriorates the
GRFs tracking performance. Fig. 3.5b shows the GRFs on a moderate terrain.
Since the c3WBC is provided with the exact terrain parameters of $T_{2}$, we
can perceive the c3WBC as STANCE with a perfect TCE on moderate terrain. As a
result, Table 3.1 shows that in this set of simulations, c3WBC outperformed
STANCE in the GRFs tracking. This shows that a more accurate TCE can result in
a better GRFs tracking. Additionally, Fig. 3.5c shows the GRFs on rigid
terrain. We can see that the sWBC resulted in a typical (desired) shape of the
GRFs for a crawl motion in rigid terrain [41]. STANCE showed a shape of the
GRFs similar to the sWBC which is expected since the TCE provided STANCE with
parameters similar to the rigid terrain. However, for c3WBC, the GRFs shape
did not change compared to the other three terrains. As shown in Table 3.1,
the best tracking to the GRFs was by the sWBC, which was expected since the
sWBC was designed for rigid terrain. However, sWBC was only slightly better
than STANCE due to small estimation errors from the TCE.

Fig. 3.5a-c show the superiority of STANCE compared to sWBC and c3WBC. STANCE
adapted to the three terrains by estimating their parameters and supplying
them to the WBC. This resulted in changing the shape of the GRFs accordingly
that improved the tracking of the GRFs. Unlike STANCE, the sWBC and the c3WBC
both are contact consistent for only one type of terrain which resulted in a
deterioration of the GRFs tracking over the other types of terrains. The
advantages of STANCE compared to sWBC and c3WBC are also shown in Fig. 3.5d-f.
Since the sWBC is always assuming a rigid contact, the penetration $\epsilon$
was always zero throughout the three terrains. Similarly, since the c3WBC
alone is aware only of one type of terrain, it is always assuming the same
contact model, in which the desired penetration $\epsilon$ was similar
throughout the three terrains. STANCE, however, was capable of predicting the
penetration correctly for all the three terrains.

In general, even if the contact model is for soft contacts, STANCE was capable
of correctly predicting the penetration of the robot even in rigid terrain
(zero penetration). This resulted in STANCE adapting to rigid, soft and
moderate terrains by means of adapting the GRFs and correctly predicting the
penetration.

###### 3.7.1.2 Longitudinal Transition Between Multiple Terrains

Figure 3.6: Simulation. Traversing multiple terrains of different compliances
($T_{4}$, $T_{1}$, $T_{2}$, $T_{3}$, $T_{4}$). Top: Tracking of the desired
terrain penetration of the LF leg in the xz-plane. Bottom: Estimated terrain
stiffness of the LF leg. For readability purposes we only plot estimated
values less than $2\times 10^{4}$. The green shaded areas highlight the
overlap between terrains that results in higher estimated stiffness (black
ellipses).

We show the adaptation of STANCE when walking and transitioning between
multiple terrains. We test the accuracy of the TCE and the effect of closing
the loop of the c3WBC with the TCE on the feet trajectories and terrain
penetration. In this simulation, HyQ is traversing five different terrains,
starting and ending with a rigid terrain: $T_{4}$, $T_{1}$, $T_{2}$, $T_{2}$,
$T_{4}$. The results are presented in Fig. 3.6. The top plot presents the
actual foot position against the desired penetration $\epsilon$ of the LF leg
in the xz-plane of the world frame. The origin of the z-direction (normal
direction) is the uncompressed terrain height. Thus, trajectories below zero
represent the penetration of the LF leg. The bottom plot shows the history of
the estimated terrain stiffness of the TCE of the LF leg. Table 3.2 reports
the mean, standard deviation, and percentage error222 The percentage error is
defined as: % Error
$=|\frac{\text{Estimate}-\text{Actual}}{\text{Actual}}|\times 100$ of the
estimated terrain stiffness of the LF leg against the ground truth value set
in ODE. The table shows that the TCE had an estimation accuracy below $2$% for
the soft terrains $T_{1}$, $T_{2}$ and $T_{3}$. However, the estimation
accuracy of the rigid terrain was lower than that of the soft terrains. This
is expected since on a rigid terrain, the penetrations are (almost) zero.
Thus, a small inaccurate penetration estimation due to any model errors could
result in a lower estimation accuracy. Apart from the rigid case, the standard
deviation is always below 6$\%$ of the ground truth value. Fig. 3.6 shows that
STANCE is always c3, the actual foot position is always consistent with the
desired penetration during stance. We can see that, when HyQ is standing over
rigid terrain, both the actual foot position and desired penetration are zero.
As HyQ walks, over the soft terrains, the penetration is highest in the
softest terrain and smallest in the stiffest terrain.

In the simulation environment, we overlapped the terrains to prevent the feet
from getting stuck between them. This overlap created a transition
(highlighted in green in the figure) which resulted in a stiffer terrain. The
overlap was captured by the TCE and resulted in a slight increase in the
estimated parameters as highlighted by the two ellipses in the lower plot. We
also noticed a lag in estimation, due to a filtering effect, since the TCE is
using the most recent $m$-samples. As highlighted by the black box in Fig.
3.6, HyQ was on rigid terrain (actual penetration is zero) while STANCE still
perceived it as being on $T_{3}$ (desired penetration is non-zero).

Table 3.2: Mean $\mu$ [N/m], Standard Deviation $\sigma$ [N/m], and Percentage Error of the Estimated Terrain Stiffness of the LF Leg in Simulation. Terrain | Actual Stiffness | Mean $\mu$ $\pm$ STD $\sigma$ | % Error
---|---|---|---
$T_{1}$ | 3500 | 3530 $\pm$ 200 | 0.9%
$T_{2}$ | 8000 | 8110 $\pm$ 400 | 1.4%
$T_{3}$ | 10000 | 10110 $\pm$ 400 | 1.1%
$T_{4}$ | 2000000 | 2240000 $\pm$ 740000 | 12%

###### 3.7.1.3 Aggressive trunk maneuvers

Figure 3.7: Simulation. Comparing sWBC and STANCE under aggressive trunk
maneuvers. Left: Top view of the front feet (RF and LF) positions. Right: Side
view of the LF position.

We tested sWBC and STANCE under aggressive trunk maneuvers by commanding
desired sinusoidal trajectories at the robot’s height ($0.05$ m amplitude and
$1.8$ Hz frequency) and at roll orientation ($0.5$ rad amplitude and $1.5$ Hz
frequency) over the soft terrain $T_{1}$. The results are shown in Fig. 3.7.
The left plot shows a top view of the actual front feet (LF and Right-Front
(RF)) positions in the world frame. The right plot shows a side view of the
actual LF foot position in the world frame. We notice that the feet of HyQ are
always in contact with the terrain in STANCE which is expected since STANCE is
c3. Unlike STANCE, the feet did not remain in contact with the terrain in the
sWBC. This is clearly seen in Fig. 3.7 where HyQ lost contact multiple times.
This resulted in the robot falling over in the sWBC case as shown in the
video.

###### 3.7.1.4 Speed Test

Figure 3.8: Simulation. Speed test. Increasing the desired forward velocity
from $0.05$ to $0.3$ m/s. Left: Side view of the RF (a) and LH (b) positions.
Right: (c) Closeup section of the top left plot. The green lines are the
desired step height. The black dashed lines are the terrain height.

We carried out a simulation where HyQ walks over soft terrain $T_{1}$,
starting with a forward velocity of $0.05$ m/s until it reaches $0.3$ m/s with
an acceleration of $0.005$ m/s2. In this simulation, we compare STANCE against
the sWBC. Fig. 3.8a and Fig. 3.8b show the actual trajectories of the RF and
Left-Hind (LH) legs in the world frame, respectively. Fig. 3.8c shows a
closeup section of the RF leg’s trajectory. The simulation shows that STANCE
was c3 over the entire simulation while the sWBC was not.

In particular, STANCE was able to remain in contact with the terrain that
allowed HyQ to start the swing phase directly from the terrain height. Unlike
STANCE, the sWBC is not terrain aware and did not remain c3 which resulted in
starting the swing trajectory while still being inside the deformed terrain.
This is highlighted by the two ellipses in the right plot. Additionally, the
compliance contact consistency property of STANCE enabled the robot to
maintain the desired step clearance (i.e., achieving the desired step height
of $0.14$ cm) compared to sWBC. Most importantly, as shown in the accompanying
video, the sWBC failed to complete the simulation and could not achieve the
final desired forward velocity; It fell at a speed of $0.21$ m/s. Note that
both approaches could reach higher velocities with a more dynamic gait (trot).
However, this simulation is not focusing on analyzing the maximum speed that
the two approaches can reach but rather the differences between these
approaches at a higher crawl speeds.

###### 3.7.1.5 Power Test

Figure 3.9: Simulation. Power consumption comparison between sWBC and STANCE
with different forward velocities (0.05 m/s, 0.15 m/s and 0.25 m/s).

In this test, we compare the power consumption using STANCE and sWBC on HyQ
during walking over the soft terrain $T_{1}$ at different forward velocities
($0.05$ m/s, $0.15$ m/s and $0.25$ m/s). Fig. 3.9 presents the energy plots of
STANCE and sWBC. The plot shows that STANCE requires less power than the sWBC
because it knows how the terrain will deform. STANCE exploits the terrain
interaction to achieve the motion. The difference in consumed energy is
negligible at $0.05$ m/s but becomes significant at higher speeds.

##### 3.7.2 Experiment

We validated the simulation presented in Section 3.7.1 on the real platform.
We analyzed sWBC, c3WBC (with fixed terrain parameters) and STANCE as well as
the performance of the TCE module itself.

A foam block of $160$ cm $\times~{}120$ cm $\times~{}20$ cm was selected as a
soft terrain for these experiments. To obtain a ground truth of the foam
stiffness, we carried out indentation tests on a $50$ cm3 sample of the foam
with a stress-strain machine that covers the range of penetration of interest
for our robot (below $0.15$ cm). The indentation test showed a softening
behavior of the foam with an average stiffness of $2400$ N/m. The MAE of the
GRFs of the upcoming experiments are shown in Table 3.3.

###### 3.7.2.1 Locomotion over Soft Terrain

Figure 3.10: Experiment. Comparing sWBC, c3WBC and STANCE over a soft foam block ($K_{t}=2400$ N/m). Top: Tracking of the GRFs of the RF leg. Bottom: Tracking of the foot penetration. The gray shaded areas represent the uncertainty of the measurements.  Table 3.3: Mean Absolute Tracking Error (MAE) [N] of the GRFs using sWBC, c3WBC and STANCE under Different Sets of Experiments. Description | sWBC | c3WBC | STANCE
---|---|---|---
Soft Terrain (Sec. 3.7.2.1) | 73.9042 | 68.5581 | 61.8207
Longitudinal Trans. (Sec. 3.7.2.2) | 70.5276 | 64.2636 | 60.6285
Lateral Trans. (Sec. 3.7.2.3) | 73.0766 | - | 53.0107
Table 3.4: Mean $\mu$ [N/m], Standard Deviation $\sigma$ [N/m], and Percentage Error of the Estimated Terrain Stiffness of the Four Legs in Experiment over Soft Terrain ($2400$ N/m). Leg | Mean $\mu$ $\pm$ STD $\sigma$ | % Error
---|---|---
LF | 2186 $\pm$ 166 | 9%
RF | 2731 $\pm$ 173 | 14%
LH | 2368 $\pm$ 317 | 1%
RF | 2078 $\pm$ 331 | 13%

In this experiment, HyQ is walking over the foam with a forward velocity of
$0.07$ m/s using the three approaches. The results are presented in Fig. 3.10
that shows the actual and desired $F_{\mathrm{grf,\perp}}$ and penetration of
the RF leg. The shaded gray area in the lower plots of Fig. 3.10 represents
the uncertainty in the estimation of the foot position (see Section 3.6.1). In
these experiments, all three approaches performed well; none of them failed.
However, the shape of GRFs were different within the three approaches. As in
Section 3.7.1.1, since sWBC is rigid contact consistent, the desired GRFs were
designed for rigid contacts. Unlike sWBC, STANCE is c3, which was capable of
changing the shape of the GRFs. This is highlighted in Table 3.3 in which
STANCE outperformed sWBC in the tracking of the GRFs.

In simulation, when we provided the c3WBC with the true value of the
stiffness, the MAE of the GRFs was better. However, in this experiment,
providing the value obtained from the indentation tests to the c3WBC resulted
in a worse GRFs MAE. This outperformance of STANCE compared to the c3WBC in
this experiment could be because of the TCE. To clarify, the actual terrain
compliances are not constant, but since the TCE is online, it is able to
capture these changes in the terrain compliances as well as model errors. As
shown in the accompanying video, STANCE had a smoother transition during
crawling compared to sWBC. We found the robot transitioning from swing to
stance more aggressively in sWBC than STANCE. Such smooth behavior was also
noticed in [49].

Table 3.4 shows the mean, standard deviation, and percentage error of the
estimated terrain stiffness of all the four legs against the ground truth
value ($2400$ N/m) obtained from the indentation tests. The table shows that
the accuracy of the TCE in simulation is better than in experiments. This is
expected since in simulation, the TCE has a perfect knowledge of the feet
penetration. However, the accuracy of our TCE is better compared to [13] in
which the percentage error exceeded 50% (the actual stiffness was more than
double that of the estimated one in [13]).

###### 3.7.2.2 Longitudinal Transition Between Multiple Terrains

Figure 3.11: Experiment. Longitudinal transition from soft to rigid terrain.
The first three plots show the tracking of the desired foot penetration of RF
leg using the three approaches (the sWBC, c3WBC with fixed terrain stiffness
and STANCE. The fourth plot shows the stiffness estimated by the TCE for the
four legs.

Similar to Section 3.7.1.2, we compare the three approaches while
transitioning between the foam block and a rigid pallet. We added a pad
between between the two terrains to avoid the foot getting stuck (see Fig.
3.1a). Fig. 3.11a-c show the actual position and the desired penetration of
the RF leg in the xz-plane for the three approaches. Fig. 3.11d shows the
estimated terrain stiffness of the TCE for all four feet.

From Fig. 3.11a-b we see that both sWBC and c3WBC did not adapt to terrain
changes. Since both controllers are designed for a specific constant terrain,
the desired penetration did not change from soft to rigid. In the sWBC, there
is no tracking of the penetration, and in the c3WBC, the tracking of the
penetration is good only when the leg is on the foam where the stiffness is
consistent to the one used in the controller. On the other hand, as shown in
Fig. 3.11c-d, STANCE changes its parameters when facing a different terrain;
it was capable of adapting its desired penetration to the type of terrain. In
fact, the desired penetration was non-zero on soft terrain and was almost zero
on rigid terrain. This again resulted in STANCE achieving the best GRFs
tracking as shown in Table 3.3.

Fig. 3.11d shows the importance of having a TCE for each leg. The estimated
terrain parameters are different between the legs where the hind legs are on
the foam while the rigid ones transitioning from foam to rigid. The figure
also shows that the LF leg walked over the rigid terrain before the RF and
that the TCE captures the intermediate stiffness estimation due to the rubber
pad (see video).

###### 3.7.2.3 Lateral Transition Between Multiple Terrains

Unlike the previous experiment, we set the foam and the pallet laterally as
shown in Fig. 3.1c and in the accompanying video. This is a more challenging
scenario for stability reasons. In particular, the robot must extend its leg
further in the soft terrain maintain the trunk’s balance. Consequently, since
the width of HyQ’s torso is smaller than its length, the Zero Moment Point
(ZMP) is more likely to get out of the support polygon. The GRFs MAE in Table
3.3 show that STANCE can outperform sWBC during both longitudinal and lateral
transitions.

###### 3.7.2.4 External Disturbances over Soft Terrain

Figure 3.12: Experiment. The sWBC and STANCE under disturbances over soft
terrain. Top: The actual and desired $F_{\mathrm{grf,\perp}}$ in sWBC and
STANCE, respectively. Bottom: The actual torque and torque limits of the Knee
Flexion-Extension (KFE) joint of the RF leg in sWBC and STANCE, respectively.

In this experiment, we test the sWBC and STANCE when the user applies a
disturbance on HyQ. The results are shown in Fig. 3.12. The top plots show the
actual and desired $F_{\mathrm{grf,\perp}}$ in sWBC and STANCE, respectively.
The bottom plots show the actual torque and torque limits of the Knee Flexion-
Extension (KFE) joint of the RF leg in sWBC and STANCE, respectively. In the
accompanying video, we can qualitatively see that with STANCE, the feet of HyQ
keep moving to remain c3 with the terrain. On the other hand, the sWBC kept
its feet stationary. This behavior was also reported by [29].

Most importantly, we noticed that HyQ reaches the torque limits in the sWBC as
shown in Fig. 3.12. However, in STANCE, since the robot was constantly moving
its feet, hence redistributing its forces, the torque limits were not reached.
This behavior was also reflected on the GRFs in which, the GRFs were
resonating in the sWBC as highlighted by the ellipse in Fig. 3.12.

###### 3.7.2.5 TCE’s Performance over Multiple Terrains

Table 3.5: Mean $\mu$ [N/m] and Standard Deviation $\sigma$ [N/m] of the Estimated Terrain Stiffness of the Four Legs in Experiments (see Fig. 3.1b). Leg | Mean $\mu$ $\pm$ STD $\sigma$
---|---
LF | 448400 $\pm$ 165100
RF | 55200 $\pm$ 48400
LH | 2645000 $\pm$ 336000
RH | 1393000 $\pm$ 442000

We analyze the performance of the TCE on HyQ over multiple terrains with
various softnesses. The softness of the four used terrains are shown in Fig.
3.1b. The estimated stiffness (mean and standard deviation) under each leg is
shown in Table 3.5. As shown in the table, the robot can differentiate between
the types of terrain. Although we did not measure the true stiffness value of
these terrains, we can observe their softness in the video and Fig. 3.1b and
compare it to the values in Table 3.5.

##### 3.7.3 Computational Analysis

STANCE is running online which means that we can estimate the terrain
compliance (using the TCE) continuously while walking, and run the entire
framework without breaking real-time requirements. We validated the first
argument by showing that indeed the TCE can continuously estimate the terrain
compliance. Hereafter, we validate the second argument by analyzing the
computational complexity of STANCE and compare it against the sWBC. Since our
WBC framework is running at $250~{}\mathrm{Hz}$, it is essential that the
computation does not exceed the $4~{}\mathrm{ms}$ time frame. Hence, we
conducted a simulation in which we calculated the time taken to process the
entire framework without the lower level control (ie., the state estimator,
the planner and the WBC) that is running on a different real-time thread at
$1~{}\mathrm{kHz}$. We compared the computation time on an Intel Core i$7$
quad core CPU in the case of STANCE (the c3WBC and the TCE) and the sWBC. We
used the same parameters and gains as in Section 3.7.1.1. The results show
that the average processing time taken was $0.68~{}\mathrm{ms}$ and
$0.74~{}\mathrm{ms}$ for the sWBC and STANCE respectively. In both cases, the
maximum computation time was always below $2~{}\mathrm{ms}$.

#### 3.8 Conclusions

We presented a soft terrain adaptation algorithm called STANCE: Soft Terrain
Adaptation aNd Compliance Estimation. STANCE can adapt online to any type of
terrain compliance (stiff or rigid). STANCE consists of two main modules: a
compliant contact consistent whole-body controller (c3WBC) and a terrain
compliance estimator (TCE). The c3WBC extends our previously implemented WBC
(sWBC) [1], such that it is contact consistent to any type of compliant
terrain given the terrain parameters. The TCE estimates online the terrain
compliance and closes the loop with the c3WBC. Unlike previous works on WBC,
STANCE does not assume that the ground is rigid. Stance is computationally
lightweight and it overcomes the limitations of the previous state of the art
approaches. As a result, STANCE can efficiently traverse multiple terrains
with different compliances. We validated STANCE on our quadruped robot HyQ
over multiple terrains of different stiffness in simulation and experiment.
This, to the best of the authors’ knowledge, is the first experimental
validation on a legged robot of closing the loop with a terrain estimator.

Incorporating the terrain knowledge makes STANCE c3. This means that STANCE is
able to generate smooth GRFs that are physically consistent with the terrain,
and continuously adapt the robot’s feet to remain in contact with the terrain.
As a result, the tracking error of the GRFs and the power consumption were
reduced, and the impact during contact interaction was attenuated.
Furthermore, STANCE is more robust in challenging scenarios. As demonstrated,
STANCE made it possible to perform aggressive maneuvers and walk at high
walking speeds over soft terrain compared to the state of the art sWBC. In the
standard case, the contact is lost because the motion of the terrain is not
taken into account. On the other hand, there are minor differences in
performance between STANCE and the sWBC for less dynamic motions.

STANCE can efficiently transition between multiple terrains with different
compliances, and each leg was able to independently sense and adapt to the
change in terrain compliance. We also tested the capability of the TCE in
discriminating between different terrains. The insights gained in simulation
have been confirmed in experiment.

In future works, we plan to implement an algorithm to improve the TCE. In
particular, we plan on using onboard sensors, such as a camera, instead of
relying on the external measurements from an MCS. We also plan to explore
other non-linear contact models in the TCE and the c3WBC.

###  Chapter 4 On 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.

Abstract. Locomotion over soft terrain remains a challenging problem for
legged robots. 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. That said, this letter answers the following
questions: how and why does soft terrain affect state estimation for legged
robots? To do so, we utilized a state estimator that fuses IMU measurements
with leg odometry that is designed with rigid contact assumptions. We
experimentally validated the state estimator with the HyQ robot trotting over
both soft and rigid terrain. 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.

#### 4.1 Introduction

Quadruped robots are advancing towards being fully autonomous as can be seen
by their recent development in research and industry, and their remarkable
agile capabilities [74, 75, 76]. This demands quadruped robots to be robust
while traversing a wide variety of unexplored complex non-flat terrain. The
terrain may not just vary in geometry, but also in its physical properties
such as terrain impedance or friction. Reliable state estimation is a major
aspect for the success of the deployment of quadruped robots because most
locomotion planners and control strategies rely on an accurate estimate of the
pose and velocity of the robot. Furthermore, reliable state estimation is
essential, not only for locomotion (low-level state estimation), but also for
autonomous navigation and inspection tasks that are emerging applications for
quadruped robots (task-level state estimation).

To date, most of the work done on state estimation for legged robots are based
on filters that fuse multiple sensor modalities. These sensor modalities
mainly include high frequency inertial measurements and kinematic measurements
(e.g., leg odometry), as well as other low frequency modalities (e.g., cameras
and lidars) to correct the drift.

For instance, an Extended Kalman Filter (EKF)-based sensor fusion algorithm
has been proposed by [67] that fuses Inertial Measurement Unit (IMU)
measurements, leg odometry, stereo vision, and lidar. In [77], a similar
algorithm has been proposed that fuses IMU measurements, leg odometry, stereo
vision, and GPS. In [75], a nonlinear observer that fuses IMU measurements and
leg odometry has been proposed. In [78], a state estimator fuses a Globally
Exponentially Stable (GES) nonlinear attitude observer based on IMU
measurements with leg odometry to provide bounded velocity estimates. The
global stability is important for cases when the robot may have fallen over
whereas typical EKF-based works may diverge. The bounded velocity estimates
help to decrease drift in the unobservable position estimates. Finally, an
approach similar to [78] has been proposed in [79]. This approach proposed an
invariant EKF-based sensor fusion algorithm that includes IMU measurements,
contact sensor dynamics, and leg odometry.

The aforementioned state estimators are shown to be reliable on stiff terrain.
Yet, over soft terrain (as shown in Fig. 4.1), the performance of these state
estimators starts to decline. Over soft terrain, the state estimator has
difficulties determining when a foot is in contact with the ground. For
instance, the state estimator has difficulties determining if the foot is in
the air, if the foot is applying more force than the terrain (terrain
compression), if the terrain itself is applying more force than the foot
(terrain expansion), or if the foot and the terrain are applying the same
force (rigid terrain). This results in a large position estimate drift, and it
was reported in our previous work [2] where we noted that we encountered
difficulties because of state estimation over soft terrain. Apart from our
previous work, other works also mention that state estimation over soft
terrain is a challenging task, e.g., [80, 46]. Yet, to the authors’ knowledge,
literature has not yet discussed the question on how soft terrain affects the
state estimation.

The contributions of this work are the experimental analysis and formal study
on: the effects of soft terrain on state estimation, the reasons behind these
effects, and simple ways to improve state estimation. This letter is building
upon our previous work on soft terrain adaptation [2] and on state estimation
[78].

The rest of this letter is organized as follows: Section 4.2 describes the
robot model, the onboard sensors, and how to estimate the Ground Reaction
Forces (GRFs) acting on the robot. Section 4.3 explains the state estimator
used in this letter, and how to estimate the base velocity of the robot using
leg odometry. Section 4.4 details the results of our experiment and
demonstrates how soft terrain affects state estimation. Finally, Section 4.5
presents our conclusions.

Figure 4.1: HyQ traversing multiple terrains of different compliances.

#### 4.2 Modeling, Sensing, and Estimating

In this letter, we consider the quadruped robot HyQ [66] shown in Fig. 4.1.
Each leg has three actuated joints. Despite experimenting on a specific
platform, the problem is generic in nature and it applies equally to any
legged robot. Furthermore, by using the $90$ kg HyQ robot, a heavy and strong
platform, we are exciting more dynamics.

##### 4.2.1 Notations

We introduce the following reference frames: the body frame $\mathcal{B}$
which is located at the geometric center of the trunk (robot torso), and the
navigation frame $\mathcal{N}$ which is assumed inertial (world frame). The
basis of the body frame are orientated forward, left, and up. To simplify
notation, the IMU is located such that the accelerometer measurements are
directly measured in $\mathcal{B}$.

##### 4.2.2 Kinematics and Dynamics

Assuming that all of the external forces are exerted on the feet, the dynamics
of the robot is

$M(\bar{x})\ddot{\bar{x}}+h(\bar{x},\dot{\bar{x}})=\bar{\tau}$ (4.1)

where
$\bar{x}=\begin{bmatrix}x^{T}&\eta^{T}&q^{T}\end{bmatrix}^{T}\in\mathbb{R}^{18}$
is the generalized robot states, $\dot{\bar{x}}\in\mathbb{R}^{18}$ is the
corresponding generalized velocities, $\ddot{\bar{x}}\in\mathbb{R}^{18}$ is
the corresponding generalized accelerations, $x\in\mathbb{R}^{3}$ is the
position of the base, $\eta\in\mathbb{R}^{3}$ is the attitude of the base,
$q\in\mathbb{R}^{12}$ is the vector of joint angles of the robot,
$M\in\mathbb{R}^{18\times 18}$ is the joint-space inertia matrix, $h$ is the
vector of Coriolis, centrifugal and gravity forces,
$\bar{\tau}=(\begin{bmatrix}0&\tau^{T}\end{bmatrix}^{T}-JF)\in\mathbb{R}^{18}$,
$\tau\in\mathbb{R}^{12}$ is the vector of actuated joint torques,
$J\in\mathbb{R}^{18\times 12}$ is the floating base Jacobian, and
$F\in\mathbb{R}^{12}$ is the vector of external forces (i.e., GRFs).

We solve for the GRFs $F_{\ell}$ of each leg $\ell$ using the actuated part of
the dynamics in (4.1).

$F_{\ell}=-\alpha_{\ell}(J_{\ell}^{T}(q_{\ell}))^{-1}(\tau_{\ell}-h_{\ell}(\bar{x}_{\ell},\dot{\bar{x}}_{\ell}))$
(4.2)

$F_{\ell}\in\mathbb{R}^{3}\subset F$ is the GRFs for $\ell$ in $\mathcal{B}$,
$J_{\ell}\in\mathbb{R}^{3\times 3}\subset J$ is the foot Jacobian of $\ell$,
$\tau_{\ell}\in\mathbb{R}^{3}\subset\tau$ is the vector of joint torques of
$\ell$, $h_{\ell}\in\mathbb{R}^{3}\subset h$ is the vector of centrifugal,
Coriolis, gravity torques of $\ell$ in $\mathcal{B}$, and
$\alpha_{\ell}\in\\{0,1\\}$ selects if the foot is on the ground or not. A
threshold of $F_{\ell}$ is typically used to calculate $\alpha_{\ell}$.

$\alpha_{\ell}=\begin{cases}1&||(J_{\ell}^{T})^{-1}(\tau_{\ell}-h_{\ell})||>\epsilon\\\
0&\text{otherwise}\end{cases}$ (4.3)

where $\epsilon>0\in\mathbb{R}$ is the threshold.

###### Assumption 4.1

There exists a force threshold $\epsilon$ that determines if the foot is in
contact with the environment.

The translational and rotational kinematics, and the translational dynamics of
the robot as a single rigid body in $\mathcal{N}$ are

$\displaystyle\dot{x}^{n}=v^{n}$ $\displaystyle\dot{v}^{n}=a^{n}+g^{n}$
$\displaystyle\dot{R}^{n}_{b}=R^{n}_{b}S(\omega^{b})$ (4.4)

where $x^{n}\in\mathbb{R}^{3}$, $v^{n}\in\mathbb{R}^{3}$,
$a^{n}\in\mathbb{R}^{3}$ are the position, velocity, and acceleration of the
base in $\mathcal{N}$, respectively, $R_{b}^{n}\in\operatorname{SO}(3)$ is the
rotation matrix from $\mathcal{B}$ to $\mathcal{N}$, and $\omega^{b}$ is the
angular velocity of the base in $\mathcal{B}$. The skew symmetric matrix
function is $S()$.

##### 4.2.3 Sensors

The modeling assumes that the quadruped robot is equipped with a six-axis IMU
on the trunk (3 Degrees of Freedom (DoFs) gyroscope and 3 DoFs accelerometer),
and that every joint contains an encoder and a torque sensor. The
accelerometer measures specific force $f_{s}^{b}\in\mathbb{R}^{3}$

$f_{s}^{b}=a^{b}+g^{b}$ (4.5)

where $a^{b}\in\mathbb{R}^{3}$ is the acceleration of the body in
$\mathcal{B}$ and $g^{b}\in\mathbb{R}^{3}$ is the acceleration due to gravity
in $\mathcal{B}$. The gyroscope directly measures angular velocity
$\omega^{b}\in\mathbb{R}^{3}$ in $\mathcal{B}$. The encoders are used to
measure the joint position $q_{i}\in\mathbb{R}$ and joint speed
$\dot{q}_{i}\in\mathbb{R}$. The pose of each joint (i.e., the forward
kinematics) is assumed to be exactly known. The torque sensors in the joints
directly measure torque $\tau_{i}\in\mathbb{R}$.

The measured values of all of the sensors differ from the theoretical values
in that they contain a bias and noise: $\tilde{x}=x+b_{x}+n_{x}$ where
$\tilde{x}$, $b_{x}$, and $n_{x}$ are the measured value, bias, and noise of
$x$, respectively. All of the biases are assumed to be constant or slowly
time-varying, and all of the noise variables have zero mean and a Gaussian
distribution.

#### 4.3 State Estimator

To compare the effect of different terrains, we use the state-of-the-art low-
level state estimator from [78]. It includes input from three proprioceptive
sensors: an IMU, encoders, and torque sensors. For reliability and speed no
exteroceptive sensors are used. The state estimator consists of three major
components: an attitude observer, leg odometry, and a sensor fusion algorithm.

##### 4.3.1 Non-linear Attitude Observer

Typically in the quadruped robot literature an EKF is used for attitude
estimation, e.g., [67, 77, 81]. However, our attitude observer [78] is GES,
and it consists of a Non-Linear Observer (NLO) [82] and an eXogeneous Kalman
Filter (XKF) [83]. The NLO is

$\displaystyle\begin{split}\dot{\hat{R}}^{n}_{b}&=\hat{R}^{n}_{b}S(\omega^{b}-\hat{b}^{b})+\sigma
K_{p}J_{s}(\hat{R}^{n}_{b})\\\
\dot{\hat{b}}^{b}&=\operatorname{Proj}\left(\hat{b}^{b},-k\operatorname{vex}\left(\mathbb{P}\left({\hat{R}^{nT}_{bs}}K_{p}J_{s}(\hat{R}^{n}_{b})\right)\right)\right)\\\
J_{s}(\hat{R}^{n}_{b})&=\sum_{j=1}^{k}(y_{j}^{n}-\hat{R}^{n}_{b}y_{j}^{b}){y_{j}^{b}}^{T}\end{split}$
(4.6)

where $K_{p}\in\mathbb{R}^{3\times 3}$ is a symmetric positive-definite gain
matrix, $k>0\in\mathbb{R}$ is a scalar gain, $\sigma\geq 1\in\mathbb{R}$ is a
scaling factor, $\hat{R}^{n}_{bs}=\operatorname{sat}(\hat{R}^{n}_{b})$, the
function $\operatorname{sat}(X)$ saturates every element of $X$ to $\pm 1$,
$\operatorname{Proj}$ is a parameter projection that ensures that
$||\hat{b}||<M_{b}$, $M_{b}>0\in\mathbb{R}$ is a constant known upper bound on
the gyro bias, $\mathbb{P}(X)=\frac{1}{2}(X+X^{T})$ for any square matrix $X$,
and $J_{s}$ is the stabilizing injection term. The observer is GES for all
initial conditions assuming there exists $k>1$ non-collinear vector
measurements, i.e., $\left|y_{i}^{n}\times y_{j}^{n}\right|>0$ where
$i,j\in\\{1,\cdots,k\\}$. Furthermore, if there is only one measurement the
observer is still GES if the following Persistency of Excitation (PE)
condition holds: if there exist constants $T>0\in\mathbb{R}$ and
$\gamma>0\in\mathbb{R}$ such that, for all $t\geq 0$,
$\int_{t}^{t+T}y_{1}^{n}(\tau)y_{1}^{n}(\tau)^{T}\mathop{d\tau}\geq\gamma I$
holds then $y_{1}^{n}$ is PE. See [82] for proof.

The XKF [83] is similar to an EKF in that it linearizes a nonlinear model
about an estimate of the state and then applies the typical Linear Time-
Varying (LTV) Kalman filter to the linearized model. If the estimate is close
to the true state then the filter is near-optimal. However, if the estimate is
not close to the true state, the filter can quickly diverge. To overcome this
problem, the XKF linearizes about a globally stable exogenous signal from a
NLO. The cascaded structure maintains the global stability properties from the
NLO and the near-optimal properties from the Kalman filter. The observer is

$\displaystyle\begin{split}\dot{\hat{x}}&=f_{x}+C(\hat{x}-\breve{x})+K\left(z-h_{x}-H(\hat{x}-\breve{x})\right)\\\
\dot{P}&=CP+PC^{T}-KHP+Q\\\ K&=PH^{T}R^{-1}\end{split}$ (4.7)

where $C=\left.\partial f_{x}/\partial{x}\right|_{\breve{x},u}$,
$H=\left.\partial h_{x}/\partial{x}\right|_{\breve{x},u}$,
$\breve{x}\in\mathbb{R}^{n}$ is the bounded estimate of $x$ from the globally
stable NLO. See [83] for the stability proof.

##### 4.3.2 Leg Odometry

Leg odometry computes the overall base velocity $\dot{x}^{b}$ of the robot by
combining the contribution of each foot velocity $\dot{x}_{\ell}^{b}$. Each
leg $\ell$ only contributes to the leg odometry when it is in contact
$\alpha_{\ell}$. Thus, we calculate the overall base velocity $\dot{x}^{b}$ as

$\dot{x}_{\ell}^{b}=-\alpha_{\ell}\left(J_{\ell}(q_{\ell})\dot{q}-\omega^{b}\times
x_{\ell}^{b}\right)\hskip
20.0pt\dot{x}^{b}=\dfrac{1}{n_{s}}\sum_{\ell}\dot{x}_{\ell}^{b}$ (4.8)

where $n_{s}=\sum\limits_{\ell}\alpha_{\ell}$ is the number of stance legs.

###### Assumption 4.2

The leg odometry assumes that the robot is always in rigid contact with the
terrain. This implies that the stance feet do not move in $\mathcal{N}$, there
is no slippage, the terrain does not expand or compress, and the robot does
not jump or fly.

##### 4.3.3 Sensor Fusion

Lastly, the inertial measurements (4.5) are fused with the leg odometry (4.8).
The main advantage of decoupling the attitude from the position and linear
velocity is that the resulting dynamics is LTV, and thus has guaranteed
stability properties. i.e., the filter will not diverge in finite time.

We use a LTV Kalman filter with the dynamics (4.4), the accelerometer (4.5),
and leg odometry (4.8).

$\displaystyle\begin{split}\dot{\hat{\underline{x}}}&=f_{\underline{x}}+\underline{K}(\underline{z}-h_{\underline{x}})\\\
\dot{\underline{P}}&=\underline{CP}+\underline{PC^{T}}-\underline{K}\underline{H}\underline{P}+\underline{Q}\\\
\underline{K}&=\underline{P}\underline{H}^{T}\underline{R}^{-1}\end{split}$
(4.9)

where the state
$\underline{x}=\begin{bmatrix}{x^{n}}^{T}&{v^{n}}^{T}\end{bmatrix}^{T}\in\mathbb{R}^{6}$
is position and velocity of the base, the input
$u=(R_{b}^{n}f_{s}^{b}-g^{n})\in\mathbb{R}^{3}$ is the acceleration of the
base, the measurement $z=R_{b}^{n}x_{\ell}^{b}\in\mathbb{R}^{3}$ is the leg
odometry, $\underline{K}\in\mathbb{R}^{6\times 3}$ is the Kalman gain,
$P\in\mathbb{R}^{6\times 6}$ is the covariance matrix,
$Q\in\mathbb{R}^{6\times 6}$ is the process noise and $R\in\mathbb{R}^{3\times
3}$ is the measurement noise covariance, and

$\displaystyle f_{\underline{x}}=\begin{bmatrix}v^{n}\\\
u\end{bmatrix}\quad\underline{C}=\begin{bmatrix}0_{3}&I_{3}\\\
0_{3}&0_{3}\end{bmatrix}\quad\underline{H}=\begin{bmatrix}0_{3}&I_{3}\end{bmatrix}$

and $I_{3}$ and $0_{3}$ are the $3\times 3$ identity matrix and matrix of all
zeros, respectively.

#### 4.4 Experimental Results

To analyze the differences in state estimation between rigid and soft terrain,
we used HyQ and our state estimator. HyQ has twelve torque-controlled joints
powered by hydraulic actuators. HyQ has three types of on board proprioceptive
sensors: joint encoders, force/torque sensors, and IMUs. Every joint has an
absolute and a relative encoder to measure the joint angle and speed. The
absolute encoder (AMS Programmable Magnetic Rotary Encoder - AS5045) measures
the joint angle when the robot is first turned on, while the relative encoder
(Avago Ultra Miniature, High Resolution Incremental Encoder - AEDA-3300-TE1)
measures how far the joint has moved at every epoch. Every joint contains a
force or torque sensor. Two joints have a load cell (Burster Subminiature Load
Cell - 8417-6005) and one joint has a custom designed torque sensor based on
strain-gauges. In the trunk of the robot there is a fibre optic-based,
military grade KVH 1775 IMU.

We used the state estimator (4.6)-(4.9) on the _Soft Trot in Place_ and the
_Rigid Trot in Place_ dataset from the dataset published in [84]. HyQ was
manually controlled to trot on a foam block of $160\times 120\times 20$ cm,
and on a rigid ground. An indentation test of the foam shows the foam has an
average stiffness of 2400 N/m. All of the sensors were recorded at 1000 Hz. A
Motion Capture System (MCS) recorded the ground truth data with millimetre
accuracy at 250 Hz.

Figure 4.2: The $z$ component of the measured specific force
$\tilde{a}_{z}^{b}$ (top), and the estimated ground reaction forces
$\hat{f}_{z}^{b}$ (bottom), in the body frame $\mathcal{B}$ of HyQ during a
trotting experiment. The highlighted regions show when the given foot is in
stance, and the feet are denoted as left-front (LF), right-front (RF), left-
hind (LH), and right-hind (RH).

The experiments confirmed our original hypothesis that soft terrain negatively
impacts state estimation and also allowed us to investigate why. It is
important to note that rigid versus soft terrain had no impact on the attitude
estimation. For space reasons, all attitude plots have been omitted.

The first distinct difference between soft and rigid terrain is the specific
force measurement of the body as seen in Fig. 4.2. On the rigid terrain there
are large impacts and then vibrations every time a foot touches down. Whereas
the soft terrain damped out these vibrations. Next, on the soft terrain more
prolonged periods of positive and negative acceleration can be seen. This
acceleration can also be seen in the plots of the GRFs in Fig. 4.2 where the
GRFs on the soft terrain are more continuous when compared to the rigid
terrain. In other words, there are longer loading and unloading phases.

The most important differences between soft and rigid terrain are seen in the
velocity and position estimates as shown in Fig. 4.3. We can see that the leg
odometry has large erroneous peaks in $z$ velocity at both touch-down and
lift-off. These peaks in velocities can then be seen in the position estimates
as a drift. On the other hand, the $x$ and $y$ position estimates are quite
accurate and only have a slow drift.

Figure 4.3:  The estimated trunk position $\hat{x}^{n}$ (top), and the
estimated trunk velocity $\hat{v}^{n}$ (middle and bottom), in the navigation
frame $\mathcal{N}$ of HyQ during a trotting experiment using sensor fusion
(purple) versus the raw leg odometry (green), and the motion capture system
(blue). The first two rows show the full experiment ($0\leq t\leq 300$) s and
the bottom row is zoomed in ($30\leq t\leq 32$) s.

In the figures, we can also see multiple of the state estimators assumptions
being broken. First, there does not exist a constant $\epsilon$ that can
describe when the foot is in contact with the ground, which is contradicting
Assumption 4.1. The contact $\epsilon$ is no longer binary (i.e.,
supporting/not-supporting the weight of the robot), but the contact is now a
continuous value with varying amounts of the robot’s weight being supported
and sometimes even pushed. When trying to use the previous simple model, the
contact ignores a large portion of the loading and unloading phase.
Furthermore, it often chatters rapidly between contact/non-contact when the
force is close to $\epsilon$. Second, the foot is moving for almost the entire
contact (i.e., non-zero acceleration) on soft terrain as shown in Fig. 4.2.
This contradicts Assumption 4.2 that the foot velocity is zero when in
contact. Third, (4.8) is broken. It assumes that all of the velocity (and all
of the acceleration) is a result of the GRFs, but not all of the acceleration
due to gravity is being accounted for. Hence, the robot appears to drift up
and away from the ground.

There are a few simple ways to try to improve the estimates of this or other
similar state estimators. The first is to tune $\epsilon$ in (4.3). By
increasing $\epsilon$ there would be less erroneous velocity, but in doing so
it would also ignore part of the leg odometry. In general, on a planar
surface, a reduced drift in the $z$ direction comes at the cost of an
increased error in the $x$ and $y$ directions. A second method could be to
have an adaptive velocity bias for the leg odometry. However, the bias is not
constant and it depends on both the gait and the terrain. Thus, the problem of
estimating the body velocity of the robot using leg odometry remains open.

#### 4.5 Conclusions

In this letter, we present an experimental validation and a formal study on
the influence of soft terrain on state estimation for legged robots. We
utilized a state-of-the-art state estimator that fuses IMU measurements with
leg odometry. We experimentally analyzed the differences between soft and
rigid terrain using our state estimator and a dataset of the HyQ robot. That
said, we report three main outcomes. First, we showed that soft terrain
results in a larger drift in the position estimates, and larger errors in the
velocity estimates compared to rigid terrain. These problems are caused by the
broken legged odometry contact assumptions on soft terrain. Second, we also
showed that over soft terrain, the contact with the terrain is no longer
binary and it often chatters rapidly between contact and non-contact. Third,
we showed that soft terrain affects many states besides the robot pose. This
includes the contact state and the GRFs which are essential for the control of
legged robots. Future works include extending the state estimator to
incorporate the terrain impedance in the leg odometry model. Additionally,
further datasets will be recorded to investigate the long-term drift in the
forward and lateral directions.

##  Part II Exteroceptive Terrain-Aware Locomotion

###  Chapter 5 ViTAL:
Vision-Based Terrain-Aware Locomotion
for Legged Robots

© 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.

Abstract. This work is on vision-based planning strategies for legged robots
that separate locomotion planning into foothold selection and pose adaptation.
Current pose adaptation strategies optimize the robot’s body pose relative to
given footholds. If these footholds are not reached, the robot may end up in a
state with no reachable safe footholds. Therefore, we present a Vision-Based
Terrain-Aware Locomotion (ViTAL) strategy that consists of novel pose
adaptation and foothold selection algorithms. ViTAL introduces a different
paradigm in pose adaptation that does not optimize the body pose relative to
given footholds, but the body pose that maximizes the chances of the legs in
reaching safe footholds. ViTAL plans footholds and poses based on skills that
characterize the robot’s capabilities and its terrain-awareness. We use the 90
kg HyQ and 140 kg HyQReal quadruped robots to validate ViTAL, and show that
they are able to climb various obstacles including stairs, gaps, and rough
terrains at different speeds and gaits. We compare ViTAL with a baseline
strategy that selects the robot pose based on given selected footholds, and
show that ViTAL outperforms the baseline.

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

#### 5.1 Introduction

Legged robots have shown remarkable agile capabilities in academia [85, 86,
74, 87, 75, 88] and industry [89, 90, 91]. Yet, to accomplish breakthroughs in
dynamic whole-body locomotion, and to robustly traverse unexplored
environments, legged robots have to be terrain aware. Terrain-Aware Locomotion
(TAL) implies that the robot is capable of taking decisions based on the
terrain [92]. The decisions can be in planning, control, or in state
estimation, and the terrain may vary in its geometry and physical properties
[93, 3, 10, 21, 94, 2, 95, 20, 13]. TAL allows the robot to use its on-board
sensors to perceive its surroundings and act accordingly. This work is on
vision-based TAL planning strategies that plan the robot’s motion (body and
feet) based on the terrain information that is acquired using vision (see Fig.
5.1).

Figure 5.1: The HyQ and HyQReal quadruped robots climbing stairs using ViTAL.

##### 5.1.1 Related Work - Vision-Based Locomotion Planning

Vision-based locomotion planning can either be coupled or decoupled. The
coupled approach jointly plans the body pose and footholds in a single
algorithm. The decoupled approach independently plans the body pose and
footholds in separate algorithms. The challenge in the coupled approach is
that it is computationally expensive to solve in real-time. Because of this,
the decoupled approach tends to be more practical since the high-dimensional
planning problem is split into multiple low-dimensional problems. This also
makes the locomotion planning problem more tractable. However, this raises an
issue with the decoupled approach because the plans may conflict with each
other since they are planned separately. Note that both approaches could be
solved using optimization, learning, or heuristic methods.

Trajectory Optimization (TO) is one way to deal with coupled vision-based
locomotion planning. By casting locomotion planning as an optimal control
problem, TO methods can optimize the robot’s motion while taking into account
the terrain information [96, 97, 98, 99, 100]. The locomotion planner can
generate trajectories that prevent the robot from slipping or colliding with
the terrain by encoding the terrain’s shape and friction properties in the
optimization problem [100]. TO methods can also include a model of the terrain
as a cost map in the optimization problem, and generate the robot’s
trajectories based on that [101]. TO methods provide guarantees on the
optimality and feasibility of the devised motions, albeit being
computationally expensive; performing these optimizations in real-time is
still a challenge. To overcome this issue, TO approaches often implement
hierarchical (decoupled) approaches. Instead of decoupling the plan into body
pose and footholds, the hierarchical approaches decouple the plan into short
and long-horizon plans [102, 103]. Additionally, other work relies on varying
the model complexity to overcome the computational issue with TO [104].

Reinforcement Learning (RL) methods mitigate the computational burden of TO
methods by training function approximators that learn the locomotion plan
[105, 106, 107, 108, 109, 110, 111]. Once trained, an RL policy can generate
body pose and foothold sequences based on proprioceptive and/or visual
information. Yet, RL methods may require tedious learning (large amounts of
data and training time) given its high-dimensional state representations.

As explained earlier, decoupled locomotion planning can mitigate the problems
of TO and RL by separating the locomotion plan into feet planning and body
planning [112, 113, 114, 115, 116, 117]. Thus, one can develop a more refined
and tractable algorithm for every module separately. In this work, planning
the feet motion (foothold locations) is called foothold selection, and
planning the body motion is called pose adaptation.

##### 5.1.2 Related Work - Foothold Selection and Pose Adaptation

Foothold selection strategies choose the best footholds based on the terrain
information and the robot’s capabilities. Early work on foothold selection was
presented by Kolter et al. [15] and Kalakrishnan et al. [16] where both
approaches relied on motion capture systems and an expert user to select
(label) the footholds. These works were then extended in [17] using
unsupervised learning, on-board sensors, and considered the terrain
information such as the terrain roughness (to avoid edges and corners) and
friction (to avoid slippage). Then, Barasuol et al. [18] extended the
aforementioned work by selecting footholds that not only considers the terrain
morphology, but also considering leg collisions with the terrain. Further
improvements in foothold selection strategies added other evaluation criteria
such as the robot’s kinematic limits. These strategies use optimization [113,
118, 119], supervised learning [114, 120, 121], RL [85, 95], or heuristic
[122] methods.

Similar to foothold selection, pose adaptation strategies optimize the robot’s
body pose based on the terrain information and the robot’s capabilities. An
early work on vision-based pose adaptation was presented in [112]. The goal
was to find the optimal pose that maximizes the reachability of given selected
footholds, avoid collisions with the terrain, and maintain static stability.
The given footholds are based on a foothold selection algorithm that considers
the terrain geometry. Another approach was presented in [123] that finds the
optimal body elevation and inclination given the selected footholds, and the
robot location in the map. The pose optimizer maximizes different margins that
increase the kinematic reachability of the legs and static stability, and
avoids terrain collisions. This approach was then extended in [124] with an
improved version of the kinematic margins. A similar approach was presented in
[113] where the goal was to find an optimal pose that can maximize the
reachability of given selected footholds. The reachability term is accounted
for in the cost function of the optimizer by penalizing the difference between
the default foothold position and the selected one. The work in [115] builds
on top of the pose optimizer of [113] to adapt the pose of the robot in
confined spaces using 3D terrain maps. This is done using a hierarchical
approach that first samples body poses that allows the robot to navigate
through confined spaces, then smooths these poses using a gradient descent
method that is then augmented with the pose optimizer of [113]. The work
presented in [125] generates vision-based pose references that also rely on
given selected footholds to estimate the orientation of the terrain and send
it as a pose reference. Alongside the orientation reference, the body height
reference is set at a constant vertical distance (parallel to gravity) from
the center of the approximated plane that fits through the selected footholds.

The aforementioned pose adaptation strategies focus on finding one optimal
solution based on given footholds; footholds have to be first selected and
given to the optimizer. Despite selecting footholds that are safe, there are
no guarantees on what would happen during execution if the footholds are not
reached or if the robot deviates from its planned motion. If any of these
cases happen, the robot might end up in a pose where no safe footholds can be
reached. This would in turn compromise the safety and performance of the
robot. Even if the strategy can re-plan, reaching a safe pose might not be
possible if the robot is already in an unsafe state.

##### 5.1.3 Proposed Approach

We propose Vision-Based Terrain-Aware Locomotion (ViTAL), an online whole-body
locomotion planning strategy that consists of a foothold selection and a pose
adaptation algorithm. The foothold selection algorithm used in this work is an
extension of the Vision-Based Foothold Adaptation (VFA) algorithm of the
previous work done by Villarreal et al. [114] and Esteban et al. [121]. Most
importantly, we propose a novel Vision-Based Pose Adaptation (VPA) algorithm
that introduces a different paradigm to overcome the drawbacks of the state-
of-the-art pose adaptation methods. Instead of finding body poses that are
optimal for given footholds, we propose finding body poses that maximize the
chances of reaching safe footholds in the first place. Hence, we are
interested in putting the robot in a state in which if it deviates from its
planned motion, the robot remains around a set of footholds that are still
reachable and safe. The notion of safety emerges from skills that characterize
the robot’s capabilities.

ViTAL plans footholds and body poses by sharing the same robot skills (both
for the VPA and the VFA). These skills characterize what the robot is capable
of doing. The skills include, but are not limited to: the robot’s ability to
avoid edges, corners, or gaps (terrain roughness), the robot’s ability to
remain within the workspace of the legs during the swing and stance phases
(kinematic limits), and the robot’s ability to avoid colliding with the
terrain (leg collision). These skills are denoted by Foothold Evaluation
Criteria (FEC). Evaluating the FEC is usually computationally expensive. Thus,
to incorporate the FEC in ViTAL, we rely on approximating them with
Convolutional Neural Networks (CNNs) that are trained via supervised learning.
This allows us to continuously adapt both the footholds and the body pose. The
VFA and the VPA are decoupled and can run at a different update rate. However,
they are non-hierarchical, they run in parallel, and they share the same
knowledge of the robot skills (the FEC). By that, we overcome the limitations
that result from hierarchical planners as mentioned in [115], where high-level
plans may conflict with the low-level ones causing a different robot behavior.

The VPA utilizes the FEC to approximate a function that provides the number of
safe footholds for the legs. Using this function, we cast a pose optimizer
which solves a non-linear optimization problem that maximizes the number of
safe footholds for all the legs subject to constraints added to the robot
pose. The pose optimizer is a key element in the VPA since it adds safety
layers and constraints to the learning part of our approach. This makes our
approach more tractable which mitigates the issues that might arise from end-
to-end policies in RL methods.

##### 5.1.4 Contributions

ViTAL mitigates the above-mentioned conflicts that exist in other decoupled
planners [115, 118, 117, 113]. This is because both the VPA and the VFA share
the same skills encoded in the FEC. In other words, the VPA and the VFA will
not plan body poses and footholds that may conflict with each other because
both planners share the same logic. In this work, the formulation of the VPA
allows ViTAL to reason about the leg’s capabilities and the terrain
information. However, the formulation of the VPA could be further augmented by
other body-specific skills. For instance, the VPA could be reformulated to
reason about the body collisions with the environment similar to the work in
[117, 126]. The paradigm of the FEC can also be further augmented to consider
other skills. We envision that some skills are best encoded via heuristics
while others are well suited through optimization. For this reason, the FEC
can also handle optimization-based foothold objectives such as the ones in
[118].

Following the recent impressive results in RL-based locomotion controllers, we
envision ViTAL to be inserted as a module into such control frameworks. To
elaborate, current RL-based locomotion controllers [105, 107, 85, 109] are of
a single network; the RL framework is a single policy that maps the
observations (proprioceptive and exteroceptive) to the actions. This may be
challenging since it requires careful reward shaping, and generalizing to new
tasks or different sensors (observations) makes the problem harder [127]. For
this reason, and similar to Green et al. [127], we envision that ViTAL can be
utilized as a planner for RL controllers where the RL controller will act as a
reactive controller that then receives guided (planned) commands in a form of
optimal poses and footholds from ViTAL.

ViTAL differs from TO and optimization-based methods in several aspects. The
FEC is designed to independently evaluate every skill (criterion). Thus, one
criterion can be optimization-based while other could be using logic or
heuristics. Because of this, ViTAL is not restricted by solving an
optimization problem that handles all the skills at once. Another difference
between ViTAL and TO is in the way the body poses are optimized. In TO, the
optimization problem optimizes a single pose to follow a certain trajectory.
The VPA in ViTAL optimizes for the body poses that maximizes the chances of
the legs in reaching safe footholds. In other words, the VPA finds a body pose
that would put the robot in a configuration where the legs have the maximum
possible number of safe footholds. In fact, this paradigm that the VPA of
ViTAL introduces may be also encoded in TO. Additionally, TO often finds body
poses that considers the leg’s workspace, but to the best of the authors’
knowledge, there is no TO method that finds body poses that consider the legs’
collision with the terrain, and the feasibility of the swinging legs’
trajectory.

To that end, the contributions of this article are

  * •

ViTAL, an online vision-based locomotion planning strategy that simultaneously
plans body poses and footholds based on shared knowledge of robot skills (the
FEC).

  * •

An extension of our previous work on the VFA algorithm for foothold selection
that considers the robot’s body twist and the gait parameters.

  * •

A novel pose adaptation algorithm called the VPA that finds the body pose that
maximizes the number of safe footholds for the robot’s legs.

#### 5.2 Foothold Evaluation Criteria (FEC)

The FEC is main the building block for the VFA and the VPA. The FEC are sets
of skills that evaluate footholds within heightmaps. The skills include the
robot’s ability to assess the terrain’s geometry, avoid leg collisions, and
avoid reaching kinematic limits. The FEC can be model-based as in this work
and [112, 114], or using optimization techniques as in [113, 118]. The FEC of
this work extends the criteria used in our previous work [18, 114, 121].

The FEC takes a tuple $T$ as an input, evaluates it based on multiple
criteria, and outputs a boolean matrix $\mu_{\mathrm{safe}}$. The input tuple
$T$ is defined as

$T=(H,z_{h},v_{b},\alpha)$ (5.1)

where $H\in\mathbb{R}^{h_{x}\times{h_{y}}}$ is the heightmap of dimensions
$h_{x}$ and $h_{y}$, $z_{h}\in\mathbb{R}$ is the hip height of the leg (in the
world frame), ${v_{b}\in\mathbb{R}^{6}}$ is the base twist, and $\alpha$ are
the gait parameters (step length, step frequency, duty factor, and time
remaining till touchdown). The heightmap $H$ is extracted from the terrain
elevation map, and is oriented with respect to the horizontal frame of the
robot [128]. The horizontal frame coincides with the base frame of the robot,
and its $xy$-plane is perpendicular to the gravity vector. Each cell (pixel)
of $H$ denotes the terrain height that corresponds to the location of this
cell in the terrain map. Each cell of $H$ also corresponds to a candidate
foothold $p_{c}\in\mathbb{R}^{3}$ for the robot.

In this work, we only consider the following FEC: Terrain Roughness (TR), Leg
Collision (LC), Kinematic Feasibility (KF), and Foot Trajectory Collision
(FC). Each criterion $C$ outputs a boolean matrix $\mu_{C}$. Once all of the
criteria are evaluated, the final output $\mu_{\mathrm{safe}}$ is the element-
wise logical AND ($\wedge$) of all the criteria. The output matrix
$\mu_{\mathrm{safe}}\in\mathbb{R}^{h_{x}\times{h_{y}}}$ is a boolean matrix
with the same size as the input heightmap $H$. $\mu_{\mathrm{safe}}$ indicates
the candidate footholds (elements in the heightmap $H$) that are safe. An
element in the matrix $\mu_{\mathrm{safe}}$ that is true, corresponds to a
candidate foothold $p_{c}$ in the heightmap $H$ that is safe. The output of
the FEC is

$\mu_{\mathrm{safe}}=\mu_{\mathrm{TR}}\wedge\mu_{\mathrm{LC}}\wedge\mu_{\mathrm{KF}}\wedge\mu_{\mathrm{FC}}.$
(5.2)

An overview of the criteria used in this work is shown in Fig. 5.2(A) and is
detailed below.

Terrain Roughness (TR). This criterion checks edges or corners in the
heightmap that are unsafe for the robot to step on. For each candidate
foothold $p_{c}$ in $H$, we evaluate the mean and standard deviation of the
slope relative to its neighboring footholds, and put a threshold that defines
whether a $p_{c}$ is safe or not. Footholds above this threshold are
discarded.

Leg Collision (LC). This criterion selects footholds that do not result in leg
collision with the terrain during the entire gait cycle (from lift-off, during
swinging, touchdown and till the next lift-off). To do so, we create a
bounding region around the leg configuration that corresponds to the candidate
foothold $p_{c}$ and the current hip location. Then, we check if the bounding
region collides with the terrain (the heightmap) by measuring the closest
distance between them. If this distance is lower than a certain value, then
the candidate foothold is discarded.

Kinematic Feasibility (KF). This criterion selects footholds that are
kinematically feasible. It checks whether a candidate foothold $p_{c}$ will
result in a trajectory that remains within the workspace of the leg during the
entire gait cycle. To do so, we check if the candidate foothold $p_{c}$ is
within the workspace of the leg during touchdown and next lift-off. Also, we
check if the trajectory of the foot from the lift-off position $p_{lo}$ till
the touchdown position at the candidate foothold $p_{c}$ is within the
workspace of the leg. In the initial implementation in [114], this criterion
was only evaluated during touchdown. In this work, we consider this criterion
during the entire leg step-cycle.

Foot Trajectory Collision (FC). This criterion selects footholds that do not
result in foot trajectory collision with the terrain. It checks whether the
foot swing trajectory corresponding to a candidate foothold $p_{c}$ is going
to collide with the terrain or not. If the swing trajectory collides with the
terrain, the candidate foothold $p_{c}$ is discarded.

###### Remark 5.1

There are three main sources of uncertainty that can affect the foothold
placement [18]. These sources of uncertainty are due to trajectory tracking
errors, foothold prediction errors, and drifts in the map. To allow for a
degree of uncertainty, after computing $\mu_{\mathrm{safe}}$, candidate
footholds that are within a radius of unsafe footholds are also discarded.
This is similar to the erosion operation in image processing.

###### Remark 5.2

The initial implementation of the FEC in [114] only considered the heightmap
$H$ as an input; the other inputs of the tuple $T$ in (5.1) were kept
constant. This had a few disadvantages that we reported in [121] where we
extended the work of [114] by considering the linear body heading velocity. In
this work, we build upon that by considering the full body twist $v_{b}$ and
the gait parameters $\alpha$ as expressed by $T$ in (5.1).

Figure 5.2:  Overview of ViTAL. Illustrations are not to scale. (A) The
Foothold Evaluation Criteria (FEC): Leg Collision (LC), Kinematic Feasibility
(KF), and Foot Trajectory Collision (FC). (B) The Vision-based Foothold
Adaptation (VFA) pipeline. First, we extract the heightmap $H_{\mathrm{vfa}}$
around the nominal foothold $p_{n}$. Then, we evaluate the heightmap either
using the exact evaluation $g(T_{\mathrm{vfa}})$ or using the CNN as an
approximation $\hat{g}(T_{\mathrm{vfa}})$. Once the optimal foothold $p_{*}$
is selected, the swing trajectory is adjusted.

Figure 5.2:  Overview of ViTAL (continued). Illustrations are not to scale.
(C) The Vision-Based Pose Adaptation (VPA) pipeline. First, we extract the
heightmap $H_{\mathrm{vpa}}$ for all the legs. The heightmaps are centered
around the projection of the leg hip locations. Then, we evaluate the FEC to
compute $\bar{\mathcal{F}}$ for all the hip heights of all the legs (pose
evaluation). Then, we approximate a continuous function $\hat{\mathcal{F}}$
from $\bar{\mathcal{F}}$ (function approximation). The pose optimizer finds
the pose that maximizes $\hat{\mathcal{F}}$ for all of the legs (pose
optimization). (D) Our locomotion framework. ViTAL consists of the VPA and the
VFA algorithms. Both algorithms rely on the robot skills which we denote by
FEC. $\tau_{d}$ are the desired joint torques that are sent to the robot.

#### 5.3 Vision-Based Foothold Adaptation (VFA)

The VFA evaluates the FEC to select the optimal foothold for each leg [18,
114, 121]. The VFA has three main stages as shown in Fig. 5.2(B): heightmap
extraction, foothold evaluation, and trajectory adjustment.

Heightmap Extraction. Using the current robot states and gait parameters, we
estimate the touchdown position of the swinging foot in the world frame as
detailed in [114]. This is denoted as the nominal foothold
$p_{n}\in\mathbb{R}^{3}$. Then, we extract a heightmap $H_{\mathrm{vfa}}$ that
is centered around $p_{n}$.

Foothold Evaluation. After extracting the heightmap, we compute the optimal
foothold $p_{*}\in\mathbb{R}^{3}$ for each leg. We denote this by foothold
evaluation which is the mapping

$g(T_{\mathrm{vfa}}):T_{\mathrm{vfa}}\rightarrow p_{*}$ (5.3)

that takes an input tuple $T_{\mathrm{vfa}}$ that is defined as

$T_{\mathrm{vfa}}=(H_{\mathrm{vfa}},z_{h},v_{b},\alpha,p_{n}).$ (5.4)

Once we evaluate the FEC in (5.2), from all of the safe candidate footholds in
$\mu_{\mathrm{safe}}$, we select the optimal foothold $p_{*}$ as the one that
is closest to the nominal foothold $p_{n}$. The aim is to minimize the
deviation from the original trajectory and thus results in a less disturbed or
aggressive motion. An overview of the foothold evaluation stage is shown in
Fig. 5.3 where the tuple $T$ of the FEC in (5.1) is extracted from the VFA
tuple $T_{\mathrm{vfa}}$ in (5.4) to compute $\mu_{\mathrm{safe}}$. Then,
using $p_{n}$ and $\mu_{\mathrm{safe}}$, we extract $p_{*}$ as the safe
foothold that is closest to $p_{n}$.

Trajectory Adjustment. The leg’s swinging trajectory is adjusted once $p_{*}$
is computed.

Figure 5.3: Overview of the foothold evaluation stage in the VFA algorithm,
and the pose evaluation stage in the VPA algorithm.

###### Remark 5.3

To compute the foothold evaluation, one can directly apply the exact mapping
$g(T_{\mathrm{vfa}})$. Yet, computing the foothold evaluation leads to
evaluating the FEC which is generally computationally expensive. Thus, to
speed up the computation and to continuously run the VFA online, we train a
CNN to approximate the foothold evaluation $\hat{g}(T_{\mathrm{vfa}})$ using
supervised learning. Once trained, the VFA can then be executed online using
the CNN. The CNN architecture of the foothold evaluation is explained in
Section 6.1.

#### 5.4 Vision-Based Pose Adaptation (VPA)

The VPA generates pose references that maximize the chances of the legs to
reach safe footholds. This means that the robot pose has to be aware of what
the legs are capable of and adapt accordingly. Therefore, the goal of the VPA
is to adapt the robot pose based on the same set of skills in the FEC used by
the VFA.

##### 5.4.1 Definitions and Notations

Number of Safe Footholds. As explained earlier, the FEC takes a tuple $T$ as
an input and outputs the matrix $\mu_{\mathrm{safe}}$. Based on that, let us
define the Number of Safe Footholds ($\mathrm{n_{sf}}$)

$\mathrm{n}_{\mathrm{sf}}:=\mathrm{cardinal}(\\{e\in\mu_{\mathrm{safe}}:e=1\\})$
(5.5)

as the number of true elements in the boolean matrix $\mu_{\mathrm{safe}}$.

Set of Safe Footholds. Consider a set of tuples $\mathcal{T}$ where each
element $T_{i}\in\mathcal{T}$ is a tuple defined as

$T_{i}=(H,z_{h_{i}},v_{b},\alpha)$ (5.6)

and $z_{h_{i}}\in\mathcal{Z}$ is a hip height element in the set of hip
heights $\mathcal{Z}$ (in the world frame). All the tuple elements
$T_{i}\in\mathcal{T}$ share the same heightmap $H$, body twist $v_{b}$ and
gait parameters $\alpha$.

Evaluating the FEC in (5.2) for every $T_{i}\in\mathcal{T}$ that corresponds
to $z_{h_{i}}\in\mathcal{Z}$, and computing the cardinal in (5.5) yields
$\mathrm{n}_{\mathrm{sf},i}$ for every $T_{i}$. This yields the Set of Safe
Footholds ($\mathcal{F}$) which is a set containing the number of safe
footholds $\mathrm{n}_{\mathrm{sf}}$ that are evaluated based on the FEC given
the set of tuples $\mathcal{T}$ that corresponds to the set of hip heights
$\mathcal{Z}$ but shares the same heightmap $H$, body twist $v_{b}$ and gait
parameters $\alpha$.

##### 5.4.2 From the Set of Safe Footholds to Pose Evaluation

The set of safe footholds $\mathcal{F}$ is one of the building blocks of the
VPA. To compute $\mathcal{F}$, we compute the input tuple $T_{\mathrm{vpa}}$

$T_{\mathrm{vpa}}=(H_{\mathrm{vpa}},v_{b},\alpha)$ (5.7)

that we then augment with the hip heights $z_{h_{i}}$ in the hip heights set
$\mathcal{Z}$ yielding the set of tuples $\mathcal{T}$. Then, we evaluate the
FEC in (5.2) for every $T_{i}\in\mathcal{T}$, and computing the cardinal in
(5.5). This can be expressed by the mapping

${g_{\mathrm{vpa}}(T_{\mathrm{vpa}})~{}:~{}T_{\mathrm{vpa}}\rightarrow\mathcal{F}}$
(5.8)

which is referred to as pose evaluation. We can express $\mathcal{F}$ as

$\mathcal{F}=\\{\mathrm{n}_{\mathrm{sf},i}=g_{\mathrm{vpa},i}(T_{i})~{}\forall
T_{i}\in\mathcal{T}\\}.$ (5.9)

###### Remark 5.4

Since $\mathcal{Z}$ is an infinite continuous set, so is $\mathcal{F}$ which
is not numerically feasible to compute. Hence, we sample a finite set
$\bar{\mathcal{Z}}$ of $N_{z_{h}}$ samples of hip heights that results in a
finite set of safe footholds $\bar{\mathcal{F}}$. To use the set of safe
footholds in an optimization problem, we need a continuous function. Thus,
after we compute $\bar{\mathcal{F}}$, we estimate a continuous function
$\hat{\mathcal{F}}$ as explained next.

An overview of the pose evaluation is shown in Fig. 5.3 where the tuple
$T_{\mathrm{vpa}}$ in (5.7) is augmented with the hip heights $z_{h_{i}}$ from
$\bar{\mathcal{Z}}$ to construct the FEC tuples $T_{i}$ in (5.6). For every
tuple $T_{i}$, we evaluate the FEC using (5.2) and compute
$\mathrm{n}_{\mathrm{sf},i}$ in (5.5) using the mapping in (5.8). Finally, the
set $\bar{\mathcal{F}}$ includes all the elements $\mathrm{n}_{\mathrm{sf},i}$
as in (5.9).

##### 5.4.3 Vision-based Pose Adaptation (VPA) Formulation

The VPA has four main stages as shown in Fig. 5.2(C). First, heightmap
extraction that is similar to the VFA. Second, pose evaluation where we
compute $\bar{\mathcal{F}}$. Third, function approximation where we estimate
$\hat{\mathcal{F}}$ from $\bar{\mathcal{F}}$. Fourth, pose optimization where
the optimal body pose is computed.

Heightmap Extraction. We extract one heightmap $H_{\mathrm{vpa}}$ per leg that
is centered around the projection of the leg’s hip location in the terrain map
($\mathrm{proj.}(p_{h})$ instead of $p_{n}$).

Pose Evaluation. After extracting the heightmaps, we compute
$\bar{\mathcal{F}}$ from the mapping in (5.8) of the pose evaluation. In the
pose evaluation, the FEC are evaluated for all hip heights in
$\bar{\mathcal{Z}}$ given the input tuple $T_{\mathrm{vpa}}$ as shown in Fig.
5.3.

Function Approximation. In this stage, we estimate the continuous function
$\hat{\mathcal{F}}$ from $\bar{\mathcal{F}}$, as explained in Remark 5.4. This
is done by training a parameterized model of the inputs
$T_{i}\in\bar{\mathcal{T}}$ and the outputs
$\mathrm{n}_{\mathrm{sf},i}\in\bar{\mathcal{F}}$. The result is the function
(model) $\hat{\mathcal{F}}$ that is parameterized by the model parameters $w$.
The function approximation is detailed later in this section.

Pose Optimization. Evaluating the FEC and approximating it with the function
$\hat{\mathcal{F}}$, introduces a metric that represents the possible number
of safe footholds for every leg. Based on this, the goal of the pose optimizer
is to find the optimal pose that will maximize the number of safe footholds
for every leg (maximize $\hat{\mathcal{F}}$) while ensuring robustness. The
pose optimizer is detailed later in this section.

###### Remark 5.5

Similar to Remark 5.3, one can directly apply the exact evaluation
$g_{\mathrm{vpa}}(T_{\mathrm{vpa}})$ for a given $T_{\mathrm{vpa}}$. Yet,
since this is computationally expensive, we rely on approximating the
evaluation $\hat{g}_{\mathrm{vpa}}(T_{\mathrm{vpa}})$ using a CNN. In fact,
the learning part is applied to both the pose evaluation and the function
approximation. This means that the pose optimization is running online,
outside the CNN. The CNN architecture of the pose evaluation is explained in
Section 6.1.

##### 5.4.4 Function Approximation

The goal of the function approximation is to approximate the set of safe
footholds $\mathcal{F}$ from the discrete set $\bar{\mathcal{F}}$ computed in
the pose evaluation stage. This is done to provide the pose optimizer with a
continuous function. Given a dataset $(\bar{\mathcal{Z}},\bar{\mathcal{F}})$
of hip heights $z_{h_{i}}\in\bar{\mathcal{Z}}$ and number of safe footholds
${\mathrm{n}_{\mathrm{sf},i}\in\bar{\mathcal{Z}}}$, the function approximation
estimates a function $\hat{\mathcal{F}}(z_{h_{i}},w)$ that is parameterized by
the weights $w$. Once the weights $w$ are computed, the function estimate
$\hat{\mathcal{F}}(z_{h_{i}},w)$ is then reconstructed and sent to the pose
optimizer.

It is important to choose a function $\hat{\mathcal{F}}$ that can accurately
represent the nature of the number of safe footholds. The number of safe
footholds approaches zero when the hip heights approach $0$ or $\infty$. Thus,
we want a function that fades to zero at the extremes (Gaussian-like
functions), and captures any asymmetry or flatness in the distribution. Hence,
we use radial basis functions of Gaussians. With that in mind, we are looking
for the weights $w$

$w=\text{arg}~{}\text{min}~{}S(w)$ (5.10)

that minimize the cost $S(w)$

$S(w)=\sum_{i=1}^{N_{h}}(\mathrm{n}_{\mathrm{sf},i}-\hat{\mathcal{F}}(z_{h_{i}},w))^{2}$
(5.11)

which is the sum of the squared residuals of $\mathrm{n}_{\mathrm{sf},i}$ and
$\hat{\mathcal{F}}(z_{h_{i}},w)$. $N_{h}$ is the number of samples (the number
of the finite set of hip heights). The function
$\hat{\mathcal{F}}(z_{h_{i}},w)$ is the regression model (the approximation of
$\mathcal{F}$) that is parameterized by $w$. The function
$\hat{\mathcal{F}}(z_{h_{i}},w)$ is the weighted sum of the basis functions

$\hat{\mathcal{F}}(z_{h_{i}},w)=\sum_{e=1}^{E}w_{e}\cdot
g(z_{h_{i}},\Sigma_{e},c_{e})$ (5.12)

where $w\in\mathbb{R}^{E}$, and $E$ is the number of basis functions. The
basis function is a radial basis function of Gaussian functions

$g(z_{h_{i}},\Sigma_{e},c_{e})=\text{exp}(-0.5(z_{h_{i}}-c_{e})^{T}\Sigma_{e}^{-1}(z_{h_{i}}-c_{e}))$
(5.13)

where $\Sigma_{e}$ and $c_{e}$ are the parameters of the Gaussian function.
Since the function model in (5.12) is linear in the parameters, the weights of
the function approximation can be solved analytically using least squares. In
this work, we keep the parameters of the Gaussians ($\Sigma$ and $c$) fixed.
Hence, the function $\hat{\mathcal{F}}$ is only parameterized by $w$. For more
information on regression with radial basis functions, please refer to [65]
and Section 6.2.

##### 5.4.5 Pose Optimization

The pose optimizer finds the robot’s body pose $u$ that maximizes the number
of safe footholds for all the legs. This is casted as a non-linear
optimization problem. The notion of safe footholds is provided by the function
$\hat{\mathcal{F}}(z_{h},w)$ that maps a hip height $z_{h}$ to a number of
safe foothold $\mathrm{n_{sf}}$, and is parameterized by $w$. Since the pose
optimizer is solving for the body pose $u$, the function
$\hat{\mathcal{F}}(z_{h})$ should be encoded using the body pose rather than
the hip heights (${\hat{\mathcal{F}}=\hat{\mathcal{F}}(z_{h}(u))}$). This is
done by estimating the hip height as a function of the body pose
(${z_{h}=z_{h}(u)}$) as shown in Section 6.3.

##### 5.4.6 Single-Horizon Pose Optimization

The pose optimization problem is formulated as

$\displaystyle\underset{u=[z_{b},\beta,\gamma]}{\text{maximize~{}}}$
$\displaystyle\mathcal{C}(\hat{\mathcal{F}}_{l}(z_{h_{l}}(z_{b},\beta,\gamma)))~{}~{}\forall
l\in N_{l}$ (5.14) subject to $\displaystyle u_{\min}\leq u\leq u_{\max}$
(5.15) $\displaystyle\Delta u_{\min}\leq\Delta u\leq\Delta u_{\max}$ (5.16)

where $u=[z_{b},\beta,\gamma]\in\mathbb{R}^{3}$ are the decision variables
(robot body pose) consisting of the robot height, roll and pitch,
respectively, $\mathcal{C}$ is the cost function, $\hat{\mathcal{F}}_{l}$ is
$\hat{\mathcal{F}}$ for every leg $l$ where $N_{l}=4$ is the number of legs,
$z_{h_{l}}\in\mathbb{R}$ is the hip height of the leg $l$, and $u_{\min}$ and
$u_{\max}$ are the lower and upper bounds of the decision variables,
respectively. $\Delta u=u-u_{k-1}$ is the numerical difference of $u$ where
$u_{k-1}$ is the output of $u$ at the previous instant, and $\Delta u_{\min}$
and $\Delta u_{\max}$ are the lower and upper bounds of $\Delta u$,
respectively. We can re-write (5.16) as

$\Delta u_{\min}+u_{k-1}\leq u\leq\Delta u_{\max}+u_{k-1}.$ (5.17)

The cost function in (5.14) maximizes $\hat{\mathcal{F}}$ for all of the legs.
We designed several types of cost functions as detailed next. The constraints
in (5.15) and (5.16) ensure that the decision variables and their variations
are bounded.

##### 5.4.7 Cost Functions

A standard cost function can be the sum of the squares of
$\hat{\mathcal{F}}_{l}$ for all of the legs

$\mathcal{C}_{\mathrm{sum}}=\sum_{l=1}^{N_{l}=4}\|\hat{\mathcal{F}}_{l}(z_{h_{l}})\|^{2}_{Q}$
(5.18)

where another option could be the product of the squares of
$\hat{\mathcal{F}}_{l}$ for all of the legs

$\mathcal{C}_{\mathrm{prod}}=\prod_{l=1}^{N_{l}=4}\|\hat{\mathcal{F}}_{l}(z_{h_{l}})\|^{2}_{Q}.$
(5.19)

The key difference between an additive cost $\mathcal{C}_{\mathrm{sum}}$ and a
multiplicative cost $\mathcal{C}_{\mathrm{prod}}$ is that the latter puts
equal weighting for each $\hat{\mathcal{F}}_{l}$. This is important since we
do not want the optimizer to find a pose that maximizes $\hat{\mathcal{F}}$
for one leg while compromising the other leg(s). One can also define the cost

$\mathcal{C}_{\mathrm{int}}=\sum_{l=1}^{N_{l}=4}\|\int_{z_{h_{l}}-m}^{z_{h_{l}}+m}\hat{\mathcal{F}}_{l}(z_{h_{l}})~{}dz_{h_{l}}~{}\|^{2}_{Q}$
(5.20)

which is the sum of squared integrals that can be numerically approximated as

$\int_{z_{h_{l}}-m}^{z_{h_{l}}+m}\hat{\mathcal{F}}_{l}(z_{h_{l}})~{}dz_{h_{l}}\approx
m\cdot(\hat{\mathcal{F}}_{l}(z_{h_{l}}-m)+\hat{\mathcal{F}}_{l}(z_{h_{l}}+m))$
(5.21)

yielding

$\mathcal{C}_{\mathrm{int}}=\sum_{l=1}^{N_{l}=4}\|m\cdot(\hat{\mathcal{F}}_{l}(z_{h_{l}}-m)+\hat{\mathcal{F}}_{l}(z_{h_{l}}+m))\|^{2}_{Q}.$
(5.22)

In this cost option, we do not find the pose that maximizes
$\hat{\mathcal{F}}$. Instead, we want to find the pose that maximizes the area
around $\hat{\mathcal{F}}$ that is defined by the margin $m$. Using
$\mathcal{C}_{\mathrm{int}}$ is important since it adds robustness in case
there is any error in the pose tracking during execution. Because of possible
tracking errors during execution, the robot might end up in the pose $u^{*}\pm
m$ instead of $u^{*}$. If we use $\mathcal{C}_{\mathrm{int}}$ as a cost
function, the optimizer will find poses that maximizes the number of safe
footholds not just for $u^{*}$ but within a vicinity of $m$. More details on
the use of $\mathcal{C}_{\mathrm{int}}$ as a cost function in the pose
optimization of the VPA can be found in Section 6.4.

##### 5.4.8 Receding-Horizon Pose Optimization

Adapting the robot’s pose during dynamic locomotion requires reasoning about
what is ahead of the robot: the robot should not just consider its current
state but also future ones. For that, we extend the pose optimizer to consider
the current and future states of the robot in a receding horizon manner. To
formulate the receding horizon pose optimizer, instead of considering
$\hat{\mathcal{F}}_{l}~{}\forall~{}{l\in N_{l}}$ in the single horizon case,
the pose optimizer will consider $\hat{\mathcal{F}}_{l,j}~{}\forall~{}{l\in
N_{l}},~{}{j\in N_{h}}$ where $N_{h}$ is the receding horizon number. We
compute $\hat{\mathcal{F}}_{l,j}$ in the same way explained in the pose
evaluation stage. More details on computing $\hat{\mathcal{F}}_{l,j}$ can be
found in Section 6.5.

The receding horizon pose optimization problem is

$\displaystyle\underset{u=[u_{1}^{T},\cdots,u_{N_{h}}^{T}]}{\text{maximize}}$
$\displaystyle\sum_{j=1}^{N_{h}}\mathcal{C}_{j}(\hat{\mathcal{F}}_{l,j}(z_{h_{l,j}}(u_{j})))$
(5.23) $\displaystyle+$ $\displaystyle\sum_{j=1}^{N_{h}-1}\|u_{j}-u_{j+1}\|$
$\displaystyle\forall l\in N_{l},~{}j\in N_{h}$ subject to $\displaystyle
u_{\min}\leq u\leq u_{\max}$ (5.24) $\displaystyle\Delta u_{\min}\leq\Delta
u\leq\Delta u_{\max}$ (5.25)

where
$u=[u_{1}^{T},\cdots,u_{j}^{T},\cdots,u_{N_{h}}^{T}]\in\mathbb{R}^{3N_{h}}$
are the decision variables during the entire receding horizon $N_{h}$. Each
variable $u_{j}=[z_{b,j},\beta_{j},\gamma_{j}]\in\mathbb{R}^{3}$ is the
optimal pose of the horizon $j$.

The first term in (5.23) is the sum of the cost functions $\mathcal{C}_{j}$
during the entire horizon ($\forall j\in N_{h}$). The cost $\mathcal{C}_{j}$
can be any of the aforementioned cost functions. The second term in (5.23)
penalizes the deviation between two consecutive optimal poses within the
receding horizon ($u_{j}$ and $u_{j+1}$). The second term is added so that
each optimal pose $u_{j}$ is also taking into account the optimal pose of the
upcoming sequence $u_{j+1}$ (to connect the solutions in a smooth way).
Similar to the single horizon pose optimizer, $u_{\min}$ and $u_{\max}$ are
the lower and upper bounds of the decision variables, respectively.
Furthermore, $\Delta u$ denotes the numerical difference of $u$, while $\Delta
u_{\min}$ and $\Delta u_{\max}$ are the lower and upper bounds of $\Delta u$,
respectively. Note that the constraints of the single horizon and the receding
horizon are of different dimensions.

#### 5.5 System Overview

Our locomotion framework that is shown in Fig. 5.2(D) is based on the Reactive
Controller Framework (RCF) [128]. ViTAL complements the RCF with an
exteroceptive terrain-aware layer composed of the VFA and the VPA. ViTAL takes
the robot states, the terrain map and user commands as inputs, and sends out
the selected footholds and body pose to the RCF (perceptive) layer. The RCF
takes the robot states and the references from ViTAL, and uses them inside a
motion generation and a motion control block. The motion generation block
generates the trajectories of the leg and the body, and adjusts them with the
reflexes from [128, 7]. The legs and body references from the motion
generation block are sent to the motion control block. The motion control
block consists of a Whole-Body Control (WBC) [1] that generates desired
torques that are tracked via a low-level torque controller [129], and sent to
the robot’s joints. The framework also includes a state estimation block that
feeds back the robot states to each of the aforementioned layers [130]. More
implementation details on ViTAL and the entire framework is in Section 6.6.

We demonstrate ViTAL on the 90 kg HyQ and the 140 kg HyQReal quadruped robots.
Each leg of the two robots has 3 degrees of freedom (3 actuated joints). The
torques and angles of the 12 joints of both robots are directly measured. The
bodies of HyQ and HyQReal have a tactical-grade Inertial Measurement Unit
(IMU) (KVH 1775). More information on HyQ and HyQReal can be found in [66],
and [74] respectively.

We noticed a significant drift in the states of the robots in experiment. To
tackle this issue, the state estimator fused the data from a motion capture
system and the IMU. This reduced the drift in the base states of the robots
albeit not eliminating it completely. Improving the state estimation is an
ongoing work and is out of the scope of this article. We used the grid map
interface [131] to get the terrain map in simulation. Due to the issues with
state estimation on the real robots, we constructed the grid map before the
experiments, and used the motion capture system to locate the map with respect
to the robot.

#### 5.6 Results

We evaluate ViTAL on HyQ and HyQReal. We consider all the FEC mentioned
earlier for the VFA and the VPA. We use the receding horizon pose optimizer of
(5.25) and the sum of squared integral of (5.22). We choose stair climbing as
an application for ViTAL. Climbing stairs is challenging for HyQ due to its
limited leg workspace in the sagittal plane. Videos associated with the
upcoming results can be found in the supplementary materials and [132].
Finally, an analysis of the accuracy of the CNNs and the computational time of
ViTAL can be found in Section 6.7 and Section 6.8, respectively.

##### 5.6.1 Climbing Stairs (Simulation)

Figure 5.4:  HyQ climbing stairs in simulation. (A) The full scenario. (B) The
robot pitches up to allow for safe footholds for the front legs. (C) The robot
lifts up the hind hips to avoid hind leg collisions with the step. (D) The
robot pitches down to allow for safe footholds for the front legs. (E) The
robot lowers the hind hips to allow for safe footholds for the legs when
stepping down.  Figure 5.5: Climbing Stairs: A More Complex Scenario. Top:
Overlayed screenshots of HyQ climbing stairs. Bottom: the optimal height and
corresponding pitch (presented by the arrows) and the foot trajectories of LF
and RH legs.

We carried out multiple simulations where HyQ is climbing the stairs shown in
Fig. 5.4. Each step has a rise of 10 cm, and a go of 25 cm. HyQ is commanded
to trot with a desired forward velocity of 0.2 m/s using the VPA and the VFA.
Figure 5.4 shows screenshots of one simulation run, and Video 1 shows three
simulation runs.

Figure 5.4 shows the ability of the VPA in adapting the robot pose to increase
the chances of the legs to succeed in finding a safe foothold. In Fig. 5.4(B),
HyQ raised its body and pitched upwards so that the front hips are raised to
increase the workspace of the front legs when stepping up. In Fig. 5.4(C), HyQ
raised its body and pitched downwards so that the hind hips are raised. This
is done for two reasons. First, to have a larger clearance between the hind
legs and the obstacle, and thus avoiding leg collision with the edge of the
stairs. Second, to increase the workspace of the hind legs when stepping up,
and thus avoiding reaching the workspace limits and collisions along the foot
swing trajectory. In Fig. 5.4(D), HyQ lowered its body and pitched downwards
so that the front hips are lowered. This is done for two reasons: First, to
increase the workspace of the front legs when stepping down, and thus avoiding
reaching the workspace limits. Second, to have a larger clearance between the
front legs and the obstacle, and thus avoiding leg collisions. In Fig. 5.4(E),
HyQ lowered its hind hips to increase the hind legs’ workspace when stepping
down.

Throughout these simulations, the robot continuously adapted its body pose and
its feet to find the best trade-off between increasing the kinematic
feasibility, and avoiding trajectory and leg collision. This can be seen in
Video 1 where the robot’s legs and the corresponding feet trajectories never
collided with the terrain. The robot took multiple steps around the same foot
location before stepping over an obstacle. The reason behind this is that the
robot waited for the VPA to change the pose and allow for safe footholds, and
then the VFA took the decision of stepping over the obstacle.

We carried out another scenario where HyQ is climbing the stairs setup in Fig.
5.5 where each step has a rise of 10 cm, and a go of 25 cm. HyQ is commanded
to trot with a desired forward velocity of $0.2$m/s using ViTAL. The results
are reported in Fig. 5.5 and Video 2. Figure 5.5 shows the robot’s height and
pitch based on the VPA, and the corresponding feet trajectories of the LF leg
and the RH leg based on the VFA. HyQ’s behavior was similar to the previous
section: it accomplished the task without collisions or reaching workspace
limits.

##### 5.6.2 Climbing Stairs (Experiments)

Figure 5.6: HyQ and HyQReal climbing stairs in experiment. (A-F) HyQ crawling
over with 0.1 m/s commanded forward velocity. (G-L) HyQ trotting over with
0.25 m/s commanded forward velocity. (M-R) HyQReal crawling over with 0.2 m/s
commanded forward velocity.  Figure 5.7:  HyQ climbing stairs in experiment.
The figure shows the optimal height and corresponding pitch (presented by the
arrows) based on the VPA, and the foot trajectories of the LF and RH legs
based on the VFA.

To validate ViTAL in experiments, we created three sets of experiments using
the setups shown in Fig. 5.1. Each step has a rise of 10 cm, and a go of 28
cm.

In the first set of experiments, HyQ is commanded to crawl over the setups in
Fig. 5.1(A,B) with a desired forward velocity of 0.1 m/s using the VPA and the
VFA. Figures 5.6(A-F) show screenshots of one trial. Video 3 shows HyQ
climbing back and forth the setup in Fig. 5.1(B) five times. Video 4 shows HyQ
climbing the Fig. 5.1(A) setup, which is reported in Fig. 5.7. Figure 5.7
shows the robot’s height and pitch based on the VPA, and the corresponding
feet trajectories of the Left-Front (LF) leg and the Right-Hind (RH) leg based
on the VFA. This set of experiments confirms that ViTAL is effective on the
real platform. The robot managed to accomplish the task without collisions or
reaching workspace limits.

In the second set of experiments, HyQ is commanded to trot over the setup in
Fig. 5.1(B) with a desired forward velocity of 0.25 m/s using the VPA and the
VFA. Figures 5.6(G-L) show separate screenshots of this trial. Video 5 shows
three trials of HyQ climbing the same setup. This set of experiments shows
that ViTAL can handle different gaits.

Finally, in the third set of experiments, HyQReal is commanded to crawl over
the setup in Fig. 5.1(C) with a desired forward velocity of 0.2 m/s using the
VPA and the VFA. The results are reported in Fig. 5.6(M-R) that show
screenshots of this trial. Video 6 shows HyQReal climbing this stair setup
(Fig. 5.1(C)). This set of experiments shows that ViTAL can work on different
legged platforms.

Table 5.1: Mean Absolute Tracking Errors of the Body Pitch $\beta$ and Height $z_{b}$ of HyQ & HyQReal using ViTAL in the Experiments detailed in Section 5.6.2 and in Fig. 5.1. Description |  $\beta$ [deg] |  $z_{b}$ [cm]
---|---|---
Exp. (A): HyQ crawling at 0.1 m/s | 1.0 | 1.5
Exp. (B): HyQ trotting at 0.2 m/s | 1.0 | 2.3
Exp. (C): HyQReal crawling at 0.2 m/s | 1.0 | 4.2

The tracking performance of these three sets of experiments are shown in Table
5.1. The table shows the mean absolute tracking errors of the body pitch
$\beta$ and height $z_{b}$ of HyQ and HyQReal.

##### 5.6.3 Climbing Stairs with Different Forward Velocities

Figure 5.8: HyQ’s performance using ViTAL under different commanded velocities. The top two plots show the numerical difference of the body height and pitch ($\Delta z_{b}$ and $\Delta\gamma$), and the bottom two plots show the tracking errors of the body height and pitch. Table 5.2: Mean Absolute Tracking Errors of the Body Pitch $\beta$ and Height $z_{b}$ of HyQ using ViTAL in Simulation with Different Forward Velocities as Detailed in Section 5.6.3 and Fig. 5.8. Description |  $\beta$ [deg] |  $z_{b}$ [cm]
---|---|---
Sim. (A): HyQ trotting at 0.2 m/s | 0.7 | 0.3
Sim. (B): HyQ trotting at 0.3 m/s | 1.1 | 0.7
Sim. (C): HyQ trotting at 0.4 m/s | 2.3 | 2.6
Sim. (D): HyQ trotting at 0.5 m/s | 1.6 | 3.8

We evaluate the performance of HyQ under different commanded velocities using
ViTAL. We carried out a series of simulations using the stairs setup shown in
Fig. 5.5. HyQ is commanded to trot at four different forward velocities: 0.2
m/s, 0.3 m/s, 0.4 m/s, and 0.5 m/s. The results are reported in Fig. 5.8 and
in Video 7. Additionally, the tracking performance of these series of
simulations at the four different forward velocities are shown in Table 5.2.
The table shows the mean absolute tracking errors of the body pitch $\beta$
and height $z_{b}$ of HyQ at the corresponding commanded forward velocity.
Figure 5.8 shows the numerical differences $\Delta z_{b}$ and $\Delta\gamma$,
and the tracking errors of the body height and pitch, respectively.

HyQ was able to climb the stairs terrain under different commanded velocities.
However, as the commanded velocity increases, HyQ started having faster
(abrupt) changes in the body pose as shown in the top two plots of Fig. 5.8.
As a result, the height and pitch tracking errors increase proportionally to
the commanded speed as shown in the bottom two plots of Fig. 5.8. This can
also be seen in Table 5.2 where the mean absolute tracking errors of the pitch
and height increase proportionally to the commanded speed.

Similarly to HyQ, we evaluate ViTAL on HyQReal and commanded it to trot with
five different forward velocities: 0.2 m/s, 0.3 m/s, 0.4 m/s, 0.5 m/s, and
0.75 m/s. We report this simulation in Video 8 where we show that ViTAL is
robot independent. Yet, since the workspace of HyQReal is larger than HyQ,
this scenario was more feasible to traverse for HyQReal. Thus, HyQReal was
able to reach a higher commanded velocity than the ones reported for HyQ.

##### 5.6.4 Comparing the VPA with a Baseline (Experiments)

Figure 5.9:  The difference between the VPA and the TBR in six experiments (3
each). (A) The number of safe footholds corresponding to the robot pose. (B,C)
The body height and pitch tracking errors, respectively. Circles (o) and
crosses (x) are successful and failed trials, respectively. Unlike the VPA,
the TBR failed to climb the stairs because the TBR resulted in almost no safe
footholds for the four legs to reach.

We compare the VPA with another vision-based pose adaptation strategy: the
Terrain-Based Body Reference (TBR) [114]. The TBR generates pose references
based on the footholds selected by the VFA. The TBR fits a plane that passes
through the given selected footholds, and sets the orientation of this plane
as a body orientation reference to the robot. The elevation reference of the
TBR is a constant distance from the center of the approximated plane that
passes through the selected footholds. We chose the TBR instead of an
optimization-based strategy since the latter does not provide references that
are fast enough with respect to the VPA.

Using the stairs setup in Fig. 5.1(A), we conducted six experimental trials:
three with the VPA and three with the TBR. All trials were with the VFA. In
all trials, HyQ is commanded to crawl with a desired forward velocity of 0.1
m/s. The results are reported in Fig. 5.9 and Video 9. Figure 5.9(A) shows the
number of safe footholds corresponding to the robot pose from the VPA and the
TBR. The robot height and pitch tracking errors are shown in Fig. 5.9(B,C),
respectively.

As shown in Video 9, HyQ failed to climb the stairs with the TBR, while it
succeeded with the VPA. This is because, unlike the VPA, the TBR does not aim
to put the robot in a pose that maximizes the chances of the legs to succeed
in finding safe footholds. As shown in Fig. 5.9(A), the number of safe
footholds from using the TBR was below the ones from using the VPA. During
critical periods when the robot was around 0 m in the x-direction, the number
of safe footholds from using the TBR almost reached zero. The low number of
safe footholds for the TBR compared to the VPA is reflected in the tracking of
the robot height and pitch as shown in Fig. 5.9(B,C) where the tracking errors
from the TBR were higher than the VPA.

The difference between the VPA and the TBR can be further explained in Video
9. When the TBR is used, the robot is adapting its pose given the selected
foothold. But, if the selected foothold is not reached, or if there is a high
tracking error, the robot reaches a body pose that results in a smaller number
of safe footholds. Thus, the feet end up colliding with the terrain and hence
the robot falls. On the other hand, the VPA is able to put the robot in a pose
that maximizes the number of safe footholds. As a result, the feet found
alternative safe footholds to select from, which resulted in no collision, and
succeeded in climbing the stairs. The VPA optimizes for the number of safe
footholds. Thus, if there is a variation around the optimal pose (tracking
error), the VFA still finds more footholds to step on, which is not the case
with the TBR.

##### 5.6.5 Comparing the VPA with a Baseline (Simulation)

Similar to experiments, and using the stairs setup in Fig. 5.5, we compare the
VPA with the TBR. We conducted six simulations: three with the VPA and three
with the TBR. All trials were with the VFA. In all trials, HyQ is commanded to
trot with a $0.2$m/s desired forward velocity. The results are reported in
Fig. 5.10 and Video 10. Figure 5.10(A) shows the number of safe footholds
corresponding to the robot pose from the VPA, and the TBR. The tracking errors
of the robot height and pitch are shown in Fig. 5.10(B,C), respectively. These
trials show that HyQ failed to climb the stairs using the TBR, while it
succeeded using the VPA.

Figure 5.10: The difference between the VPA and the TBR in six simulations (3
each). (A) The number of safe footholds corresponding to the robot pose. (B,C)
The body height and pitch tracking errors, respectively. Circles (o) and
crosses (x) are successful and failed trials, respectively. Unlike the VPA,
the TBR failed to climb the stairs because the TBR resulted in almost no safe
footholds for the four legs to reach.

##### 5.6.6 Climbing Stairs with Gaps

We show HyQ’s capabilities of climbing stairs with gaps using ViTAL, and we
compare the VPA with the TBR. In this scenario, HyQ is commanded to trot at
0.4 m/s. Figure 5.11(A) shows overlayed screenshots of the simulation and the
used setup. Figure 5.11(B) shows the robot’s height and pitch based on the
VPA, and the corresponding feet trajectories of the LF leg and the RH leg
based on the VFA, and Fig. 5.11(C) shows the number of safe footholds using
the VPA and the TBR. Because of ViTAL, HyQ was able to climb the stairs with
gaps while continuously adapting its pose and feet. Furthermore, the number of
safe footholds from using the TBR is always lower than from using the VPA,
which shows that indeed the VPA outperforms the TBR. Video 11 shows the output
of this simulation using ViTAL.

Figure 5.11: HyQ climbing gapped stairs. (A) Screenshots of HyQ climbing the
setup. (B) The robot’s height and pitch based on the VPA, and the
corresponding feet trajectories of the LF and RH legs based on the VFA. (C)
The number of safe footholds using the VPA and the TBR. Figure 5.12: Pose
Optimization: Single vs. Receding Horizons. (A) The number of safe footholds.
(B) Variation (numerical difference) of the robot’s height. (C) Variation
(numerical difference) of the robot’s pitch. (D) The tracking error of the
robot’s height.

##### 5.6.7 Pose Optimization: Single vs. Receding Horizons

To analyze the differences between the receding horizon and the single horizon
in pose optimization, we use the stairs setup in Fig. 5.5 with a commanded
forward velocity of 0.4 m/s, and report the outcome in Fig. 5.12 and Video 12.
The main advantage of using a receding horizon instead of a single horizon is
that the pose optimization can consider future decisions. Thus, if the robot
is trotting at higher velocities, the pose optimizer can adapt the robot’s
pose before hand. This can result in a better adaptation strategy with less
variations in the generated optimal pose. Thus, we analyze the two approaches
by taking a look at the variations in the body pose

$\acute{z}_{b}=\frac{\Delta z_{b}}{\Delta
x_{b}}~{},~{}~{}~{}\text{and}~{}~{}~{}\acute{\gamma}=\frac{\Delta\gamma}{\Delta
x_{b}}$ (5.26)

where $\acute{z}_{b}$ and $\acute{\gamma}$ are the numerical differences
(variations) of the robot height $z_{b}$ and pitch $\gamma$ with respect to
the robot forward position $x_{b}$, respectively.

Figure 5.12 reports the differences between the two cases. The number of safe
footholds is shown in Fig. 5.12(A). The variations in $\acute{z}_{b}$ and
$\acute{\gamma}$ are shown in Fig. 5.12(B,C), respectively. Finally, the
tracking error of the robot’s height is shown in Fig. 5.12(D). As shown in
Figure 5.12 the receding horizon resulted in less variations in the body pose
compared to the single horizon. This resulted in a smaller tracking error for
the receding horizon in the body height, which resulted in slightly larger
number of safe footholds. All in all, the receding horizon reduces variations
in the desired trajectories which improves the trajectory tracking response.

The differences between the receding and single horizon in the pose
optimization can also be noticed in Video 12. In the case of a single horizon,
the robot was struggling while climbing up the stairs but was able to recover
and accomplish the task. However, using the receding horizon, the robot was
able to adapt its pose in time, and thus resulting in safer footholds that
allowed the robot to accomplish the task.

Figure 5.13: Pose Optimization: $\mathcal{C}_{\mathrm{sum}}$ vs.
$\mathcal{C}_{\mathrm{int}}$. The shaded areas are the envelopes of the number
of safe footholds. The lines are the thicknesses (errors) between these
envelopes.

##### 5.6.8 Pose Optimization: $\mathcal{C}_{\mathrm{sum}}$ vs.
$\mathcal{C}_{\mathrm{int}}$

To analyze the differences between $\mathcal{C}_{\mathrm{sum}}$ and
$\mathcal{C}_{\mathrm{int}}$ in the pose optimization, we use the stairs setup
in Fig. 5.5 with a commanded forward velocity of 0.4 m/s, and report the
outcome in Fig. 5.13. The main advantage of using $\mathcal{C}_{\mathrm{int}}$
over $\mathcal{C}_{\mathrm{sum}}$ is that $\mathcal{C}_{\mathrm{int}}$ will
result in a pose that does not just maximize the number of safe footholds for
all of the legs, but also ensures that the number of safe footholds of the
poses around the optimal pose is still high. To compare the two cost
functions, we take a look at the number of safe footholds. In particular, we
evaluate the number of safe footholds corresponding to the optimal pose, and
the poses around it with a margin of $m=$ 0.025 m. Thus, in Fig. 5.13, we plot
the envelope (shaded area) between $\hat{\mathcal{F}}(u^{*}+m)$ and
$\hat{\mathcal{F}}(u^{*}-m)$ for both cases, and the thickness between these
envelopes which we refer to as error

$\textbf{error}~{}=|\hat{\mathcal{F}}(u^{*}+m)-\hat{\mathcal{F}}(u^{*}-m)|.$
(5.27)

As shown in Fig. 5.13 the envelope of the number of safe footholds resulting
from $\mathcal{C}_{\mathrm{int}}$ is almost always encapsulated by
$\mathcal{C}_{\mathrm{sum}}$. The thickness (error) of the number of safe
footholds resulting from using $\mathcal{C}_{\mathrm{int}}$ is always smaller
than $\mathcal{C}_{\mathrm{sum}}$. This means that any variation of $m$ in the
optimal pose will be less critical if $\mathcal{C}_{\mathrm{int}}$ is used
compared to $\mathcal{C}_{\mathrm{sum}}$.

Figure 5.14: HyQ traversing rough terrain and climbing stairs sideways. (A,B)
HyQ traversing rough terrain with and without ViTAL, respectively. (C,D) HyQ
climbing stairs while yawing (commanding the yaw rate) using ViTAL. (E,F) HyQ
climbing stairs sideways using ViTAL.

##### 5.6.9 Locomotion over Rough Terrain

We evaluate the performance of HyQ in traversing rough terrain as shown in
Fig. 5.14(A,B) and in Video 13. We conducted two simulations: one with ViTAL
and thus with exteroceptive and proprioceptive reactions as shown in Fig.
5.14(A), and another without ViTAL and thus only with proprioceptive reactions
as shown in Fig. 5.14(B). HyQ was commanded to traverse the rough terrain with
a forward velocity of 0.2 m/s. No hyper parameters re-tuning, or CNNs re-
training were needed.

As shown in Video 13, HyQ was able to successfully traverse the terrain in
both cases. With ViTAL, HyQ collided less with the terrain and continuously
adapted its footholds over the small cobblestones. Without ViTAL, HyQ
traversed the rough terrain, yet, with significantly more effort.
Additionally, without ViTAL, HyQ continuously collided with the terrain, and
in some incidents, the feet got stuck. For this reason, we had to re-tune the
gait parameters, and increase the step height to reduce these incidents. The
robot’s feet also kept slipping since the feet were always close to edges and
corners.

##### 5.6.10 Climbing Stairs with Different Commands

Instead of commanding only forward velocities as in the previous sections, we
command HyQ to climb the stairs with ViTAL while yawing (commanding the yaw
rate) as shown in Video 14 and Fig. 5.14(C,D), and to climb stairs laterally
as shown in Video 15 and Fig. 5.14(E,F). Climbing stairs sideways is more
challenging than facing the stairs since the range of motion of the robot’s
roll orientation is more restricted versus the pitch orientation. That said,
because of ViTAL, HyQ was still able to climb these stairs in both cases as
shown in Video 14 and Video 15.

#### 5.7 Conclusion

We presented ViTAL which is an online vision-based locomotion planning
strategy. ViTAL consists of the VPA for pose adaptation, and the VFA for
foothold selection. The VPA introduces a different paradigm to current state-
of-the-art pose adaptation strategies. The VPA finds body poses that maximize
the chances of the legs to succeed in reaching safe footholds. This notion of
success emerges from the robot’s skills. These skills are encapsulated in the
FEC that include (but are not limited to) the terrain roughness, kinematic
feasibility, leg collision, and foot trajectory collision. The VFA is a
foothold selection algorithm that continuously adapts the robot’s trajectory
based on the FEC. The VFA algorithm of this work extends our previous work in
[121, 114] as well as the state of the art [113, 118]. Since the computation
of the FEC is usually expensive, we rely on approximating these criteria with
CNNs.

The robot’s skills and the notion of success provided by the FEC allowed the
VPA to generate body poses that maximize the chances of success in reaching
safe footholds. This resulted in body poses that are aware of the terrain and
aware of what the robot and its legs can do. For that reason, the VPA was able
to generate body poses that give a better chance for the VFA to select safe
footholds. As a result, because of ViTAL, HyQ and HyQReal were able to
traverse multiple terrains with various forward velocities and different gaits
without colliding or reaching workspace limits. The terrains included stairs,
gaps, and rough terrains, and the commanded velocities varied from 0.2 m/s to
0.75 m/s. The VPA outperformed other strategies for pose adaptation. We
compared VPA with the TBR which is another vision based pose adaptation
strategy, and showed that indeed the VPA puts the robot in a pose that
provides the feet with higher number of safe footholds. Because of this, the
VPA made our robots succeed in various scenarios where the TBR failed.

#### 5.8 Limitations and Future Work

One issue that we faced during experiment was in tracking the motion of the
robot, especially for HyQReal. We were using a WBC for motion tracking. We
believe that the motion tracking and our strategy can be improved by using a
Model Predictive Control (MPC) alongside the WBC. Similarly, instead of using
a model-based controller (MPC or WBC), we hypothesize that an RL-based
controller can also improve the robustness and reliability of the overall
robot behavior.

As explained in Section 5.5, one other key limitation was regarding the
perception system. State estimation introduced a significant drift that caused
a major noise and drift in the terrain map. Albeit not being a limitation to
the suggested approach, we plan on improving the state estimation and
perception system of HyQ and HyQReal to allow us to test ViTAL in the wild.

The pose optimization problem of the VPA does not reason about the robot’s
dynamics. This did not prevent HyQ and HyQReal from achieving dynamic
locomotion while traversing challenging terrains at high speeds. However, we
believe that incorporating the robot’s dynamics into ViTAL may result in a
better overall performance. That said, we believe that in the future, the VPA
should also reason about the robot’s dynamics. For instance, one can augment
the FEC with another criterion that ensures that the selected footholds are
dynamically feasible by the robot.

Additionally, in the future, we plan to extend the VPA of ViTAL to not only
send pose references, but also reason about the robot’s body twist. We also
plan to augment the robot skills to not only consider foothold evaluation
criteria, but also skills that are tailored to the robot pose. Finally, in
this work, ViTAL considered heightmaps which are 2.5D maps. In the future, we
plan to consider full 3D maps that will enable ViTAL to reason about
navigating in confined space (inspired by [115]).

###  Chapter 6 ViTAL - Implementation Details

© 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.

#### 6.1 CNN approximation in the VFA and the VPA

In the VFA, the foothold evaluation stage is approximated with a CNN [133] as
explained in Remark 5.3. The CNN approximates the mapping between
$T_{\mathrm{vfa}}$ and $p_{*}$. The heightmap $H_{\mathrm{vfa}}$ in
$T_{\mathrm{vfa}}$ passes through three convolutional layers with $5\times 5$
kernels, $2\times 2$ padding, Leaky ReLU activation [134], and $2\times 2$
max-pooling operation. The resulted one-dimensional feature vector is
concatenated with the rest of the variables in the tuple $T_{\mathrm{vfa}}$,
namely, $z_{h},v_{b},\alpha,$ and $p_{n}$. This new vector passes through two
fully-connected layers with Leaky ReLU and softmax activations. The parameters
of the CNN are optimized to minimize the cross-entropy loss [135] of
classifying a candidate foothold location as optimal $p_{*}$.

In the VPA, the pose evaluation and the function approximation is approximated
with a CNN as explained in Remark 5.5. The CNN infers the weights $w$ of
$\hat{\mathcal{F}}$ given $T_{\mathrm{vpa}}$ (the mapping between
$T_{\mathrm{vpa}}$ and $w$). The heightmap
$H_{\mathrm{vpa}}\in\mathbb{R}^{33\times 33}$ passes through three
convolutional layers with $5\times 5$ kernels, $2\times 2$ padding, Leaky ReLU
activation, and $2\times 2$ max-pooling operation. The body velocities $v_{b}$
pass through a fully-connected layer with Leaky ReLU activation that is then
concatenated with the one-dimensional feature vector obtained from the
heightmap. This new vector passes through two fully-connected layers with
Leaky ReLU and linear activations. The parameters of this CNN are optimized to
minimize the mean squared error loss between the number of safe footholds
$\mathrm{n_{sf}}$ predicted by $\hat{\mathcal{F}}(z_{h},w)$ and
$\hat{\mathcal{F}}(z_{h},\hat{w})$ where $\hat{w}$ are the function parameters
approximated by the CNN.

For both CNNs, we used the Adam optimizer [136] with a learning rate of
$0.001$, and we used a validation-based early-stopping using a $9$-to-$1$
proportion to reduce overfitting. The datasets required for training this CNNs
are collected by running simulated terrain scenarios that consist of bars,
gaps, stairs, and rocks. In this work, we considered a $33\times 33$ heightmap
with a resolution of 0.02 m
($H_{\mathrm{vfa}},H_{\mathrm{vpa}}\in\mathbb{R}^{33\times 33}$).

Figure 6.1: An illustration of the Function Approximation of the VPA.

#### 6.2 On the Function Approximation of the VPA

As explained in Section 5.4.4, the function $\hat{\mathcal{F}}(z_{h_{i}},w)$

$\hat{\mathcal{F}}(z_{h_{i}},w)=\sum_{e=1}^{E}w_{e}\cdot
g(z_{h_{i}},\Sigma_{e},c_{e})$ (6.1)

is defined as the weighted sum of Gaussian basis functions

$g(z_{h_{i}},\Sigma_{e},c_{e})=\text{exp}(-0.5(z_{h_{i}}-c_{e})^{T}\Sigma_{e}^{-1}(z_{h_{i}}-c_{e})).$
(6.2)

The parameters $\Sigma_{e}$ and $c_{e}$ are the widths and centers of the
Gaussian function $g_{e}$ (see Section 3.1 in [65]). In the literature,
$c_{e}$ is usually referred to as the mean or the expected value, and
$\Sigma_{e}$ as the standard deviation. The regression algorithm should
predict the weights $w_{e}$, and the parameters $\Sigma_{e}$ and $c_{e}$. To
reduce the dimensionality of the problem, as explained in Section 5.4.4, and
in Section 4.1 in [65], we decided to fix the values of the parameters of the
Gaussian functions $\Sigma_{e}$ and $c_{e}$. In detail, the centers $c_{e}$
are spaced equidistantly within the bounds of the hip heights $z_{h_{i}}$, and
the widths are determined by the value at which the Gaussian functions
intersect. That way, the regression algorithm only outputs the weights
$w_{e}$. Figure 6.1 shows an example of the function approximation. In this
example, the bounds of the hip heights $z_{h_{i}}$ are 0.2 m and 0.8 m.
Assuming a number of basis functions $E=3$, the centers $c_{e}$ are then
chosen to be equidistant within the bounds, and thus, the centers $c_{e}$ are
0.2 m, 0.3 m and 0.8 m. By choosing the Gaussian functions to intersect at
0.5, the widths $\Sigma_{e}$ are 0.13.

#### 6.3 Representing the Hip Heights in terms of the Body Pose

To represent the hip heights in terms of the body pose, we first write the
forward kinematics of the robot’s hips

$p_{h_{i}}^{W}=p_{b}^{W}~{}+~{}R_{b}^{W}p_{h_{i}}^{b}$ (6.3)

where $p_{h_{i}}^{W}\in\mathbb{R}^{3}$ is the position of the hip of the $i$th
leg in the world frame, $p_{b}^{W}\in\mathbb{R}^{3}$ is the position of the
robot’s base in the world frame, $R_{b}^{W}\in SO(3)$ is the rotation matrix
mapping vectors from the base frame to the world frame, and
$p_{h_{i}}^{b}\in\mathbb{R}^{3}$ is the position of the hip of the $i$th leg
in the base frame. The rotation matrix $R_{b}^{W}$ is a representation of the
Euler angles of the robot’s base with sequence of roll $\beta$, pitch
$\gamma$, and yaw $\psi$ (Cardan angles) [137]. The variable $p_{h_{i}}^{b}$
is obtained from the CAD of the robot. Expanding (6.3) yields

$\displaystyle\begin{bmatrix}x_{h_{i}}^{W}\\\ y_{h_{i}}^{W}\\\
z_{h_{i}}^{W}\end{bmatrix}$ $\displaystyle=$
$\displaystyle\begin{bmatrix}x_{b}^{W}\\\ y_{b}^{W}\\\
z_{b}^{W}\end{bmatrix}+R_{b}^{w}(\beta,\gamma,\psi)\begin{bmatrix}x_{h_{i}}^{b}\\\
y_{h_{i}}^{b}\\\ z_{h_{i}}^{b}\end{bmatrix}$ (6.4) $\displaystyle=$
$\displaystyle\begin{bmatrix}x_{b}^{W}\\\ y_{b}^{W}\\\
z_{b}^{W}\end{bmatrix}+\begin{bmatrix}\cdots&\cdots&\cdots\\\
\cdots&\cdots&\cdots\\\
-s\gamma&c\gamma~{}s\beta&c\gamma~{}c\beta\end{bmatrix}\begin{bmatrix}x_{h_{i}}^{b}\\\
y_{h_{i}}^{b}\\\ z_{h_{i}}^{b}\end{bmatrix}$ (6.5)

where $s$ and $c$ are sine and cosine of the angles, respectively. Since we
are interested only in the hip heights, the z-component (third row) of (6.5)
yields

$z_{h_{i}}^{W}=z_{b}^{W}-x_{h_{i}}^{b}s\gamma+y_{h_{i}}^{b}c\gamma
s\beta+z_{h_{i}}^{b}c\gamma c\beta.$ (6.6)

#### 6.4 Using the Sum of Squared Integrals in the Pose Optimization of the
VPA

Figure 6.2: Using the sum of squared integrals as a cost function in the pose
optimization of the VPA. The two curves represent $\hat{\mathcal{F}}_{l}$. The
x-axis is the hip height and the y-axis is $\mathrm{n_{sf}}$. The figure shows
two optimal poses: $u_{1}^{*}$ which is from using
$\mathcal{C}_{\mathrm{sum}}$ or $\mathcal{C}_{\mathrm{prod}}$, and $u_{2}^{*}$
which is from using $\mathcal{C}_{\mathrm{int}}$.

Using $\mathcal{C}_{\mathrm{int}}$ as a cost function can be motivated by
taking Fig. 6.2 as an example. In this figure, there are two curves that
represent $\hat{\mathcal{F}}_{l}$ where the horizontal axis is the hip height
and the vertical axis represent $\mathrm{n_{sf}}$. The figure shows two
optimal poses where $u_{1}^{*}$ is the optimal pose using the cost functions
$\mathcal{C}_{\mathrm{sum}}$ or $\mathcal{C}_{\mathrm{prod}}$ that only
maximize for $\hat{\mathcal{F}}_{l}$, and $u_{2}^{*}$ is the optimal pose
using the cost function $\mathcal{C}_{\mathrm{int}}$. As shown in the figure,
if $\mathcal{C}_{\mathrm{sum}}$ or $\mathcal{C}_{\mathrm{prod}}$ is used, the
optimal pose will be $u_{1}^{*}$ which is indeed the one that results in the
maximum $\hat{\mathcal{F}}_{l}$. However, if there is a tracking error of $m$
(thus the robot reaches $u_{1}^{*}\pm m$), the robot might end up in the pose
$u_{1}^{*}\pm m$ that results in a small number of safe footholds. Using
$\mathcal{C}_{\mathrm{int}}$ will take into account the safe footholds within
a margin $m$. This might result in a pose that does not yield the maximum
number of safe footholds, but it will result in a safer foothold in case the
robot pose has any tracking errors. Note that using the sum of squared
integrals $\mathcal{C}_{\mathrm{int}}$ as a cost function is similar to
smoothing $\hat{\mathcal{F}}_{l}$ with respect to the hip height (a moving
average smoothing). Using a smoothing function $\mathcal{C}_{s}$ may take the
form

$\mathcal{C}_{s}=\sum_{l=1}^{N_{l}=4}\|\frac{1}{2\epsilon}\sum_{i=-\epsilon}^{\epsilon}\hat{\mathcal{F}}_{l}(z_{h_{l}}+i)\
\|^{2}_{Q}.$ (6.7)

#### 6.5 Defining the Receding Horizon

In the receding horizon there is a tuple $T_{\mathrm{vpa},j}$ for every $j$th
horizon that is defined as

$T_{\mathrm{vpa},j}=(H_{\mathrm{vpa},j},v_{b},\alpha)$ (6.8)

hence sharing the same body twist $v_{b}$ and gait parameters $\alpha$ but a
different heightmap $H_{\mathrm{vpa},j}$. For every leg, a heightmap of
horizon $j+1$ is overlapping with the previous horizon’s $j$ heightmap. This
overlap has a magnitude of $\Delta h$ taking the same direction as the body
velocity $\dot{x}_{b}$. Without loss of generality, we chose the magnitude of
the overlap to be half of the diagonal size of the heightmap in this work. To
sum up, we first gather $T_{\mathrm{vpa},j}$ that share the same $v_{b}$ and
$\alpha$, but a different $H_{\mathrm{vpa},j}$. Then, we evaluate
$T_{\mathrm{vpa},j}$ and approximate the output using the function
approximation yielding $\hat{\mathcal{F}}_{j}$ that is sent to the optimizer
for all of the legs.

#### 6.6 Miscellaneous Settings

In this work, all simulations were conducted on an Intel Core i7 quad-core
CPU, and all experiments were running on an onboard Intel Core i7 quad-core
CPU where state estimation, mapping, and controls were running. The RCF
(including the WBC) runs at 250 Hz, the low-level controller runs at 1000 Hz,
the state estimator runs at 333 Hz, and the mapping algorithm runs at 20 Hz.
The VPA and the VFA run asynchronously at the maximum possible update rate.

ViTAL is implemented in Python. The CNNs are implemented in PyTorch [138]. As
explained in Section 6.1, in this work, we considered a $33\times 33$
heightmap with a resolution of 0.02 m
($H_{\mathrm{vfa}},H_{\mathrm{vpa}}\in\mathbb{R}^{33\times 33}$). The finite
set $\bar{\mathcal{Z}}$ consisted of a hip height range between 0.2 m and 0.8
m with a resolution of 0.02 m yielding $N_{z_{h}}=31$ samples. The number of
radial basis functions used in the function approximation was $E=30$. The pose
optimization problem is solved with a trust-region interior point method [139,
140] which is a non-linear optimization problem solver that we solved using
SciPy [141]. The bounds of the pose optimization problem $u_{\min}$ and
$u_{\max}$ are $[0.2\,\mathrm{m},-0.35\,\mathrm{rad},-0.35\,\mathrm{rad}]$,
and $[0.8\,\mathrm{m},0.35\,\mathrm{rad},0.35\,\mathrm{rad}]$, respectively.
We used a receding horizon of $N_{h}=2$ with a map overlap of half the size of
the heightmap. For a heightmap of a size of $33\times 33$ and a resolution of
0.02 m, the map overlap $\Delta h$ is 0.33 m. We used Gazebo [142] for the
simulations, and ROS for communication.

#### 6.7 Estimation Accuracy

We compare the estimation accuracy of the VFA by comparing the output of the
foothold evaluation stage (explained in Section 5.3) given the same input
tuple $T_{\mathrm{vfa}}$. That is to say, we compare the estimation accuracy
of the VFA by comparing $\hat{g}(T_{\mathrm{vfa}})$ versus
$g(T_{\mathrm{vfa}})$ (see Remark 5.3). To do so, once trained, we generated a
dataset of 4401 samples from randomly sampled heightmaps for every leg. This
analysis was done on HyQ.

As explained in Section 5.3, from all of the safe candidate footholds in
$\mu_{\mathrm{safe}}$, the VFA chooses the optimal foothold to be the one
closest to the nominal foothold. Thus, to fairly analyse the estimation
accuracy of the VFA, we present three main measures: perfect match being the
amount of samples where $\hat{g}(T_{\mathrm{vfa}})$ outputted the exact value
of $g(T_{\mathrm{vfa}})$, safe footholds, being the amount of samples where
$\hat{g}(T_{\mathrm{vfa}})$ did not output the exact value of
$g(T_{\mathrm{vfa}})$, but rather a foothold that is safe but not closest to
the nominal foothold, and mean distance, being the average distance of the
estimated optimal foothold from $\hat{g}(T_{\mathrm{vfa}})$ relative to the
exact foothold from $g(T_{\mathrm{vfa}})$. These measures are presented as the
mean of all legs.

Based on that, the perfect match measure is $74.0\%$. Thus, $74\%$ of
$\hat{g}(T_{\mathrm{vfa}})$ perfectly matched $g(T_{\mathrm{vfa}})$. The safe
footholds measure is $93.7\%$. Thus, $93.7\%$ of $\hat{g}(T_{\mathrm{vfa}})$
were deemed safe. Finally, the mean distance of the estimated optimal foothold