How to extend lipm-walking for hand trajectories?

[ndehio]

I want to use lipm-walking for controlling HRP4’s right hand while being in double-support with all the stabilizer functionalities. Therefore I need to extend the state “Standing” (correct?). I tried two different versions. For both I added to the transition map in the config file:

  "transitions":
  [
    ["Standing", "TestState", "TestState"],

First approach via config file only:

I added the following state

  "states":
  {
    "TestState":
    {
      "base": "Standing",
      "tasks":
      {
        "RightHandTrajectory":
        {
          "type": "bspline_trajectory",
          "setupMode": false,
          "surface": "RightHandPad",
          …

I can access the new state via the Rviz gui -> “force transition to standing” -> “force transition to TestState” and read the expected output in the terminal Starting state TestState
However, for some reason the right-hand trajectory is not executed...

Second approach via c++ code:

I created new state files .h and .cpp inside the states-directory and adapted the cmake file with add_fsm_state_simple(TestState).
This new state derives from Standing and overrides the main functions

...
struct TestState : Standing
{
  void start() override;
...

and

...
void states::TestState::start()
{
  LOG_SUCCESS("TestState::start")
  states::Standing::start();
}
...

The code compiles sucessfully, but when launching the controller I receive the error message:

Invalid transition:
- destination state (TestState) is not loaded

Whats wrong here?

Maybe related to that, I noticed that Stephane uses

  "StatesFiles": [],
  "StatesLibraries": ["@MC_RTC_LIBDIR@/mc_controller/lipm_walking_controller/states"],

while in other controllers we used

  "StatesLibraries": ["@MC_STATES_DEFAULT_INSTALL_PREFIX@","@MC_STATES_INSTALL_PREFIX@"],
  "StatesFiles": ["@MC_STATES_DEFAULT_INSTALL_PREFIX@/data","@MC_STATES_INSTALL_PREFIX@/data"],

[gergondet]

Hi Niels,

First approach

This cannot work. Standing is its own state and doesn't care that you have a tasks entry

Second approach

You're probably missing two things:

• target_link_libraries(TestState PUBLIC Standing) to link your state with the Standing state that it's using
• set_target_properties(TestState PROPERTIES INSTALL_RPATH ${FSM_STATES_INSTALL_PREFIX}) this one will ensure that the state finds Standing when it's loaded. Depending on how you build it you might also want:
• set_target_properties(TestState PROPERTIES BUILD_RPATH ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME}/states) to get it to work in a catkin setup

However, I think there is a third (and better) approach in your case: Parallel

Third approach

MyState:
  base: MetaTasks
  tasks:
    RightHandTrajectory:
      type: bspline_trajectory
...

MyStabilzedState:
  base: Parallel
  states: [Standing, MyState]

In that case, the two states will run together. If you want a fully stabilized FSM, you can write your FSM using the Meta state and use this FSM in parallel with the Standing state