Gazebo2 fix build
Gazebo 1.9.5: simplify formula