Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Basic Footstep Planning Setup

Overview

At this page we will provide basic information how to setup the footstep planning system to work with your robot. All used example files can be found in the basic vigir_footstep_planner package.

Configuration Layout

The footstep planning uses multiple configuration files to run cleanly. The example footstep planner uses following structure:

<vigir_footstep_planner root dir>/

• config/

  • planner/ (Location for parameter sets (dynamic parameters))

    • footstep_planner_params_default.yaml

  • default_plugin_sets.yaml (Default plugin set used by planner (add additional sets [optional: each in a separate file] when required))

  • robot_params.yaml (Robot specific settings (static during runtime))

• launch/
  • footstep_planner_test.launch

It's recommended to reuse this structure for you own footstep planning launch package, but you can also adapt all config and launch files to fit your needs. However, these file are the only one needed to run a fully customized version of the footstep planner (no c++-code required!).

Robot Parameters

The robot_params.yaml defines all static robot specific parameters such as foot size and foot separation:

robot_name: MyRobot

world_frame_id: /world

### upper body settings ########################################################
upper_body:
  size: {x: 0.7, y: 1.1, z: 1.95} # [m]
  origin_shift: {x: -0.1, y: 0.00, z: 0.00} # [m], transformation from center to shifted frame

### foot settings ##############################################################
foot:
  size: {x: 0.26, y: 0.14, z: 0.0508} # [m]
  separation: 0.22 # [m]
  left: # position of planner foot frame (center of sole) given in robot's "foot" tf frame
    frame_id: /l_foot
    foot_frame:
      {x:  0.04, y:  0.00, z: -0.085, roll:  0.00, pitch:  0.00, yaw:  0.00}  # 3x[m] 3x[rad]
  right: # position of planner foot frame (center of sole) given in robot's "foot" tf frame
    frame_id: /r_foot
    foot_frame:
      {x:  0.04, y:  0.00, z: -0.085, roll:  0.00, pitch:  0.00, yaw:  0.00}  # 3x[m] 3x[rad]

### pelvis pose to feet center (on ground) transform ###########################
pelvis_to_feet_center: {x: 0.0, y: 0.0, z: -0.912} # [m]

In this file all robot specific values should be placed, also those used by custom extensions (e.g. advanced body collision checking models).

The config file explained

robot_name: MyRobot

world_frame_id: /world

At the beginning the basic header information are placed, especially in which reference frame all footsteps should be planned (usually "world"). You can also switch over to other frames if preferred. The robot_name field is currently not used by the system.

### upper body settings ########################################################
upper_body:
  size: {x: 0.7, y: 1.1, z: 1.95} # [m]
  origin_shift: {x: -0.1, y: 0.00, z: 0.00} # [m], transformation from center to shifted frame

In this section you can define a primitive bounding box for simple upper body collision checking. Here, the box is always placed on the ground (based on feet level) in center between both feet. If the upper body is not centered on this point you can set the origin_shift to consider this shift.

In this example, a z-value of 1.95 defines a box with a height of 1.95m measured from ground level, where the ground level is computed by the mean height of the left and right foot. The upper body is shifted slighty backwards.

### foot settings ##############################################################
foot:
  size: {x: 0.26, y: 0.14, z: 0.0508} # [m]
  separation: 0.22 # [m]
  left: # position of planner foot frame (center of sole) given in robot's "foot" tf frame
    frame_id: /l_foot
    foot_frame:
      {x:  0.04, y:  0.00, z: -0.085, roll:  0.00, pitch:  0.00, yaw:  0.00}  # 3x[m] 3x[rad]
  right: # position of planner foot frame (center of sole) given in robot's "foot" tf frame
    frame_id: /r_foot
    foot_frame:
      {x:  0.04, y:  0.00, z: -0.085, roll:  0.00, pitch:  0.00, yaw:  0.00}  # 3x[m] 3x[rad]

This is maybe the most important section. Here the foot dimensions are defined as well as the corresponding tf frames to track feet poses. All units are given in meters.

In the given example the tf frames are sitting in the feet ankle and a static transform (8.5cm downwards) to the sole is required. Additionally the foot ankle is not exactly attached in the center of the foot (4cm shift in x).

### pelvis pose to feet center (on ground) transform ###########################
pelvis_to_feet_center: {x: 0.0, y: 0.0, z: -0.912} # [m]

This parameter defines the static transformation of the pelvis position to the central ground projected point between both feet when the robot is in neutral (default) stance. This transformation is used to determine feet poses based on given pelvis pose (e.g. used by the flag FLAG_PELVIS_FRAME in FeetPoseRequestMsg).

Plugin Sets

The footstep planning system uses the comprehensive plugin system vigir_pluginlib. It allows to change the used plugin set during runtime dynamically. Which plugin set should be used is defined by the parameter set given either in the footstep planning request or globally set in the planning system. For details read the parameter sets section.

Each plugin set file must be loaded via rosparam command into the rosparam server:

   1 <rosparam file="$(find my_package)/config/my_plugin_sets.yaml" command="load" />

In future releases it is planned to add a folder crawler which loads all files within a folder automatically.

A typical plugin set has following layout:

plugin_sets:   # ALL PLUGIN SETS MUST LOADED IN THE plugin_sets NAMESPACE
  default:     # NAME OF THE PLUGIN SET
    # StateGeneratorPlugin
    reachability_state_generator: none

    # StepPlanMsgPlugin
    step_plan_msg_plugin: none

    # ReachabilityPlugins
    reachability_polygon: none

    # StepCostEstimatorPlugins
    const_step_cost_estimator: none
    euclidean_step_cost_estimator: none
    ground_contact_step_cost_estimator: none

    # HeuristicPlugins
    euclidean_heuristic: none
    step_cost_heuristic: none

    # PostProcessPlugins
    step_dynamics_post_process: none

    # CollisionCheckPlugins
    2_upper_body_grid_map_model:
      import: upper_body_grid_map_model
      params:
        collision_check_flag: 2 # Upper Body
        grid_map_topic: /body_level_grid_map
    3_foot_grid_map_model:
      import: foot_grid_map_model
      params:
        collision_check_flag: 1 # Foot
        grid_map_topic: /ground_level_grid_map

    # TerrainModelPlugins
    1_terrain_model:
      import: terrain_model
      params:
        collision_check_flag: 4 # Foot Contact Support
        terrain_model_topic: /terrain_model

The plugin set defines the composition of used step (StepCostEstimatorPlugins) and heuristics (HeuristicPlugins) functions. Thus, here the planning policy can be simply defined and modified. Please take note of the option to also set parameters for each single plugin. However, these parameters are supposed to be non-dynamic, thus, not changed during runtime. Dynamic parameters should only set here to define a default value, but in general they are (re)defined in the parameter set which can be changed and reloaded in the planning system anytime.

Note: The parameter handling for plugins will be improved in future version.

The basic footstep planning system provides already basic selection of plugins (see here).

Parameter Sets

All following explanations are referring to this example here. As most parameters are already well explained in this file, only the most important parameters are highlighted here.

The footstep planning system supports handling of multiple parameter sets identified by name. For this purpose each parameter set must be stored in a own file, collected in a single folder. The folder containing all parameter sets must be declared in the launch file as described here.

Most Important Parameters

name: default

The name of the parameter set as used as identifier in the footstep planning request.

const_step_cost_estimator:
  step_cost: 0.1

This is a typical example how to set dynamic parameters for a plugin. The full parameter identifier is defined by the plugin (when obtained in loadParams(...) function).

plugin_set: default

Name of the plugin set to be used in combination with this parameter set. Each time this parameter set is loaded into the planning system, it will also trigger to load the named plugin set.

planner_type: ARAPlanner

Not supported yet. Please leave it ARAPlanner.

max_risk: 1.0

The risk metric defines at which point the risk is too high and the step should be discarded. The risk is an accumulated value computed by step cost estimator plugins.

max_planning_time: 10.0
initial_epsilon: 8.0
decrease_epsilon: 0.2

Those parameters control the low level planner behavior. The initial heuristic inflation (initial_epsilon) as well as deflation step (decrease_epsilon) during plan optimization can adjusted.

cell_size: 0.02
num_angle_bins: 32

The discretization of the world into a search graph is defined here. The cell_size is given in Meter and defines the planning grid. The number of considered discrete angular values for each step is controlled by the num_angle_bins. The bins are equally spaced within a full rotation [0; 2*PI).

foot:
  max:
    step: {theta: 0.40}
    inverse:
      step: {theta: -0.30}

This section defines the min and max foot angle:

• foot/max/step/theta: max toe out angle
• foot/max/inverse/step/theta: max toe in angle

It is used by the reachability_state_generator to generate the successor steps set within the given range of valid toe angles while the num_angle_bins is used to determine the discretiztaion.

step_range:
  x: [ 0.00, -0.20, -0.17, -0.13,  0.13,  0.20,  0.38]
  y: [ 0.20,  0.25,  0.30,  0.35,  0.35,  0.30,  0.20]

The step_range array defines the stepping range polygon relative to the right foot. It is used by the reachability_state_generator to determine valid x/y-positions for all successor steps and additional used by the reachability_polygon to verify that each step is within the given stepping range (in case when another state generator is used).

sway_duration: 0.10
step_duration: 0.63
swing_height: 0.10

These parameters defines basic step timing (in seconds) and trajectory (in meters). The swing_height should be considered as trajectory peak in z during step replacement. If the planner does 3D planning, the swing height will be automatically adjusted to also included a lift height.

feedback_rate: 10.0 # [Hz]
threads: 4
jobs_per_thread: 50

In general those parameters should be left it as is.

• feedback_rate: Controls rate of feedback message generation of planning state and progress during planning.
• threads: Number of threads to use during planning.
• jobs_per_thread: Controls load distribution; A higher number reduces thread management overhead, but may degenerate the system to single thread.

Launching the Footstep Planner

   1 <include file="$(find vigir_footstep_planning)/launch/footstep_planning_basics.launch" />
   2 <node name="footstep_planner" pkg="vigir_footstep_planner" type="footstep_planner_node" respawn="true" output="screen" />

This snippet launches the footstep planning system. Please take note of the included launch file which will spawn helper nodes.

   1 <rosparam file="$(find vigir_footstep_planner)/config/default_plugin_sets.yaml" command="load" />

In this section all plugin sets are loaded into the rosparam server.

   1 <rosparam file="$(find vigir_footstep_planner)/config/robot_params.yaml" command="load" />
   2 <param name="params_path" value="$(find vigir_footstep_planner)/config/planner/" />
   3 <param name="default_params" value="default" />

Here, the robot configuration is loaded into the rosparam server and the folder containg all available configuration files (parameter sets) are told to the planning system. All parameter sets in this folder will be loaded and are available for planning requests afterwards. The name of the parameter set to be used at startup is set here as well to guarantee a predefined configuration after startup.

In general the footstep planning launch files will work in namespaces as well.

A full example can be found here.