About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Only released in EOL distros:   electric fuerte groovy hydro indigo kinetic

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, and the randomized A* (R*) search.

Details are available at http://hrl.informatik.uni-freiburg.de/research/animations/footsteps/ and in the following publications. Please consider citing our paper if you use the footstep planner in your research.

• Anytime Search-Based Footstep Planning with Suboptimality Bounds by A. Hornung, A. Dornbush, M. Likhachev, and M. Bennewitz (Humanoids 2012).

  • @INPROCEEDINGS{hornung12humanoids,
      author = {Armin Hornung and Andrew Dornbush and Maxim Likhachev and 
      Maren Bennewitz},
      title = {Anytime Search-Based Footstep Planning with Suboptimality 
      Bounds},
      booktitle = {Proc. of the IEEE-RAS International Conference on Humanoid 
      Robots (HUMANOIDS)},
      year = 2012
    }

• 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.

Planners

For footstep planning, the planners Anytime Repairing A* (ARA*), Anytime Dynamic A* (AD*), and Randomized A* (R*) are supported. All of them offer anytime capabilities (see parameters below) and may be suitable for different scenarios.

The parameter ~planner_type can have the following values: ARAPlanner, ADPlanner, RSTARPlanner.

The R* implementation in the currently released SBPL version (1.1.0) unfortunately segfaults, so using R* requires the compilation and installation of a newer version (see Tutorial).

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 parametrized 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.

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. Choices: ARAPlanner, ADPlanner, RSTARPlanner

~search_until_first_solution (bool, default: False)

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

~allocated_time (float, default: 7.0)

• Maximum search time (anytime planning) if search_until_first_solution is set to False

~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)

~num_random_nodes (int, default: 20)

• Number of random node expansions for R*

~random_node_dist (float, default: 1.0)

• Distance (in m) at which the random nodes in R* should be placed

~heuristic_scale (float, default: 1.0)

• Scaling factor by which the heuristic value (e.g. euclidean distance) will be multiplied. This can make the heuristic more realistic in terms of costs, but may not overestimate them.

~step_cost (float, default: 0.1)

• Constant costs associated to executing one step

~diff_angle_cost (float, default: 0.0)

• Costs to estimate the difference between the current foot orientation and the goal foot orientation.

~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/max/step/{x,y,theta} (float)

• The maximal allowed step has to be in the range defined by x, y, theta.

~foot/max/inverse/step/{x,y,theta} (float)

• The maximal allowed backward step has to be in the range defined by x, y, theta.

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

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

~accuracy/collision_check (int, default: 2)

• The accuracy of the collision check performed for each step. (0: just the circumcircle of the foot, 1: just the incircle of the foot, 2: circumcircle and incircle recursivly checked for the whole foot).

~accuracy/cell_size (float, default: 0.01)

• The size of each grid cell used to discretize the robot positions in cm.

~accuracy/num_angle_bins (int, default: 128)

• The number of bins used to discretize the robot orientations.

footstep_navigation_node

The navigation ROS node for the NAO robot which listens to goal callbacks to plan and execute footstep paths. This node is an extension of the footstep_planner_node, so it offers the same topics, parameters etc as above in addition.

Subscribed Topics

amcl_pose (geometry_msgs/PoseWithCovarianceStamped)

• Current robot pose (x, y, theta) in the map frame, e.g. from RViz.

Services Called

footstep_srv (humanoid_nav_msgs/StepTargetService)

• Used to send a footstep to be executed by the NAO robot.

clip_footstep_srv (humanoid_nav_msgs/ClipFootstep)

• Used to clip a footstep into the executable range.

Parameters

~feedback_frequency (float, default: 5.0)

• Determines how often the action server should send its feedback.

~safe_execution (bool, default: true)

• Determines whether to use the slower but more cautious footstep execution or not.

~accuracy/footstep/{x,y,theta} (float, default: {x: 0.01, y: 0.01, theta: 0.1})

• Determines how accurate a executed footstep has to fit into a planned one.

~{r,l}foot_frame_id (string, default: /{r,l}_sole)

• tf frame ID of the right and left foot

footstep_planner_walls

Extended from footstep_planner_node with identical functionality. New feature is a second map topic that indicates tall obstacles such as walls to which a larger clearing needs to be maintained. Topics / parameters are the same except the ones listed below:

Subscribed Topics

map_walls (nav_msgs/OccupancyGrid)

• 2D Grid Map that only contains large obstacles such as walls on a second topic.

Parameters

~footstep_wall_dist (float, default: 0.15)

• The wall map will be inflated by this radius so that footsteps remain at a safe distance