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.

System Overview

Overview

The vigir_step_controller is designed to provide easy access to high-level online footstep planning and execution capabilities. In order to achieve online footstep planning, a StepQueue is maintaining all received step plans. It is able to update single steps, merge and stitch entire step plans.

Furthermore, an integrated state machine is tracking the current walking progress and executes steps from the step queue using a dedicated plugin interfacing the robot's motion layer.

ROS API

Action Goal

• Action server for step plan execution

Subscribed Topics

• Loads StepPlanMsgPlugin converting generic step plan messages into robot speicific format

• Loads step controller plugin implementing low-level interfaces for the motion layer

• All step plan published at this topic are added to the step queue for execution

Published Topics

• Topic publishing step execution feedbacks

Plugins

The step controller relies on two plugins based on vigir_pluginlib explained next. Those plugins are loaded at startup of the system but can be replaced during runtime.

StepPlanMsgPlugin

Motion algorithms require often detailed information for step execution. During footstep planning, this plugin is used to decode optional additional custom data into the step plan (see vigir_footstep_planning package). For this reason, the step controller relies on this plugin to decode all this custom data if available from the step plan during footstep execution. Furthermore, this plugin can be used to provide proper algorithms to convert the generic step data into robot/algorithm-specific format.

However, if the motion algorithm does not require any custom data, then the provided default implementation (namely StepPlanMsgPlugin) is sufficient.

StepControllerPlugin

The StepControllerPlugin defines a bottom-up designed interface for the step controller to interface the robot's motion layer directly. Therefore, for every robot/motion algorithm, such a plugin must be implemented. Through this plugin, the user is also able to (re-)implement the entire behavior (state machine) of the controller and with the help of the StepPlanMsgPlugin to perform all required conversion from high-level planned steps to low-level motion commands.

For this purpose, it defines multiple methods called during an update iteration as described in Callback Order) in detail. It is also responsible for providing proper state transitions for the controller's state machine.

The StepControllerPlugin partially pre-implements a basic common state machine (see State Machine) for proper step execution. Therefore, each plugin needs at minimum to implement following methods

• preProcess(...)
• initWalk(...)
• executeStep(...)

as described next.

The full details of the provided interface are documented in the code API of the StepControllerPlugin

Callback Order

The StepController is responsible for interfacing the ROS API and calling the step controller's update loop with a fixed rate (default 10Hz).

The update loop (see image above) consists of three processing steps based on virtual function calls to the used StepControllerPlugin and an additional feedback call publishing the feedback messages in between. The intended purpose and pre-implemented functionality of each processing step are described in detail next.

preProcess(...)

Before any steps should be processed, the preProcess(...) method has to pre-check the current robot and walking condition. It shall be checked if the motion layer has detected any errors and everything is still working as expected. Furthermore, the step plan execution progress should be supervised in this step. This includes checking and triggering of step execution (fetching steps from the queue, see Execution Progression) as well as determining successful execution of the entire step plan. If the step plan has been completely executed, the controller's state must be set to FINISHED here.

initWalk(...)

This method should implement all required initialization to enable the use of the motion algorithms and controllers. If a new step plan is received while the controller is in READY state, it will be automatically called by preProcess(...) using the default StepControllerPlugin implementation. It will not be triggered again during an already started motion sequence.

process(...)

The process(...) method is responsible for checking if a single or even multiple steps must be forwarded to the motion layer (triggered by preProcess(...)). Each required step can be easily obtained from the step queue and must be given as argument to an executeStep(...) callback. This method must not manage the step queue (i.e. step plan updates) as it is already done by the controller. The default implementation of process(...) can already handle step playback well and should be sufficient in most cases.

executeStep(...)

As soon the step controller has detected that the motion algorithm requires the next step data, this method is called by process(...). Hence, this method is only responsible for forwarding the given step to the motion layer performing all required data conversions and calling the low-level motion layer. The default process(...) implementation does already contain the required logic to trigger the execution callback which is explained deeper in the Execution Progression section.

postProcess(...)

During the postProcess(...) callback all cleanups, data logging and walking status checking should be performed.

State Machine

The state machine transitions should be triggered within the preProcess(...), process(...) and postProcess(...) calls by individually implemented logic as required to run the motion layer properly.

Abstract Transitions

The StepControllerPlugin implements already an abstract state machine behavior illustrated above. It is called abstract as not all displayed transition are pre-implemented: All transitions labeled with setState(state) have to be triggered by the user implementation, calling the setState(...) method with the proposed argument at the appropriate place in any of the callback methods introduced above.

It is also open to the user to add or replace transition triggers to the pre-implemented state machine.

Execution Progression

The default implemented behavior in StepControllerPlugin is using the step indices to check if new steps have to be executed. Each time the condition getLastStepIndexSent() < getNextStepIndexNeeded() evaluates to true the next step is taken from the queue. Therefore triggering a step execution event becomes very simple. As soon the motion layer requests a new step, the setNextStepIndexNeeded(index) has to be called with the proper step index argument. Usually, this can happen in the preProcess(...) step.

On the other side setLastStepIndexSent(index) must be set to the corresponding value when the step has been forwarded to the motion layer to stop triggering duplicated step execution callbacks. The default StepControllerPlugin implementation takes already care about this.

Those presented fields can be used by cross checking the step queue to determine if the step plan execution has been completed and thus, the state machine can progress to the FINISHED state.

Feedback

The step controller does also provide feedback for the high-level system. For this purpose the feedback state must be updated continuously, e.g. by calling updateQueueFeedback() as soon changes have been applied to the step queue. The default StepControllerPlugin implementation does already cover all cases. But if any custom code does further modifications on the step queue, the programmer should consider updating the feedback as well.

In order to report step execution progress in the feedback message, this information has to be updated as well. This can be achieved by updating the corresponding fields in the feedback message directly. A typical example is given in the code snippet below demonstrating the incrementation of all values after successful execution of a single step.

   1 msgs::ExecuteStepPlanFeedback feedback = getFeedbackState();
   2 
   3 feedback.header.stamp = ros::Time::now();
   4 
   5 feedback.last_performed_step_index++;
   6 feedback.currently_executing_step_index++;
   7 feedback.first_changeable_step_index++;
   8 
   9 setFeedbackState(feedback);

Note: Feedback has to be updated during preProcess(...) and process(...) calls. Changes during postProcess(...) become only visible in the next iteration and may be overwritten by an updated state.

Examples

The StepControllerTestPlugin provides already a basic fully working test implementation to demonstrate the usage of the system (see Example Startup). This plugin does just simulate the execution of single steps.

A real world example can be found in our THORMANG3 setup.