Towards Capable Open Source ROS- Frameworks for Real … · 09/08/2016 · Towards Capable Open...
Transcript of Towards Capable Open Source ROS- Frameworks for Real … · 09/08/2016 · Towards Capable Open...
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 2
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 3
Sense, Plan, Act and Human-in-the-loop
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 4
Sense
Plan
Act Supervision
A lot of progress recent years
Sense, Plan, Act and Human-in-the-loop
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 5
Sense
Plan
Act Supervision
Our Focus is on Plan, Sense and
Supervision
Sense, Plan, Act and Human-in-the-loop
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 6
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 7
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 8
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
…
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 9
[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!
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 10
First steps (Atlas v3 - 2014) Autonomous Stair Climbing
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 11
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 12
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?
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 14
ESCHER
THORMANG
Footstep Planning Data Flow
• Basic data flow:
• What is the challenge?
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 15
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 16
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 17
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
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:
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 18
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)
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 19
• 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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 20
3D Footstep Planning Experiment: Complex Terrain (Post DRC – 2015)
3D Footstep Planning Experiment: Complex Terrain (Post DRC – 2015)
• Resulting step plan in robot‘s field of view
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 21
3D Footstep Planning Experiment: Complex Terrain (Post DRC – 2015)
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 22
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!
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 23
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 24
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 25
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
• Adaptive State Space Sampling
Increase planning speed by intelligent space sampling considering the surrounding environment
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 26
Ongoing Work
• Migration of other robots systems
• New RViz User Interface
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 27
WALK-MAN
Nao
Ongoing Work New Planning Modes: Continuous Planning
• MIT (DRC):
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 28
[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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 29
http://wiki.ros.org/vigir_step_control
Example: THORMANG3
Sense, Plan, Act and Human-in-the-loop
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 30
Sense
Plan
Act Supervision
Terrain Model Generation
• Terrain Model is crucial
Even the best planner system cannot plan without data
• Development history
Until DRC Trials 2014: Static Environment Snapshots
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 31
Terrain Model Generation
• 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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 32
Terrain Model Generation
• 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
Since 2016: MASON
- Multi-Sensor Perception and Processing Framework
- Using Truncated Signed Distance Functions (TSDF) as data structure
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 33
MASON
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 34
• Basic Idea: Processing data in an assembly line
• Each processing step is computed by a plugin
Plugin A
Plugin B Plugin C
MASON
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 35
Advantages:
• Easy recomposition
Plugin A
Plugin B Plugin C
MASON
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 36
Advantages:
• Easy recomposition
Plugin A
Plugin D Plugin C
MASON
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 37
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 38
[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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 39
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 40
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
TSDF
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 41
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
TSDF
PointClouds
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 42
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
TSDF
PointClouds
Meshes
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 43
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
TSDF
PointClouds
Meshes
OctoMap
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 44
MASON Core Features
• Online Mapping in high dynamic environments
• Easy extendable
• Tracking changes in TSDF data
• Efficient online generation of data representations
TSDF
PointClouds
Meshes
OctoMap
ETHZ Grid Map
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 45
MASON Detection of Environmental Changes
• Reference World; Generated either by CAD and/or real laser data
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 46
MASON Detection of Environmental Changes
• Negative Obstacle
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 47
MASON Detection of Environmental Changes
• Noise-Robustness: Spinning Laserscanner in heavy rain
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 48
MASON Detection of Environmental Changes
• Noise-Robustness: Spinning Laserscanner in heavy rain
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 49
MASON Detection of Environmental Changes
• Noise-Robustness: Spinning Laserscanner in heavy rain
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 50
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 51
Sense, Plan, Act and Human-in-the-loop
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 52
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 53
3D Footstep Planning Human-in-the-loop
• Footstep planning system provides comprehensive interface for Interactive Footstep Planning
Implementation example of a user interface:
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 54
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
27.06.2017 | ICRA Workshop 2017 | Alexander Stumpf | 55