ANYmal at the ARGOS Challenge
Tools and Experiences from the Autonomous
Inspection of Oil & Gas Sites with a Legged Robot
Péter Fankhauser
Remo Diethelm, Samuel Bachmann, Christian Gehring, Martin Wermelinger, Dario Bellicoso,
Vassilios Tsounis, Andreas Lauber, Michael Bloesch, Philipp Leemann, Gabriel Hottiger,
Dominik Jud, Ralf Kaestner, Linus Isler, Mark Hoepflinger, Roland Siegwart, Marco Hutter

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

1

ARGOS Challenge“Creating the first autonomous robot for gas and oil sites”

Finals

1st Competition
2nd Competition

2015

2016

2017

http://www.argos-challenge.com

We are here

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

3

ARGOS Challenge
Autonomous Inspection of Oil and Gas Sites
DESCRIPTION OF THE ENVIRONMENTDESCRIPTION OF THE TESTING GROUND
The performances of the robot systems will be evaluated during the 3rd competition in thesame decommissioned unit representative of a typical TOTAL production site, i.e. the UMAD
facility.

Figure 2: UMAD map

The facility used for the competition is used as a training site for human operators. It now
uses utilities such as water and nitrogen. It is equipped with pumps for circulating these utilities.
Leaks of water can be generated. As it can be seen on the picture (Figure 1), it is an outdoor
facility.
The important elements from the mobility perspective are detailed below. In particular, theUMAD site is a multi-floor facility without elevator between the floors. The robot system will
have to manage this constraint under requirements described in “The Robot System” section
Autonomous Systems Lab(page 18).
A 3D model of the competition site has been provided during the Challenge Kick-Off

Péter Fankhauser|

08.10.2016|

4

ARGOS Challenge
5 International Teams

AIR-K
Japan

ARGONAUTS
Austria & Germany

LIO
Switzerland
FOXIRIS
Spain & Portugal

VIKINGS
FranceAutonomous Systems Lab

Péter Fankhauser|

08.10.2016|

5

ARGOS Challenge
Team LIO

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

6ANYmal – A High-Performance & Versatile Quadrupedal Robot

Main body

Hokuyo laser scanner

Joint actuators

Contact sensors

Pan/tilt inspection head

Protection frame

Aluminium thigh and carbon shankBattery housing

M. Hutter, C. Gehring, D. Jud, A. Lauber, C. D. Bellicoso, V. Tsounis, J. Hwangbo, P. Fankhauser, M. Bloesch, R. Diethelm, and S. Bachmann,“ANYmal - A Highly Mobile and Dynamic Quadrupedal Robot,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|7

ANYdrive – A Integrated, Robust, Torque-Controllable Robot Joint§ Fully integrated§ Accurate position & torque control§ Absolute position sensing§ Programmable controller§ Impact robust§ Hollow-shaft§ Water-proof

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

8

System Overview
12× ANYdrive

IMU

4× Foot contact
sensor

2× Laser range 2× Wide-angle
sensorcameraZoom camera

2× Microphone

Industrial
Wireless

Thermal camera Visible & IR light

Operator PC
Locomotion PC

Navigation PC

Inspection PC

Wireless Router

WiFi

Remote control UI
Manual, supervised control

Visualizations

Localization
ICP, Rovio, Tango etc.

Visual inspection

Sensors, robot state, environment

Legged odometry, ground est.

Terrain mapping

Gauges, levers

MissionState estimation

Locomotion Control
Trotting, crawling, Free Gait

ROS interface

Elevation mapping, traversability est.

Motion planning
Navigation, foothold selection

Mission execution

Thermal inspection
Audio inspection
Pump & alarm sounds

Ethernet

RT

Mission creating and protocol

Radio

Safety operator

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|9Autonomous Systems Lab

Robot
torques

Actuation

Jointcommands
Joint

Whole-Body Control
stateState Estimation

Robot

Sensormeasurements

Locomotion

Péter Fankhauser|

08.10.2016|10Locomotion
State Estimation
Extended Kalman Filter§ No assumption on terrain

M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation for Legged Robots on Unstable and Slippery Terrain”,
in International Conference on Intelligent Robots and Systems (IROS), 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|11Locomotion
State Estimation
Extended Kalman Filter§ No assumption on terrain§ Kinematic measurements(encoders) for legs in contact

M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation for Legged Robots on Unstable and Slippery Terrain”,
in International Conference on Intelligent Robots and Systems (IROS), 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|11Locomotion
State Estimation
Extended Kalman Filter§ No assumption on terrain§ Kinematic measurements(encoders) for legs in contact

Kinematic measurements
M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation for Legged Robots on Unstable and Slippery Terrain”,
in International Conference on Intelligent Robots and Systems (IROS), 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|11Locomotion
State Estimation
Inertial measurements

Extended Kalman Filter§ No assumption on terrain§ Kinematic measurements(encoders) for legs in contact§ Fused with inertial
measurements (IMU)§ Error < 5% over distance

Kinematic measurements
M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation for Legged Robots on Unstable and Slippery Terrain”,
in International Conference on Intelligent Robots and Systems (IROS), 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|11Locomotion
State Estimation
Inertial measurements

Ope

nS
g
i
t
our
hub
gen
ce
.com
eral
ized
_inf /ethz-aormsatio l/n_fi
lter

External posemeasurements

Extended Kalman Filter§ No assumption on terrain§ Kinematic measurements(encoders) for legs in contact§ Fused with inertial
measurements (IMU)§ Error < 5% over distance

Kinematic measurements§ Optionally combined withexternal pose (GPS, laser,
vision, etc.)

M. Bloesch, C. Gehring, P. Fankhauser, M. Hutter, M. A. Hoepflinger and R. Siegwart, “State Estimation for Legged Robots on Unstable and Slippery Terrain”,
in International Conference on Intelligent Robots and Systems (IROS), 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|11Locomotion
Whole-Body Control

Ope
(com

ing

Locomotion Controller Modules (Loco)

our

ce

soo

n)

Robot Controller Manager (Rocoma)
Controller Layer

Virtual model
controlPose
optimization

Reflexes

Trajectory
optimization

Whole-Body
ControlGaitpatterns

Zero-Moment
Point Traj.

Contact force
distribution…

Trotting

Emergency
Controller A

C. Gehring, S. Coros, M. Hutter, D. Bellicoso, H. Heijnen, R. Diethelm, M. Bloesch, P.
Fankhauser, J. Hwangbo, M. A. Hoepflinger, and R. Siegwart, “Practice Makes Perfect: An
Optimization-Based Approach to Controlling Agile Motions for a Quadruped Robot.”, inIEEE Robotics & Automation Magazine, 2016.
Autonomous Systems Lab

nS

Crawling

Free Gait…

Emergency
Controller B

First
Emergency
Layer

Fail-proof
Controller

SecondEmergency
Layer

C. Dario Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, M. Hutter, “Emerging Terrain
Adaptation from Hierarchical Whole Body Control,”in IEEE Internal Conference on Humanoid Robots (Humanoids), 2016.

Péter Fankhauser|

08.10.2016|12

Open Source

github.com/legg
edrobot
free_gaics/
it (in pro
gress)

Locomotion
Free Gait – An Architecture for the Versatile Control of Legged Robots§ Abstraction Layer for Whole-Body
Motions (Free Gait API)P. Fankhauser, D. Bellicoso, C. Gehring, R. Dubé, A. Gawel, and M. Hutter, “Free Gait – An Architecture for the Versatile Control of Legged Robots,”
in IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2016.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|13

Open Source

github.com/legg
edrobot
free_gaics/
it (in pro
gress)

Locomotion
Free Gait – An Architecture for the Versatile Control of Legged Robots
Base frame

try
e
m
do
O
ed
g
g
Le

Base motion§ Abstraction Layer for Whole-Body
Motions (Free Gait API)
§ Robust motion execution in task space

Odometry frame

ion
t
a
z
i
l
a
Loc

Leg
motionGlobal frame
Support legs

P. Fankhauser, D. Bellicoso, C. Gehring, R. Dubé, A. Gawel, and M. Hutter, “Free Gait – An Architecture for the Versatile Control of Legged Robots,”
in IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2016.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|13

Open Source

github.com/legg
edrobot
free_gaics/
it (in pro
gress)

Locomotion
Free Gait – An Architecture for the Versatile Control of Legged Robots
steps:- step:- base_auto:- step:- end_effector_target:
name: RF_LEG
ignore_contact: true
target_position:frame: footprint
position: [0.39, -0.24, 0.20]- step:- base_auto:
height: 0.38
ignore_timing_of_leg_motion: true
- end_effector_target: &footname: RF_LEG
ignore_contact: true
ignore_for_pose_adaptation: true
target_position:frame: footprint
position: [0.39, -0.24, 0.20]- step:- base_auto:
height: 0.45
ignore_timing_of_leg_motion: true
- end_effector_target: *foot- step:- footstep:
name: RF_LEG
profile_type: straighttarget:frame: footprint
position: [0.32, -0.24, 0.0]
- step:- base_auto:

Base frame

try
e
m
do
O
ed
g
g
Le

Base motion§ Abstraction Layer for Whole-Body
Motions (Free Gait API)
§ Robust motion execution in task space

Odometry frame

ion
t
a
z
i
l
a
Loc

Leg
motionGlobal frame§ Implemented as ROS Action (withframeworks for YAML, Python, C++)

Support legs

P. Fankhauser, D. Bellicoso, C. Gehring, R. Dubé, A. Gawel, and M. Hutter, “Free Gait – An Architecture for the Versatile Control of Legged Robots,”
in IEEE-RAS International Conference on Humanoid Robots (Humanoids), 2016.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|13Locomotion
Kindr – Kinematics and Dynamics for Robotics§ C++ library for the consistent handling of
3d position and rotations§ Support for rotation matrices, quaternions,angle-axis, rotation vectors, Euler angles,
etc.

gith

Kindr Library – Kinematics and Dynamics for Robotics(Hyper-)complex number
Column vector
MatrixIdentity matrix
Coordinate system (CS)
Inertial frame
Body-fixed frame
Rotation
Machine precisionQ
normal capital letterabold small letterMbold capital letter1n⇥mn ⇥ m-matrix
AAA
ex , ey , ez Cartesian right-hand system A with basis (unit) vectors eeIx , eIy , eIz global / inertial / world coordinate system (never moves)BBeB
x , ey , ez local / body-fixed coordinate system (moves with body)
2 SO(3) generic rotation (for all parameterizations)✏

Cross product/skew/unskew

Euclidean norm
Exponential map for matrix
Logarithmic map for matrix

2

2
2 3
a1
0
b1
^
4
5
4
5
a ⇥ b = a2 ⇥ b2 = (a) b = âb = 4 a3a3b3a2
âT , a ⇥ b = b ⇥ a
a = â_ , â = qp
kak = aT a = a21 + . . . + a2n
3

32 3
a2
b1
a 1 5 4 b2 5
0
b3

a3
0
a1

exp : R3⇥3 ! R3⇥3 , A 7! eA , A 2 R3⇥3
log : R3⇥3 ! R3⇥3 , A 7! log A, A 2 R3⇥3

Position & Orientation
Position§ Support for all common operations andincludes time-derivates

VectorPosition vector
Homogeneous pos. vector

Orientation/Rotation
1) Active Rotation:2) Passive Rotation:3) Elementary Rotations§ Based on Eigen, 1000+ unit tests
M. Bloesch, H. Sommer, T. Laidlow, M. Burri, G. Nuetzi, P. Fankhauser, D. Bellicoso, C. Gehring,
S. Leutenegger, M. Hutter, R. Siegwart, “A Primer on the Diﬀerential Calculus of 3D Orientations,”in arXiv:1606.05285, 2016.
Autonomous Systems Lab

A :
P :

around x-axis:

6) Exponential map:
7) Logarithmic map:
8) Box plus:
9) Box minus:
10) Discrete integration:11) Discrete diﬀerential:
12) (Spherical) linear interpolation t 2 [0, 1]:

1

I rOP = CIB B rOP

around y-axis:

5) Concatenation:from point O to point P
from point O to point P expr. in frame B
from point O to point P expr. in frame B⇤T

I rOP 7! I rOQ (rotates the vector rOP )II
I
I rOP 7! B rOP (rotates the frame (ex , ey , ez ))around z-axis:

4) Inversion:§ ROS interface available

rOP
3
B rOP 2 R⇥ T
B r̄OP = B rOP

A 1 (r) =
A
2

⇣

P (r)

⌘

=

⇣

A
2 ⌦

A
1

⌘

(r) =

⇣

eB
y

O

A 1
⌦
1

✓

✓

eIx

eB
x
A
2

P
I rOP , B rOP

⌘
1

1

(r)

=(

P
1 (r)

1⌦

⌘

0

1 t

) ⌦

0

ethz

Rotation Matrix

eIy
Q
I rOQ , B rOQ

ce

-asl

/kin

dr

CIB 2 SO(3)The rotation matrix (Direction Cosine Matrix)
I rOP = CIB B rOP is a coordinate transformation matrix,
Twhich transforms vectors from frame B to frame I.
CIB = CBIqIB = [q0 q1 q2 q3 ]T Hamiltonian unit quaternion (hypercomplex number)
Q = q0 + q1 i + q2 j + q3 k 2 H, qi 2 R, kQk = 1(✓, n)IB
Rotation with unit rotation axis n and angle ✓ 2 [0, ⇡].

Rotation
Quaternion
Angle-axis
Rotation Vector

Rotation with rotation axis n =IBEuler Angles ZYX
Euler Angles YPR[z, y, x]TIB

Euler Angles XYZ
Euler Angles RPY[x, y, z]TIB

A rotation quaternion is a Hamiltonian unit quaternion:
Q = q0 + q1 i + q2 j + q3 k 2 H,kk

and angle ✓ = k k.

Tait-Bryan angles (Flight conv.): z y 0 x00 , i.e.
yaw-pitch-roll. Singularities are at y = ± ⇡2 .
z 2 [ ⇡, ⇡), y 2 [ ⇡2 , ⇡2 ), x 2 [ ⇡, ⇡)Cardan angles: x y 0 z 00 , i.e. roll-pitch-yaw.
Singularities are at y = ± ⇡2 .
x 2 [ ⇡, ⇡), y 2 [ ⇡2 , ⇡2 ), z 2 [ ⇡, ⇡)qi 2 R, i2 = j 2 = k2 = ijk =

kQk =

1,

q
q02 + q12 + q22 + q32 = 1

Tuple:Q = (q0 , q1 , q2 , q3 ) = (q0 , q̌) with q̌ := (q1 , q2 , q3 )T⇤T⇥
4 ⇥ 1-vector:
q = q0 q 1 q 2 q 3⇤
Conjugate:
Q = (q0 , q̌)Inverse:Q 1 = Q⇤ = (q0 , q̌)Quaternion multiplication:
Q · P = (q0 , q̌) · (p0 , p̌) = (q0 p0 q̌T p̌, q0 p̌ + p0 q̌ + q̌ ⇥ p̌) ,
10 1
0 1 0
q0
p0
q1
q2
q3
✓
◆ p0
T
C
B
B C
B
q0
q̌
q0
q3
q2 C
C Bp 1 C
B p 1 C = Bq 1
q ⌦ p = Q(q) p =ˆ @ p 2 A @q 2
q3
q0
q 1 A @p 2 A
q̌ q0 13⇥3 + q̌
| {z }
quaternion matrix
p3
q3
q2
q1
q0
p3
10 1
0 1 0
p
q0
q
p
p
p
0
0
1
2
3
✓
◆
Bq1 C
B q 1 C Bp 1
p0
p̌T
p0
p3
p2 C
C
B
C
B
C
B
= Q̄(p) q =ˆ @ q 2 A = @p 2
p3
p0
p 1 A @q2 Ap̌ p0 13⇥3 p̌
| {z }
conjugate quat. matrix
q3
p3
p2
p1
p0
q3
Note that QIB and QIB represent the same rotation, but not the same unit quaternion.

Rotation Quaternion , Rotation Vector

eIz eB
z

⌘
⇣
⌘ 1
⇣
1
P
P
P 1
=
⌦ P
(r)
2 ⌦ 1 (r) =1
2
exp : R3 ! SO(3), v 7! exp(v̂), v 2 R3
log : SO(3) ! R3 , 7! log( )_ ,2 SO(3)3v = exp (v) ⌦ 1 ,
2 = 1
1 , 2 2 SO(3), v 2 R
13
v= 1
2 = log ( 1 ⌦ 2 ),1 , 2 2 SO(3), v 2 R
k+1
k+1
k
k
k
k
=
(
!
t),
=
(
!
t)
I
B
IB
IB
BI
IB
IB
BI
k+1
k+1
k
k )/ t,kk )/ tI ! IB = ( IB
B ! IB = ( BI
IB
BI
(( 1
t = 0
t = (t), 0 = (0), 1 = (1)0 )t) ,
P
2

⇣

A
1 (r)

3
cos ✓
sin ✓
0
cos ✓
05
CIB = 4 sin ✓
0
13
2 0
cos ✓
0
sin ✓
0
1
0 5
CIB = 40cos ✓ 32 sin ✓
100
cos ✓sin ✓ 5
CIB = 40
0
sin ✓
cos ✓
2

om/

our

Rotation Parameterizations

Rotation Quaternion

Operators

nS

ub.c

Christian Gehring, C. Dario Bellicoso, Michael Bloesch, Hannes Sommer, Peter Fankhauser,
Marco Hutter, Roland Siegwart

Nomenclature

Ope

qIB =

8 
>
<

cos ( 12 k k),k⇥>: 1, 1 T ⇤T
2

T

k

T

sin ( 12 k k)

if k k✏otherwise,

Rotation Quat ernion , Angle-Axis
qIB =



cos ✓2n sin ✓2,(✓, n)IB =(q̌
)(2 arccos (q0 ), kq̌k⇥⇤T(0, 1 0 0 )IB =if kq̌k(q̌
2 atan2 (kq̌k, q0 ) kq̌k
2 sign(q0 )q̌

if kq̌k ✏otherwise✏otherwise

Rotation Quaternion , Rotation Matrix

ˆ + 2q̌ˆ 2 = (2q02 1)13⇥3 + 2q0 q̌
ˆ + 2q̌q̌T
CIB = 13⇥3 + 2q0 q̌
3
2 2
2
2
2
2q1 q2 2q0 q3
2q0 q2 + 2q1 q3
q0 + q 1 q 2 q 32222
q0 q 1 + q 2 q 3
2q2 q3 2q0 q1 5
= 4 2q0 q3 + 2q1 q2
2q1 q3 2q0 q2
2q0 q1 + 2q2 q3
q02 q12 q22 + q32
ˆ + 2q̌ˆ2
CIB1 = CBI = 13⇥3 2q0 q̌
3
2 2
2
2
2
q0 + q1 q2 q3
2q0 q3 + 2q1 q2
2q1 q3 2q0 q22222
q0 q 1 + q 2 q 3
2q0 q1 + 2q2 q3 5
= 4 2q1 q2 2q0 q3
2q0 q2 + 2q1 q3
2q2 q3 2q0 q1
q02 q12 q22 + q32

Cheat-sheet (incl. derivations)Péter Fankhauser|

08.10.2016|14

Navigation
Laser Range Data

Localization

Elevation Mapping

Traversability Est.

Navigation Planning

Execution

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|15

Navigation
Laser-Based Localization (Iterative Closest Point (ICP))

Ope

nS
gith
our
u
b
ce
ethz .com/easl_
icp_ thz-as
l/
map
ping

§ Point cloud registration for localizationin reference map§ Full rotation of LiDAR is aggregatedfor point cloud§ Use of existing maps or online
mapping

Pomerleau, F., Colas, F., Siegwart, R., Magnenat, S., “Comparing ICP variants on real-world data sets”, in Autonomous Robots, 2013.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|16

Navigation
Elevation Mapping – Dense Terrain Mapping

Ope

nS
gith
our
ub.c
ce
om/
elev
ethz
atio
n_m
-asl
/
app
ing

§ Probabilistic fusion of range
measurements and pose estimation§ Explicitly handles drift of state
estimation (robot-centric)§ Input data from laser, Kinect, stereo
cameras, Velodyne etc.

P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, R. Siegwart “Robot-Centric Elevation Mapping with Uncertainty Estimates,” inInternational Conference on Climbing and Walking Robots (CLAWAR), 2014.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|17

Navigation
Grid Map – Universal Multi-Layer Grid Map Library

gith

ub.c

Ope
om/

nS

our

ethz

-asl

ce

/grid

_ma

p

Collision
checkingp

LiDAR / Kinect

i

Gr

Poin
clou tdStereo vision

a
dm

Depth
image

Footholdscoring

ap

m
Grid

Elevation
Mapping

Grid map
Grid

ose

P

map

Gr

id

Robot pose

Classificationmap

Traversability
estimation

Visualization

multiple layers

setPosition(…)
one cell on
diﬀerent layersmove(…)

Height, uncertainty, surface normal,roughness, color, friction, traversability …
grid map frame

P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,”in Robot Operating System (ROS) - The Complete Reference, Springer, 2015.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|18

Navigation
Grid Map – Universal Multi-Layer Grid Map Library

gith

ub.c

Ope
om/

nS

our

ethz

ce

-asl

/grid

_ma

p

§ 2D circular buﬀer data structure➡ Eﬃcient map repositioning

setPosition(…)

move(…)

P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,”in Robot Operating System (ROS) - The Complete Reference, Springer, 2015.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|18

Navigation
Grid Map – Universal Multi-Layer Grid Map Library

gith

ub.c

Ope
om/

nS

our

ethz

ce

-asl

/grid

_ma

p

§ 2D circular buﬀer data structure➡ Eﬃcient map repositioning§ Based on Eigen (C++)➡ Versatile and eﬃcient data manipulation
double rmse =sqrt(map["error"].array().pow(2).sum() / nCells);

P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,”in Robot Operating System (ROS) - The Complete Reference, Springer, 2015.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|18

Navigation
Grid Map – Universal Multi-Layer Grid Map Library

gith

ub.c

Ope
om/

nS

our

ethz

ce

-asl

/grid

_ma

p

§ 2D circular buﬀer data structure➡ Eﬃcient map repositioning§ Based on Eigen (C++)➡ Versatile and eﬃcient data manipulation§ Convenience functions➡ Iterators, math tools, etc.

P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,”in Robot Operating System (ROS) - The Complete Reference, Springer, 2015.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|18

Navigation
Grid Map – Universal Multi-Layer Grid Map Library

gith

Ope

ub.c

om/

nS

our

ethz

ce

-asl

/grid

_ma

p

§ 2D circular buﬀer data structure➡ Eﬃcient map repositioning§ Based on Eigen (C++)➡ Versatile and eﬃcient data manipulation§ Convenience functions➡ Iterators, math tools, etc.

§ ROS & OpenCV interfaces➡ Conversion from/to images, point clouds,
occupancy grids, grid cells
P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,”in Robot Operating System (ROS) - The Complete Reference, Springer, 2015.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|18

Navigation
Traversability Estimation

Ope

nS
gith
our
trav ub.co
ce
ersa
m/e
th
bilit
y_e z-asl/
stim
atio
n

Slope

Roughness∑

Elevation map

Traversability map
Step height

M. Wermelinger, P. Fankhauser, R. Diethelm, P. Krüsi, R. Siegwart, M. Hutter, “Navigation Planning for Legged Robots in
Challenging Terrain,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016.

Autonomous Systems Lab

Not
traversable

Fully
traversable

01

Péter Fankhauser|

08.10.2016|19

Navigation
Navigation Planning§ Online navigation planning basedon RRT* (OMPL)§ Works with and without initial map§ Continuous for changing
environments

M. Wermelinger, P. Fankhauser, R. Diethelm, P. Krüsi, R. Siegwart, M. Hutter, “Navigation Planning for Legged Robots in
Challenging Terrain,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016.

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|20

A.2.3. Checkpoint 13: thermal point 3
A.1.10. Checkpoint 9: Level gauge 1

point 1: pressure gauge 1

Inspection

cal axis from West walkway;
nge: 1 to 2 bar;
re to accuracy of 0.2 bar.

Robot to measure tank level 0% (empty) to 100% (full);Robot to read tank level to accuracy of 10%.

Robot will have to measure the temperature on the hot spot located on the middle pipe at thelocation pointed by the arrow.
ARGOS CHALLENGE

A.1.7. Checkpoint 6: valve R24
During normal operation this valve is in the normally open (NO) position. Note the photographs
and existing 3D model show this valve in the closed position.

Visual inspection

Thermal Inspection

Auditive Inspection

Project LIO

DETECTION AND ANALYSIS (REQ. 3)
This section is individually discussed in the following subrequirements.

Pressure&

Level gauges

Zoom-camera

Autonomous Systems Lab

ValvesThermal points
Thermalcamera

Pumps

GENERAL PLATFORM ALARM (GPA, SUB REQ. 3-1)
To detect the platform alarm, a piece of audio of length t gets recorded. From thisthe Fast Fourier Transform to obtain the spectrum of the signal. The spectrum issignal- and noise-bins. The signal-bins have the locations, received by the Fourier
sawtooth wave, n times the fundamental frequency. Therefore the remaining fr
considered as noise. With this strong approximation we can calculate a signal noise rinterpreted as a quantity to measure the presence of the signal. With increasing t
resolution in the frequency domain which leads to smaller bins and ultimately
selective algorithm. The power inside these bins can now more precisely be attribu
which leads to a better result. However, with a longer time window the power in t
also increases while the power in the noise-bins might be more distributed over tim
time window size are parameters which can be chosen freely by the operator. The
threshold value is depending on t. Resulting from our analysis of the algorithm's
window size of five seconds can be considered a reasonable setting.

Gas leaks

Platform alarm

Microphones (audible and ultra-sonic)

21 noise
Péter
Fankhauser
Figure 20 Spectrogram
plots,a) alarm,| b)08.10.2016
alarm with| robot

Inspection
Figure 27 Matching and de-warping example. On the left hand side is the known tem
indicate matched
SIFT features, the green box visualises the homography and theVisual Inspectionlinesof Pressure
GaugesA.1.9. Checkpoint 8: pressure gauge 5

al operating range: 1 to 2bar;
to read pressure to accuracy of 0.5 bar.

the right hand side shows the de-warped and rotated sensor dial, ready to be read.

view point generation
Visual servoing
Indicator readingꇝ AutomaticꇟꇡFigure 27 Matching and de-warping example. On the left hand side is the known template, theIn casethetheSIFT
does
work, either
lines indicate matched
SIFT thatfeatures,greenmatchingbox visualises
the not
homography
and the because
image on the manometer is rotated totothelighting
issues,a backup
strategy
implemented.
The idea is to use the predefined
the right hand sidedue
shows
de-warped
and rotatedsensor dial,
ready is
to be
read.

and rotation of the dial to de-warp the dial manually without the SIFT features. However, tIn case that the SIFT matching does not work, either because the manometer is rotated too much or
works
only
if
the
actualmanometerorientation
doesn'tdeviate
too
much
from
the
due to lighting issues, a backup strategy is implemented. The idea is to use the predefined orientation
thewithout
resultthewill
befeatures.
a unusablewrong
de-warped image.
and rotation of the orientation.
dial to de-warp Otherwise,
the dial manually
SIFT
However,
this method
Checkpoint 8: pressure gauge 5
works only if the actual manometer orientation doesn't deviate too much from the predefined NormalA.1.9.
operating range: 1 to 2bar;
Robot to read pressure to accuracy of 0.5 bar.
orientation. Otherwise, the result will be a unusable wrong de-warped image.
➔ Reading okꇞ Whole-body camera positioningꇠ Image de-warping➔ Reading unsuccessful, try alternative
position or report as unknown
S. Bachmann, “Visual Inspection of Manometers and Valve Levers”, Master’s Thesis, ETH Zurich, 2015.

Figure 28 The left image is the original recorded image and the other one is manually de-warped.
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

22

Figure 28 The left image is the original recorded image and the other one is manually de-Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

User Interface

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.
Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

Interface for remote control, semi-, and full autonomous operation.

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.

Figure 15 Interactive control of PTZ

Figure 15 Interactive control of PTZ

13/72
13/72

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

23

Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

User Interface

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.
Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

Interface for remote control, semi-, and full autonomous operation.

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.

Situational
cameraInspection
cameras3D view (RViz)

Figure 15 Interactive control of PTZ

Figure 15 Interactive control of PTZ

13/72

Robot actuators& sensors

13/72

Mission control& protocol
Error protocol
Other modules
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

23

Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

User Interface

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.
Figure 14 Pan-tilt-zoom, illumination, video recording and image capturing

Interface for remote control, semi-, and full autonomous operation.

We furthermore reconstruct the environment from the multiple cameras around the robot
giving the operator an overview of its surrounding (e.g. to search something). The operator
can click in the image and select areas of interest where the zoom camera provides a detailedview.

Situational
cameraInspection
cameras3D view (RViz)

Figure 15 Interactive control of PTZ

Figure 15 Interactive control of PTZ

13/72

Robot actuators& sensors

13/72

Mission control& protocol
Error protocol
Other modules
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

23

User Interface
Bandwidth Considerations§ Only critical data is transmitted by default(robot state and position)§ Other data is transmitted on demand(video, maps, etc.)§ Separation of onboard TF and operator TF

Robot
High-frequency:/tf
Low-frequency:/tf_minimal

Operator/tf
Remapping
Networktransfer/tf_minimal§ Connection status node monitors WiFi
status and triggers recovery behavior

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

24

User Interface
ANYping Indicator

gith

Ope

nS
ub.c
our
om/
ce
any
legg
_pin
e
g_in drobo
tics
dica
/
tor

§ Indicates PC network availability in
Ubuntu menu bar

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

25

User Interface
Pose Graph§ Pose graph for inspection,
special maneuvers (e.g. stairs),
docking station etc.
§ Visualization and interactive
editing of pose graph§ Continuous updating and(re-)planning on pose graphduring mission

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

26

User Interface
Mission Creation§ Task-level state machine (C++ library,similar to SMACH)§ State machine defined in YAML format§ Common building blocks to facilitate
construction

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|

27

User Interface
Mission Creation§ Task-level state machine (C++ library,similar to SMACH)§ State machine defined in YAML format§ Common building blocks to facilitate
construction§ Typical missions programmed in
5–20 minutes

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016

|

27

Ope

RQT Multiplot Plugin & Variant Topic Tools

nS

gith
our
ub.c
ce
rqt_
om/
mul
e
tiplo thz-as
l/
t_pl
ugin

gith

Ope

nS

ub.c

om/

ethz

our

-asl

ce

/var

iant

§ C++ library (alternative to rqt_plot)§ Multiple plots in one window§ Edit, save, and load configurations§ Live plotting or load rosbags

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016

|

28

Ope

RQT Multiplot Plugin & Variant Topic Tools

nS

gith
our
ub.c
ce
rqt_
om/
mul
e
tiplo thz-as
l/
t_pl
ugin

gith

Ope

nS

ub.c

om/

ethz

our

-asl

ce

/var

iant

§ C++ library (alternative to rqt_plot)§ Multiple plots in one window§ Edit, save, and load configurations§ Live plotting or load rosbags§ Easy to setup configurations

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016

|

28

Ope

RQT Multiplot Plugin & Variant Topic Tools

nS

gith
our
ub.c
ce
rqt_
om/
mul
e
tiplo thz-as
l/
t_pl
ugin

gith

Ope

nS

ub.c

om/

our

ethz

-asl

ce

/var

iant

§ C++ library (alternative to rqt_plot)§ Multiple plots in one window§ Edit, save, and load configurations§ Live plotting or load rosbags§ Easy to setup configurations§ x- and y-axis freely configurable

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|28

Operator PC
Locomotion PC✓

Navigation PC✓
Autonomous Systems Lab

Inspection PC✓✓
Péter Fankhauser|

08.10.2016|29

Software Tools – How We (Try) To Keep Things Smooth§ All developers and robots same setup➡ Ubuntu 14.04 LTS, ROS Indigo

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|30

Software Tools – How We (Try) To Keep Things Smooth§ All developers and robots same setup➡ Ubuntu 14.04 LTS, ROS Indigo§ Software version control with Git➡ Bitbucket & GitHub

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|30

Software Tools – How We (Try) To Keep Things Smooth§ All developers and robots same setup➡ Ubuntu 14.04 LTS, ROS Indigo§ Software version control with Git➡ Bitbucket & GitHub§ Conventions for package structure,format, naming, and code style➡ github.com/ethz-asl/ros_best_practices/wiki

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|30

Software Tools – How We (Try) To Keep Things Smooth§ All developers and robots same setup➡ Ubuntu 14.04 LTS, ROS Indigo§ Software version control with Git➡ Bitbucket & GitHub§ Conventions for package structure,format, naming, and code style➡ github.com/ethz-asl/ros_best_practices/wiki§ Extensive use of simulation➡ Gazebo

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|30

Software Tools – How We (Try) To Keep Things Smooth§ All developers and robots same setup➡ Ubuntu 14.04 LTS, ROS Indigo§ Software version control with Git➡ Bitbucket & GitHub§ Conventions for package structure,format, naming, and code style➡ github.com/ethz-asl/ros_best_practices/wiki§ Extensive use of simulation➡ Gazebo§ Visualizing as much as possible
Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|30

Software Tools – How We (Try) To Keep Things Smooth§ Lots of tests on hardware➡ Weekly “shakeouts” for defined tasks➡ Lots of demos

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|31

Software Tools – How We (Try) To Keep Things Smooth§ Lots of tests on hardware➡ Weekly “shakeouts” for defined tasks➡ Lots of demos§ Continuous Integration➡ Jenkins➡ Unit tests (after each change)➡ ROS integration tests (at night)

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|31

Software Tools – How We (Try) To Keep Things Smooth§ Lots of tests on hardware➡ Weekly “shakeouts” for defined tasks➡ Lots of demos§ Continuous Integration➡ Jenkins➡ Unit tests (after each change)➡ ROS integration tests (at night)

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|31

Software Tools – How We (Try) To Keep Things Smooth§ Lots of tests on hardware➡ Weekly “shakeouts” for defined tasks➡ Lots of demos§ Continuous Integration➡ Jenkins➡ Unit tests (after each change)➡ ROS integration tests (at night)§ Logging (rosbag)➡ All important information is always logged➡ Review logs with RViz and RQT Multiplot

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|31

Conclusion§ Introduced 10 open-source packages, 250+ internal packages§ Coordination of a big team is hard
§ Good naming is important§ ROS as “glue”§ WiFi is often problematic§ Reliability is crucial

Autonomous Systems Lab

Péter Fankhauser|

08.10.2016|32

Thank You
Open-Source Software
github.com/ethz-asl
github.com/leggedrobotics
Péter Fankhauser
pfankhauser@ethz.ch

Autonomous Systems Lab
www.rsl.ethz.ch

Samuel Bachmann
Dario Bellicoso
Michael Bloesch
Remo Diethelm
Christian Gehring
Mark Hoepflinger
Gabriel Hottiger
Marco Hutter
Jemin Hwangbo
Linus Isler

Dominic Jud
Ralf Kaestner
Bruno Kaufmann
Philipp Krüsi
Andreas LauberPhilipp Leemann
Konrad Meyer
Roland Siegwart
Vassilios Tsounis
Martin Wermelinger

www.asl.ethz.ch
Péter Fankhauser|

08.10.2016|33