Fix three legged push up error

[remod]

Free gait and rviz plugin throw exceptions when starting the three legged push up from a non-default configuration.

How to reproduce:

• Checkout release branches (might also appear on master)
• Do some steps with the static walk and stop at a non-default configuration
• Switch to free gait and start the three legged push up

In my case free gait and rviz plugin throw the same exceptions, but the type of exception depends on Debug or Release build:

• Debug: std::runtime_error because the UnitQuaternion length in kindr is not 1 (by far not, so no numerical issue)
• Release: map::at

Debug error:

[marco-tranzatto]

The same error appears in simulation in master branch (Release mode). To reproduce the error it's even not necessary to do anything but previewing the action push_ups_three_legged. This can be done directly in the initial spot, without static walking.

[INFO] [1518692372.476881, 13.293000]: Loading Free Gait action from YAML file "/home/marco/catkin_ws/src/anymal_ctrl_free_gait/anymal_demo_actions/actions/push_ups_three_legged.yaml". [INFO] [1518692372.904855, 13.414000]: Action switched to state: Active. [INFO] [1518692372.975218, 13.442000]: Action was successfully started. [ INFO] [1518692372.976296167, 13.442000000]: FreeGaitActionPlugin Started terminate called after throwing an instance of 'std::out_of_range' what(): _Map_base::at

[dbellicoso]

I have created a branch which attemps to address the issue (https://github.com/leggedrobotics/free_gait/tree/fix/out_of_range). The problem appears to be in functions like PoseOptimizationFunctionConstraints::getInequalityConstraintValues:

for (const auto& positionBaseToHipInBaseFrame : positionsBaseToHipInBaseFrame_)  {
  ...
  const auto limb = positionBaseToHipInBaseFrame.first;
  const Position& footPosition = stance_.at(limb); // out_of_range thrown here
  ...
}

I can also reproduce this by executing the locomotion planner. @remod the runtime_error with the unit quaternion should be fixed by now @pfankhauser could you please verify this?

[pfankhauser]

57 fixes this. Will be merged back to master after the release.