Towards Capable Open Source ROS- Frameworks for Real … · 09/08/2016 · Towards Capable Open Source ROS-Frameworks for Real-Time Perception, World Modeling and Footstep Planning

Towards Capable Open Source ROS-Frameworks for Real-Time Perception, World Modeling and Footstep Planning for Humanoid and Legged Robots

Alexander Stumpf

27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 1

New Generation of Humanoids

• Since the DARPA Robotics Challenge (2013) many new high capable full size humanoids have been introduced


Atlas

ESCHER

WALK-MAN

DRC-Hubo

THORMANG3

(Robotis)

1. How to leverage this new locomotion capabilities towards

real world scenarios?

2. How to prevent reinvetion of the wheel?

The Mission: Rough Terrain Challenge (DRC)

How to perform challenging terrain?

• Complex Terrain Tasks requires:

3D perception and modeling of environment

Generate safe sequence of foot placements

Foot placements in 6 DoF

Navigation through obstacles

high versatile locomotion capabilities

robust stabilizer controllers

• Divide & Conquer:

Terrain Model Generation

3D Footstep Planning

Walking Controller


Sense, Plan, Act and Human-in-the-loop


Sense

Plan

Act Supervision

A lot of progress recent years



Sense

Plan

Act Supervision

Our Focus is on Plan, Sense and

Supervision



Sense

Plan

Act Supervision

2D Footstep Planning Classic Approach

• The presented work is based on results from Hornung et. al. [1]

Using A*-search-based planning approach

Implicit model, capability determined by experiments


Successor Set

𝑠′ = (𝑥′, 𝑦′, 𝜃′)

𝑠 = (𝑥, 𝑦, 𝜃)

𝑎 = (△ 𝑥,△ 𝑦,△ 𝜃)

Discrete Foot Placements

[1] AnytimeSearch-based footstep planning with Suboptimality Bounds, Hornung et. al., 2012 14th IEEE-

RAS International Conference on Humanoid Robots

First steps (Atlas v3 - 2014) Autonomous Door Passing


3D Footstep Planning

• The presented work is based on results from Hornung et. al. [1]

Using A*-search-based planning approach

Implicit model, capability determined by experiments

• Extension to full 3D [2]

Introduce Terrain Model and …

…novel collision check strategy

Using reachability polygon

Foot Placements in 6 DoF

…


[2] Supervised Footstep Planning for Humanoid Robots in Rough Terrain

Tasks using a Black Box Walking Controller, Alexander Stumpf et. al., 2014 14th IEEE-RAS International

Conference on Humanoid Robots

[1] AnytimeSearch-based footstep planning with Suboptimality Bounds, Hornung et. al., 2012 14th IEEE-

RAS International Conference on Humanoid Robots

Successor Set

𝑠′ = (𝑥′, 𝑦′, 𝜃′)

𝑠 = (𝑥, 𝑦, 𝜃)

𝑎 = (△ 𝑥,△ 𝑦,△ 𝜃)

Discrete Foot Placements

3D Footstep Planning Extension to 3D

• Extension: 3D model

States: 𝑠 = (𝑥, 𝑦, 𝑧, 𝜙, 𝜓, 𝜃)

Action: 𝑎 = (Δ𝑥, Δ𝑦, Δ𝜃)

• States: Becomes full 6 DoF

• Actions: Remains the same!

𝑧, 𝜙 and 𝜓 are constrained by underlying terrain

Search space does not enlarge

- No expensive branching tree!


First steps (Atlas v3 - 2014) Autonomous Stair Climbing


3D Footstep Planning Collision Check Strategy

• Ground contact estimation

Sampling of foot surface

Estimate contact situation of each sample using height map

Suitable collision checking model for 3D

Allows overhanging steps


DRC Trials 2014 Chevron Hurdle


Solution for everyone?

• Solution was implemented based on BDI Atlas library State representation dedicated for Atlas

Cost-functions implemented and tuned for Atlas

…

• Request by 2015: Provide a solution for other teams How to generalize to cope with distinct locomotion

capabilities and robot libraries?


ESCHER

THORMANG

Footstep Planning Data Flow

• Basic data flow:

• What is the challenge?


Footstep Planner Robot

(Walking Controller)

Hard

ware

Inte

rface

Step plan

State Estimator

Operator

Start pose

Goal pose

World Model

Footstep Planning Challenges in Diversity

Challenges come into play, when…

…Walking Controllers require often special data (e.g. intermediate foot trajectory points, convex hull of foot contact, etc.).

…step plan has to follow special policies (e.g. RTR-movement).

…new collision check strategies should be applied.

Therefore,…

• …planner policy must be adaptable.

• …step plan has to compute and carry

custom data.

But do not reinvent the wheel!

Reuse of existing and tested software


Footstep Planning Solution: Extendable Framework by Plugins

Idea: Use plugin system to load individual code dynamically into the planner software.

• Motivation for the vigir_pluginlib package* - Functionality-based plugin management system for ROS


Footstep Planner Robot

(Walking Controller)

Hard

ware

Inte

rface

Step plan

State Estimator

Operator

Start pose

Goal pose

World Model Plugins

*http://wiki.ros.org/vigir_pluginlib

http://wiki.ros.org/vigir_pluginlib


Footstep Planning Framework [3]

Various plugin types has been integrated into planning pipeline:

• Customize planning policy

• Embed individual code

• Preserve existing code

• Easy exchange of code

• Keep efficiency high

Open Source:


http://wiki.ros.org/vigir_footstep_planning

[3] Open Source Integrated 3D Footstep Planning Framework for Humanoid Robots,

Alexander Stumpf et. al., 2016 16th IEEE-RAS International Conference on Humanoid Robots



3D Footstep Planning Experiment: Complex Terrain (Post DRC – 2015)


• Complex terrain scenario: Pitch Ramp and Chevron Hurdle

Completing both resulted in one point at DRC Trials

• Commanding goal behind chevron hurdles

Planning time t = 4.42s (max t = 5s, final = 1.2)

• Robot‘s field of view




• Resulting step plan in robot‘s field of view




Footstep Planning Framework Summary

• Modular, expandable and capable footstep planning framework

• Cope with different robots with distinct walking capabilities

• Research platform for planning and walk control benchmarking

• Adaptable planning policies (during runtime)

• Individual step data computation

• Enables easy code sharing

• Quick deployment into existing ROS systems

• This is not the end of the road!


Ongoing Work New Plugins

• Convex Hull Contact Estimator Current limitation: Center of foot must be on ground

Upcoming: Determining stable feet contact points by convex hull estimation





- Either by a deterministic algorithm

- Or using machine learing approaches for prediction





- Either by a deterministic algorithm

- Or using machine learing approaches for prediction

• Adaptive State Space Sampling

Increase planning speed by intelligent space sampling considering the surrounding environment


Ongoing Work

• Migration of other robots systems

• New RViz User Interface


WALK-MAN

Nao

Ongoing Work New Planning Modes: Continuous Planning

• MIT (DRC):


[3] Continuous Humanoid Locomotion over Uneven Terrain using Stereo Fusion,

Maurice F. Fallon et. al., 2015 15th IEEE-RAS International Conference on Humanoid Robots

Ongoing Work New Planning Modes: Continuous Planning

• First Step: vigir_step_controller

Low-Level Walking Controller Migration

Step Queue Management

- En-/Dequeuing Steps

- Stitch Step Plans

- Update Single Steps

Step Execution Management


http://wiki.ros.org/vigir_step_control

Example: THORMANG3





Sense

Plan

Act Supervision


• Terrain Model is crucial

Even the best planner system cannot plan without data

• Development history

Until DRC Trials 2014: Static Environment Snapshots







Until DRC Finals 2015: Online Terrain Model Generation







Until DRC Finals 2015: Online Terrain Model Generation

Since 2016: MASON

- Multi-Sensor Perception and Processing Framework

- Using Truncated Signed Distance Functions (TSDF) as data structure


MASON


• Basic Idea: Processing data in an assembly line

• Each processing step is computed by a plugin

Plugin A

Plugin B Plugin C

MASON


Advantages:

• Easy recomposition

Plugin A

Plugin B Plugin C

MASON


Advantages:

• Easy recomposition

Plugin A

Plugin D Plugin C

MASON


Advantages:

• Easy recomposition and reuse of exisiting code

• Multi-Threaded system but no deep copy of data required

Plugin A

Plugin D Plugin C

Plugin C

MASON Data Structure

• The MASON framework does not rely on specific data types

• Currently many plugins are based on TSDF data

TSDF Implementation based on OpenChisel [4]

Continuous world representation


[4] Chisel: Real Time Large Scale 3D Reconstruction Onboard a Mobile Device using Spatially Hashed

Signed Distance Fields. Matthew Klingensmith et. al., 2015

MASON Real World Application

• Originally developed for humanoids, but heavily used during ARGOS Competition


MASON Core Features

• Online Mapping in high dynamic environments

• Easy extendable

• Tracking changes in TSDF data

• Efficient online generation of data representations


MASON Core Features


• Easy extendable



TSDF


MASON Core Features


• Easy extendable



TSDF

PointClouds


MASON Core Features


• Easy extendable



TSDF

PointClouds

Meshes


MASON Core Features


• Easy extendable



TSDF

PointClouds

Meshes

OctoMap


MASON Core Features


• Easy extendable



TSDF

PointClouds

Meshes

OctoMap

ETHZ Grid Map


MASON Detection of Environmental Changes

• Reference World; Generated either by CAD and/or real laser data



• Negative Obstacle



• Noise-Robustness: Spinning Laserscanner in heavy rain








MASON Outlook

• Integration of 3D Slam approaches

Currently in work: Google Cartographer

• Preparation of Open-Source Release

Documentation & Tutorials

• Use as online 3D terrain generator for footstep planning




Sense

Plan

Act Supervision

Human Supervision

• Why?

• Prevent robots from doing stupid actions which…

…may damage the robot

…lead to inefficient mission progress

…overcome planner limitations


3D Footstep Planning Human-in-the-loop

• Footstep planning system provides comprehensive interface for Interactive Footstep Planning

Implementation example of a user interface:


Thanks for your Attention!

Summary:

• 3D Footstep Planning Framework

Extendable by plugins

• Step Queue Management

• Online Terrain Generation

MASON as Multi-Sensor Perception and Processing Framework

Online generation of various state of the art data representation

• Human Supervision

Please checkout our software at ROS Wiki:

• http://wiki.ros.org/vigir_pluginlib

• http://wiki.ros.org/vigir_footstep_planning

• http://wiki.ros.org/vigir_step_control











Documents

Towards Capable Open Source ROS- Frameworks for Real … · 09/08/2016 · Towards Capable Open Source ROS-Frameworks for Real-Time Perception, World Modeling and Footstep Planning