About | Support | answers.ros.org	Search:

humanoid_navigation: footstep_planner | gridmap_2d

Package Summary

A footstep planner for humanoid / biped robots. The planner builds on SBPL and has anytime (ARA*) as well as dynamic replanning (AD*) capabilities.

Details can be found in the corresponding paper "Humanoid Navigation with Dynamic Footstep Plans" by J. Garimort, A. Hornung, and M. Bennewitz; published in Proc. of the IEEE International Conference on Robotics & Automation (ICRA), 2011

• Author: Johannes Garimort, Armin Hornung
• License: GPL 3
• Repository: alufr-ros-pkg
• Source: svn http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner

Overview

This package provides footstep planning for humanoid (biped) robots. The first release built on D* Lite (Koenig et al. 2002) for efficient replanning. The current version now builds on sbpl. This enables D* and A* implementations with anytime capabilities.

Details are available at http://hrl.informatik.uni-freiburg.de/animations-footsteps.html and in the corresponding publication "Humanoid Navigation with Dynamic Footstep Plans" by J. Garimort, A. Hornung, and M. Bennewitz (ICRA 2011).

@INPROCEEDINGS{garimort11icra,
  author = {Johannes Garimort and Armin Hornung and Maren Bennewitz},
  title = {Humanoid Navigation with Dynamic Footstep Plans},
  booktitle = {Proc.~of the IEEE International Conference on Robotics and 
  Automation (ICRA)},
  year = 2011
}

The planner can be invoked by interacting with it via RViz, or by continuously using it during plan execution (Video below). A pose estimate from a localization system is required in the latter case.

Getting started

First, follow the installation instructions of the humanoid_navigation stack (you might have to check out the source for that or download a recent package).

For interactive planning (without any plan execution by a robot), you're almost set now. Simply run

roslaunch footstep_planner footstep_planner_complete.launch

which loads a sample map, starts up the planner, and launches RViz to interact with it. Set a "2D pose estimate" as start, and a "2D nav goal" as goal (Hotkeys P and G). After some time (depending on footsteps, heuristic and the environment), the planner will visualize the footstep plan in RViz or output an error on the terminal.

Heuristics

Currently, there are three heuristics available to the planner, described in detail in the paper. They can be selected with the heuristic_type parameter:

• "EuclideanHeuristic": Straight-line distance to the goal

• "EuclStepCostHeuristic": Straight-line distance to the goal with added footstep costs

• "PathCostHeuristic": Distance along 2D path pre-planned with A* (preferred). This heuristic works best in most cases, as it is most informed of the environment. Note that it is potentially inadmissible in the case of stepping over obstacles.

Footstep parameterization

Model

Since version 0.3, footsteps are parameterized as a general 2D transform (x,y,theta) of the swinging (walking) leg's foot from the current supporting leg's foot origin. Footstep sets are specified in the parameter arrays ~footsteps/{x,y,theta} (see below).

Old Model (v0.2 and before)

Before, footsteps were parameterized by the displacement (x,y,theta) of the swinging (walking) leg's foot from its default position next to the current supporting leg's foot, which was specific to the Nao's old walking algorithm. For a displacement of (0,0,0), the walking foot is placed next to the supporting foot at a distance of ~foot/separation.

Parameterization for the Nao

A footstep parameterization for the Nao humanoid is given in config/footsteps_nao.yaml.

Parameterization for larger humanoids

A footstep set for a larger humanoid which is capable of stepping over obstacles (e.g. Asimo or HRP-2) is given in config/footsteps_asimo.yaml.

ROS API

See Code API for documentation of all classes and relevant code. The ROS node interface is described below. Both might be subject to change in the future as we develop the package further.

footstep_planner_node

The planning ROS node which listens to pose callbacks to plan footstep paths, either interactively in RViz (set pose estimate and navigation goal) or continuously while a robot executes the path.

Subscribed Topics

goal (geometry_msgs/PoseStamped)

• Goal pose (x, y, theta) in the map frame

initialpose (geometry_msgs/PoseWithCovarianceStamped)

• Current starting pose (x, y, theta) in the map frame

map (nav_msgs/OccupancyGrid)

• 2D Grid Map e.g. originating from map_server. A map change will trigger replanning.

Published Topics

~footsteps_array (visualization_msgs/MarkerArray)

• Visualization of the planned footsteps

~path (nav_msgs/Path)

• Visualization of the planned path (connecting left and right footsteps)

~heuristic_path (nav_msgs/Path)

• Visualization of the heuristic

~expanded_states (sensor_msgs/PointCloud)

• Visualization of the expaded nodes

~start (visualization_msgs/PoseStamped)

• Visualization of the requested start pose

Services

plan_footsteps (humanoid_nav_msgs/PlanFootsteps)

• Plan a footstep sequence from a given start to goal, return the costs and success.

Services Called

cmd_step_srv (humanoid_nav_msgs/StepTargetService)

• Requests footstep commands to be executed when in "navigation" mode.

Parameters

~heuristic_type (string, default: EuclideanHeuristic)

• Which heuristic to use (see Heuristics). For the fastest results, PathCostHeuristic is recommended.

~planner_type (string, default: ARAPlanner)

• Which SBPL planner to use (e.g. ARAPlanner, ADPlanner)

~search_until_first_solution (bool, default: False)

• Search until a the first (non-optimal) solution is found (False), or incrementally improve it (True)

~initial_epsilon (float, default: 8.0)

• Initial epsilon to overestimate costs (for anytime planning). 1.0= no overestimation.

~forward_search (bool, default: False)

• Search forward (start to goal) or backwards (goal to start)

~step_costs (double, default: 0.1)

• Constant costs associated to executing one step

~footsteps/{x,y,theta} (array of floats)

• Footstep actions as displacement vectors (see Model above). Make sure that each array of x,y,theta has the same length and contains only floats.

~foot/size/{x,y,z} (float)

• Size of each foot's bounding rectangle in m. (z only used for visualization)

~foot/separation (float)

• Default footstep separation (see old model above) in m. No longer used since v0.3!

~foot/origin_shift/{x,y} (float)

• Translation of the foot origin from the center of the bounding rectangle in m.

~{r,l}foot_frame_id (string, default: {R,L}Foot_link)

• tf frame ID of the right and left foot (needed for navigation mode)