This paper introduces an approach to probabilistically fuse contact models for event-based locomotion in unstructured terrains. It presents a discrete-time extension of the generalized-momentum based disturbance observer to estimate contact force from joint torque. It also describes a Kalman filter method to fuse the estimated contact force with a gait-specific contact schedule and ground height measurements. In experiments, the algorithm identified leg contact with 99.3% accuracy and a small 4-5ms delay when tested on a cheetah robot traversing various terrain conditions including flat, rough ground and small steps.

1. [Review]
Gerardo Bledt, etc. 2018.
Contact Model Fusion for
Event-Based Locomotion in Unstructured Terrains
ModuLabs
강남Dynamics Lab
Hancheol Choi
(babchol@gmail.com)

2. Abstract
Gait pattern and trajectory generation is based-on contact schedule.
In ideal situation, contact schedule is identical to real contact state.
However, in unstructured environment (most of situation;), contact
schedule is different from the real contact state.
 The ability to manage contact transitions will be a critical skill!
This paper introduces an approach to probabilistically fuse contact models.
Contribution of this paper is,
Discrete-time extension of the generalized-momentum based
disturbance observer(GM based DOB),
and Kalman-filter approach fusing probabilistically contact priors with the
other informations.
It shows 99.3% accuracy and a small 4-5ms delay.

3. Periodic Phase-Based State Scheduler
Current leg contact state is defined by boolean variables.
𝑠 ∈ {0 = 𝑠𝑤𝑖𝑛𝑔, 1 = 𝑐𝑜𝑛𝑡𝑎𝑐𝑡}
But, true value of contact state is unknwon.
Base information for true contact state is gait-specific scheduler, 𝑠 𝜙.
Phase signal describes periodic gait pattern with normalized and periodic variable
and its offset.
𝜙 ∈ 0,1 , 𝜙 =
𝑡 − 𝑡0
𝑇
, 𝜙𝑖 = 𝜙 + 𝜙𝑖,𝑜𝑓𝑓𝑠𝑒𝑡
and 𝑠 𝜙 gives contact state depending on each leg’s phase signal.
𝑠 𝜙 ∈ {0 = 𝑠𝑤𝑖𝑛𝑔, 1 = 𝑐𝑜𝑛𝑡𝑎𝑐𝑡}
Stance state, 𝑐 𝜙 = {𝑠 𝜙 = 1}
Swing state, 𝑐 𝜙 = {𝑠 𝜙 = 0}

4. Contact Sensing: GM based DOB: Continuous Time
Next information for true contact state is contact force at the foot, and this
is related with the disturbance torque at each joint.
Disturbance observer is designed by low-pass filter with cut-off frequency,
𝜆 = 2𝜋𝑓 (𝑓 is cut-off frequency)
 𝜏 𝑑 = 𝜆(𝜏 − 𝜏 𝑑)
(this means the rate of disturbance torque estimate is from error of
disturbance torque.)
(1)(2): 𝜏 𝑑 = 𝜆 𝑀𝑞 + 𝐶𝑞 + 𝑔 − 𝑆 𝑇
𝜏 − 𝜏 𝑑
Integrate (3): 𝜏 𝑑 = 𝜆 𝑀𝑞 + 𝐶𝑞 + 𝑔 − 𝑆 𝑇 𝜏 − 𝜏 𝑑
𝑡
0
𝑑𝑡
- (1)
- (2)
- (4)
- (3)
Joint acceleration is noisy after finite difference from encoder data.

5. Contact Sensing: GM based DOB: Continuous Time
Not to use joint acceleration, introduce integration by parts at inertia term.
𝑀𝑞
𝑡
0
𝑑𝑡 = 𝑀 𝑡 𝑞 𝑡 − 𝑀 0 𝑞 0 − 𝑀 𝑞
𝑡
0
𝑑𝑡
Using (5), and skew-symmetric property 𝑀 − 2𝐶  𝑀 = 𝐶 + 𝐶 𝑇
Final equation of GM based DOB (continuous version)
𝜏 𝑑 = 𝜆𝑝 𝑡 − 𝜆 𝑆 𝑇 𝜏 + 𝐶 𝑇 𝑞 − 𝑔 + 𝜏 𝑑
𝑡
0
𝑑𝑡
- (5)
- (6)
𝑓 𝑔′ 𝑓𝑔(𝑡) 𝑓𝑔(0) 𝑓′𝑔

6. Contact Sensing: GM based DOB: Continuous Time
There is alternative view on this disturbance observer that helps to explain
new results in the discrete-time case.
Introduce new variable 𝑤 = 𝜆𝑝 − 𝜏 𝑑,
This equation is recognized as a low pass filter
With this insight, disturbance torque is,
 Filtered GM and dynamic effects
- (7)
- (8)

7. Contact Sensing: GM based DOB: Discrete Time
Apply discrete time low-pass filter to observer dynamics
𝜏 𝑑 =
1 − 𝛾
1 − 𝛾𝑧−1
𝜏 𝑑 =
1 − 𝛾
1 − 𝛾𝑧−1
𝑀𝑞 + 𝐶𝑞 + 𝑔 − 𝑆 𝑇
𝜏
0 < 𝛾 < 1, 𝛾 = 𝑒−𝜆Δ𝑡, 𝜆 = 2𝜋𝑓
To handle the above acceleration term, Introduce new variable 𝑢
𝑢 =
1 − 𝛾
1 − 𝛾𝑧−1
𝑀𝑞
Apply z-transform,
𝑢 𝑛 = 1 − 𝛾 𝑀 𝑛 𝑞 𝑛 + 𝛾𝑢 𝑛−1
𝛾𝑢 𝑛−1 = 1 − 𝛾 𝛾𝑀 𝑛−1 𝑞 𝑛−1 + 𝛾2
𝑢 𝑛−1
⋮
𝛾 𝑛−1 𝑢1 = 1 − 𝛾 𝛾 𝑛−1 𝑀1 𝑞1 + 𝛾 𝑛 𝑢0
𝑢 𝑛 = 1 − 𝛾 𝛾 𝑛−𝑘 𝑀 𝑘 𝑞 𝑘
𝑛
𝑘=0
+
= 𝑤 𝑛−𝑘

8. Contact Sensing: GM based DOB: Discrete Time
Summation by parts
Apply this to,
= 𝑀 𝑘+1(backward?)
= 𝛽

9. Contact Sensing: GM based DOB: Discrete Time
Discrete low-pass filter to inertia term is,
Finally, disturbance torque is,
Similar structure with continuous time, but 𝛽𝑝 𝑘 is carefully calibrated.
Experiments 15Hz cut-off frequency, sample at 1kHz.
 Continuous time: 8.7N error
 Discrete time: 4.1N error (twice accurate)
Computing contact force by leg jacobian
= 𝑦

10. Probabilistic Contact Model Fusion
Contact state, 𝑠 ∈ {0 = 𝑠𝑤𝑖𝑛𝑔, 1 = 𝑐𝑜𝑛𝑡𝑎𝑐𝑡}
Estimated contact state, 𝑠.
Ideally 𝑠 = 𝑠, but realistically it is NOT.
We can naturally use probabilistic contact states.
To get this probabilistic contact states, fuse
1) Probabilistic contact states from gait schedule
2) Several measurements such as ground height and contact force

11. Probabilistic Contact Model Fusion: Prediction
Prediction: Standart Linear Kalman filter method used.
We simply using current contact probability from gait scheduling.
We can define probability to contact given gait schedule.
gait schedule
Swing phase: low probability to contact
Stance phase: high probability to contact
Stance
Swing
𝜇 𝑐0 𝜇 𝑐1
𝜇 𝑐0
𝜇 𝑐1
𝜎𝑐0
𝜎𝑐1
𝜎𝑐0
𝜎𝑐1

12. Probabilistic Contact Model Fusion: Measurement
Measurement model Two measurements: ground height
and contact force
1) Ground Height
Ground height: initially 0, can be updated by the other
sensor estimation system
Ground roughness
If the foot height is close to, or under the ground height,
contact probability grows!

13. Probabilistic Contact Model Fusion: Measurement
2) Contact Force
We can define contact probability depending on contact force
Average contact force sensed at the initiation of contact (contact threshold)
Measurement noise of contact force
If the contact force is over average contact threshold,
contact probability grows!

14. Simulation & Experiments
A simulation was run in various scenarios including trotting on flat even ground, on
random rough terrain, and up a small step.
To define contact situation, use probability of contact, 𝑃𝐶(𝑐), as decision threshold
Optimization was run offline using fmincon in MATLAB, to find best parameters.
Result is,
In the experiments on Cheetah 3,
the algorithm can identify contact 99.3%
and small 4-5 ms delay occurred
Prediction uncertainty from gait schedule
Measurement noise from ground height
Measurement noise from contact force

15. Event-based gait switching
Contact state FSM
Swing  Contact Transition
(double check using some delay time)
- Normal
- Early
- Late: wait until contact detection
Contact  Swing Transition
: follow gait schedule, but contact time 𝑡0 is reset by contact detection

16. Reference
[1] G. Bledt, P.M. Wensing, S. Ingeroll, and S. Kim, “Contact Model Fusion for
Event-Based Locomotion in Unstructured Terrains”, in ICRA, 2018.