On Terrain Aware Locomotion for Legged Robots

Shamel Fahmi

Doctoral Thesis Defense

April, 2021

 Supervisors: Victor Barasuol, Michele Focchi, Andreea Radulescu, and Claudio Semini

Legged Robots are moving out from research labs into the real world, with one main goal in mind.

The terrains that legged robots traverse are never the same.

Legged Robots Have to be Terrain Aware.

Terrain Aware Locomotion (TAL) means that the robot is able to take decisions based on the surrounding terrain using its perception.

Terrain Aware Locomotion (TAL)

• Proprioceptive TAL (PTAL)

• Exteroceptive TAL (ETAL)

7

Proprioceptive TAL (PTAL)

M. Focchi et al., Local Reflex Generation for Obstacle Negotiation in Quadrupedal Locomotion, CLAWAR, 2013.
V. Barasuol et al., On the detection and localization of shin collisions and reactive actions in quadruped robots, CLAWAR, 2019.

8

Exteroceptive TAL (ETAL)

V. Barasuol et al., Reactive Trotting with Foot Placement Corrections through Visual Pattern Classification, IROS, 2015.
O. Villarreal et al., Fast and Continuous Foothold Adaptation for Dynamic Locomotion through CNNs, RA-L, 2019.

9

Proprioceptive TAL vs. Exteroceptive TAL

PTAL is crucial when

• Visual feedback is denied (smoky areas, or areas with thick vegetation). 

• Terrain map is unreliable.

But

• Limited to corrective actions (step reflexes).

• Do not act on what is ahead of the robot.

10

Research Statement

Let legged robots take decisions based on the surrounding terrain using their perception.

decisions

terrain

perception

Let legged robots take decisions based on the surrounding terrain using their perception.

Outline

• Part I: Proprioceptive Terrain-Aware Locomotion

  • Passive Whole-Body Control (pWBC)

  • STANCE: Locomotion Adaption over Soft Terrain

  • On State Estimation for Legged Robots over Soft Terrain

• Part II: Exteroceptive Terrain-Aware Locomotion

  • ViTAL: Vision-base Terrain-Aware Locomotion

Whole-Body Control (WBC)

• To achieve dynamic locomotion, the robot should:

  • Track the desired reference trajectories.

  • Maintain balance.

  • Reason about the robot's (full) dynamics and limits.

• To achieve TAL, the robot should reason about the contact interaction such as the terrain's inclination and frictional properties, etc.

• How to deal with all these objectives in an optimal way?

• A Whole-Body Control (WBC) Framework.

14

Whole-Body Control (WBC) Framework

• Locomotion Planner [1]-[3]

• State Estimator [4]

• WBC

• Low-level Control [5]

[1] B. Aceituno-Cabezas et al., Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization, RA-L 2017.
[2] C. Mastalli et al., Trajectory and Foothold Optimization using Low-Dimensional Models for Rough Terrain Locomotion, ICRA, 2017.
[3] M. Focchi et al., Heuristic planning for rough terrain locomotion in presence of external disturbances and variable perception quality, STAR, 2020.
[4] S. Nobili et al., Heterogeneous sensor fusion for accurate state estimation of dynamic legged robots, RSS, 2017.
[5] T. Boaventura et al., Modelbased hydraulic impedance control for dynamic robots," T-RO, 2015.

15

• Execute the planned trajectories of the body and swinging leg (control tasks).

• Keep the robot balanced.

• Respect the un-actuated part of the dynamics. 

• Respect the robot's actuated part of the dynamics, and the joint & torque limits.

• Respect the contact constraints due to the interaction with the terrain.

• Maintain contact consistency.

Whole-Body Control (WBC) Objectives

• Cast these objectives as an optimization problem via Quadratic Programming (QP).

• Map the solution to joint torques using inverse dynamics.

16

WBOpt Problem in the pWBC

17

Results

B. Aceituno-Cabezas et al., Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization, RA-L 2017.
C. Mastalli et al., Trajectory and Foothold Optimization using Low-Dimensional Models for Rough Terrain Locomotion, ICRA, 2017.

18

Results

pWBC versus a quasi-static WBC from [1]

[1] M. Focchi et al., High-slope terrain locomotion for torque-controlled quadruped robots, AuRo, 2017.

19

Results

Hip Adduction-Abduction (HAA)

Hip Flexion-Extension (HFE)

Knee Flexion-Extension (KFE)

20

Conclusions: pWBC

• The pWBC is aware of the terrain's geometry and friction.

• The pWBC can achieve dynamic locomotion while compliantly balancing the robot's trunk.

• The pWBC is formulated as a QP that considers:

  • The robot's full dynamics.

  • Joint and torque limits.

  • The interaction with the rigid terrain.

21

• We showed the capabilities of the WBC under:

  • Different gaits.

  • Rigid terrain conditions.

  • Any sudden change in the torque and joint limits.

• We compared the pWBC against a quasi-static WBC.

• Now, is the WBC terrain aware?

• Not really, not over soft terrain. 

Conclusions: pWBC

22

Published In:

S. Fahmi, C. Mastalli, M. Focchi, and C. Semini, “Passive Whole-Body Control for Quadruped Robots: Experimental Validation over Challenging Terrain,”

IEEE Robot. Automat. Lett. (RA-L), vol. 4, no. 3, pp. 2553–2560, Jul. 2019,

doi: 10.1109/LRA.2019.2908502.

Outline

• Part I: Proprioceptive Terrain-Aware Locomotion

  • Passive Whole-Body Control (pWBC)

  • STANCE: Locomotion Adaption over Soft Terrain

  • On State Estimation for Regged Robots over Soft Terrain

• Part II: Exteroceptive Terrain-Aware Locomotion

  • ViTAL: Vision-base Terrain-Aware Locomotion

• Most of WBCs are not terrain-aware. They fail to generalize beyond rigid terrain.

• Locomotion over soft terrain by itself is challenging because of the induced contact dynamics.

What is the problem?

• Mismatch between what the WBC assumes vs. what is actually happening.

26

• Drop the assumption of the rigid terrain. Use a more generic model that accounts for the terrain impedance.

• But that’s not enough, how do you know the impedance parameters of the terrain. 

• Can we, estimate it?

How to solve it?

27

Passive WBC (pWBC) = Standard WBC (sWBC)

Reformulate the sWBC to account for the contact dynamics

C3 = compliant contact consistent

How will the new WBC know about the current terrain impedance parameters?

C3 = compliant contact consistent

?

Feedback the terrain impedance parameters using a Terrain Compliance Estimator (TCE)

C3 = compliant contact consistent

TCE

Simultaneously, at every control loop

C3 Whole-Body Control (C3WBC)

• The stance task of the sWBC.

• To be compliant contact consistent (C3), we need to model the soft contact dynamics. 

34

WBOpt Problem in C3WBC

35

• Now, the WBC is C3, but it still needs the terrain impedance parameters.

• The goal of the TCE is to estimate the terrain parameters online based on the current measurements (states), and feed them back to the C3WBC.

• The TCE is decoupled from the WBC but uses the same contact model (KV model).

WBC Framework

36

Terrain Compliance Estimation (TCE)

We start with the contact model.

37

Terrain Compliance Estimation (TCE)

• The TCE estimates the GRFs and feet penetration based on the current robot measurements. 

• We used a MCS to estimate the base states.

38

Results

39

Results

40

Results

41

Results

42

• Unlike the state of the art WBC, we do not assume that the ground is rigid.

• Adapt online to any type of terrain compliance.

• C3WBC

  • ​Extends the sWBC but incorporating soft contact dynamics.

  • C3WBC can adapt to the given terrain compliance.

• ​TCE

  • ​Estimate the terrain compliance online.

  • Not computationally expensive.

Conclusions: STANCE

43

• With STANCE, HyQ can traverse and transition between multiple terrains with different compliance without pre-tuning.

• STANCE makes the locomotion strategy C3. It allowed HyQ to remain consistent with ground, and thus more robust in challenging and scenarios.

• Each leg independently senses and adapts to changes in terrain compliance.

Conclusions: STANCE

44

STANCE's Limitations

• The TCE estimates the GRFs and feet penetration based on the current robot measurements. 

• We used a MCS to estimate the base states.

45

Published In:

† S. Fahmi, M. Focchi, A. Radulescu, G. Fink, V. Barasuol, and C. Semini,

“STANCE: Locomotion Adaptation over Soft Terrain,” IEEE Trans. Robot. (T-RO), vol. 36, no. 2, pp. 443–457, Apr. 2020, doi: 10.1109/TRO.2019.2954670.

† Selected as a finalist for the IEEE RAS Italian Chapter Young Author Best Paper Award 2020, and for the IEEE RAS Technical Committee on Model-Based Optimization for Robotics Best Paper Award 2020.

Outline

• Part I: Proprioceptive Terrain-Aware Locomotion

  • Passive Whole-Body Control (pWBC)

  • STANCE: Locomotion Adaption over Soft Terrain

  • On State Estimation for Legged Robots over Soft Terrain

• Part II: Exteroceptive Terrain-Aware Locomotion

  • ViTAL: Vision-base Terrain-Aware Locomotion

State Estimation for Legged Robots

• Most of the current state estimators are based on filters that fuse multiple sensor modalities.

  • High frequency: IMU, kinematic measurements (leg odometry), etc.

  • Low frequency: cameras & lidars. 

• Similar to WBCs, state estimators are shown to be reliable over stiff terrains. But over soft terrain, the performance starts to decline.

• Experimental analysis and formal study on the effects of soft terrain on state estimation, and the reasons behind these effects.

48

State Estimator

G. Fink et al., Proprioceptive Sensor Fusion for Quadruped Robot State Estimation, IROS, 2020.

49

Experimental Setup

• Soft and Rigid Terrain Trotting Experiments.

• Using MCS for the ground truth.

50

Results

Conclusions: State Estimation over Soft Terrain

• Over soft terrain, there is a larger error in the velocity estimates which resulted in a larger drift in position estimates.

• The contact with the terrain is no longer binary: it chatters rapidly between contact and non-contact.

• Many states are affected besides the robot pose, including the contact state and the GRFs.

52

Published In:

S. Fahmi, G. Fink, and C. Semini, “On State Estimation for Legged Locomotion over Soft Terrain,” IEEE Sensors Lett. (L-SENS), vol. 5, no. 1, pp. 1–4, Jan. 2021, doi: 10.1109/LSENS.2021.3049954.

Outline

• Part I: Proprioceptive Terrain-Aware Locomotion

  • Passive Whole-Body Control (pWBC)

  • STANCE: Locomotion Adaption over Soft Terrain

  • On State Estimation for Regged Robots over Soft Terrain

• Part II: Exteroceptive Terrain-Aware Locomotion

  • ViTAL: Vision-base Terrain-Aware Locomotion

Locomotion Planning

Two ways to deal with locomotion planning:

• Plan the feet & base states simultaneously.

• Separate the plan into feet & base plans.

• Planning the feet motion ⇒ foothold selection

• Planning the body motion ⇒ pose adaptation

55

Esteban 2020

Villarreal 2019

Barasuol 2015

Foothold Selection

V. Barasuol et al., Reactive Trotting with Foot Placement Corrections through Visual Pattern Classification, IROS, 2015.
O. Villarreal et al., Fast and Continuous Foothold Adaptation for Dynamic Locomotion through CNNs, RA-L, 2019.
D. Esteban et al., On the Influence of Body Velocity in Foothold Adaptations for Dynamic Legged Locomotion via CNNs, CLAWAR, 2020.

56

What is the problem?

57

Pose Adaptation

• The goal is to find the optimal robot's pose that can:

  • Maximize the reachablity of the selected footholds.

  • Avoid terrain collision and maintain static stability.

• An pose optimizer to deal with these objectives.

M. Kalakrishnan et al., Learning, planning, and control for quadruped locomotion over challenging terrain, IJRR 2011.
P. Fankhauser et al., Robust Rough-Terrain Locomotion with a Quadrupedal Robot, IROS, 2018.

Kalakrishnan 2011

Fankhauser 2018

58

The Problem with Pose Adaptation

• The problem with current pose adaptation strategies is that they focus on finding one optimal solution based on given selected footholds.

• First, the footholds are selected.

• Then, the pose is optimized based on that.

• What if the given footholds not reached (if the robot gets disturbed)?

• The robot may end up in a state with no reachable safe footholds. 

59

How to Solve This Problem?

60

Vision-Based Terrain Aware Locomotion (ViTAL)

• ViTAL is an online vision-based locomotion planning strategy.

• ViTAL selects footholds based on the robot's skills

• Also, find the robot pose that maximizes the chances of success in reaching these footholds.

• The notion of safety & success emerges from skills that characterize the robot's capabilities.

Foothold Evaluation Criteria (FEC)

62

Foothold Evaluation Criteria (FEC)

63

Vision-Based Foothold Adaptation (VFA)

64

From the VFA to the VPA

65

Vision-Based Pose Adaptation (VPA)

66

Vision-Based Pose Adaptation (VPA)

Then, the pose evaluation stage will evaluate the criteria for all the samples of the hip heights. 

67

Vision-Based Pose Adaptation (VPA)

68

Vision-Based Pose Adaptation (VPA)

69

Pose Optimization

Find the optimal robot's trunk pose that maximizes the number of safe footholds for all the legs

Height

Roll

Pitch

70

Pose Optimization

71

Results: Detailed Explanation

72

Baseline: Terrain-Based Reference (TBR)

O. Villarreal et al., MPC-based Controller with Terrain Insight for Dynamic Legged Locomotion, ICRA, 2020.

73

Results: Comparison with the baseline

74

Results: Comparison with another approach

75

Results: Performance with Different Velocities

76

Preliminary Experiments

78

Preliminary Experiments

78

Conclusions: ViTAL

• We presented ViTAL, a decoupled visual planning strategy

• ViTAL consisted of :

• VFA for foothold selection

• VPA for pose adaptation

  • Introduced a different paradigm in pose adaptation

  • Finds the body poses that maximize the chances of the legs to succeed in reaching safe footholds

• FEC (notion of safety) that characterizes the robot's capabilities

79

Conclusion: ViTAL

• ViTAL allows HyQ and HyQReal to traverse a wide variety of terrains under various forward velocities.

  • These terrains included stairs, gaps, etc.

  • Forward velocities varied from 0.2m/s to 0.75 m/s.

• We showed that the VPA resulted in body poses that are aware of the terrain, and of what the robot and its legs can do.

• The VPA puts the robot in a pose that provides the feet with higher number of safe footholds which allows the robot to succeed unlike the baseline that fails.

80

To be Published:

S. Fahmi, D. Esteban, O. Villarreal, C. Semini, and V. Barasuol, “ViTAL: Terrain-Aware Locomotion Planning for Legged Robots,”  May. 2021.

Outline

• Part I: Proprioceptive Terrain-Aware Locomotion

  • Passive Whole-Body Control (pWBC)

  • STANCE: Locomotion Adaption over Soft Terrain

  • On State Estimation for Regged Robots over Soft Terrain

• Part II: Exteroceptive Terrain-Aware Locomotion

  • ViTAL: Vision-base Terrain-Aware Locomotion

Let legged robots take decisions based on the surrounding terrain using their perception.

Future Work

• Whole-Body Control

  • Relies on inverse dynamics, can we learn these dynamics or contact interaction

• Locomotion over Soft Terrain

  • STANCE made the WBC c3, can we have a c3 MPC-based controller?

  • Improving the state estimator, contact dynamics in legged odometry?

• Vision-based Terrain Aware Locomotion

  • Experiments

  • Analysis of the CNNs

84

List of Publications

• S. Fahmi, D. Esteban, O. Villarreal, C. Semini, and V. Barasuol, "ViTAL: Vision-based Terrain-Aware Locomotion Planning for Legged Robots," to be published, May 2021.

• S. Fahmi, G. Fink, and C. Semini, "On state estimation for legged locomotion over soft terrain," IEEE Sensors Letters (L-SENS), vol. 5, no. 1, pp. 1-4, Jan. 2021.

• D. Esteban, O. Villarreal, S. Fahmi, C. Semini, and V. Barasuol, "On the influence of body velocity in foothold adaptation for dynamic legged locomotion via cnns," in Proc.  International Conference on Climbing and Walking Robots (CLAWAR), Moscow, Russia, Aug. 2020, pp. 353-360.

• † S. Fahmi, M. Focchi, A. Radulescu, G. Fink, V. Barasuol, and C. Semini, "STANCE: Locomotion adaptation over soft terrain," IEEE Transactions on Robotics (T-RO), vol. 36, no. 2, pp. 443-457, Apr. 2020.

• C. Semini, V. Barasuol, M. Focchi, C. Boelens, M. Emara, S. Casella, O. Villarreal, R. Orsolino, G. Fink, S. Fahmi, G. Medrano-Cerda, and D. G. Caldwell, "Brief introduction to the quadruped robot HyQReal," in Italian Conference on Robotics and Intelligent Machines (I-RIM), Rome, Oct. 2019, pp. 1-2.

• S. Fahmi, C. Mastalli, M. Focchi, and C. Semini, "Passive whole-body control for quadruped robots: Experimental validation over challenging terrain," IEEE Robotics and Automation  Letters (RA-L), vol. 4, no. 3, pp. 2553-2560, Jul. 2019.

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

85

On Terrain Aware Locomotion for 

Legged Robots

- SHAMEL FAHMI -

On Terrain Aware Locomotion

for Legged Robots

Shamel Fahmi

Doctoral Thesis Defense

April, 2021