fixing orientation optimiztation

source/armarx/navigation/factories/GlobalPlannerFactory.cpp

source/armarx/navigation/global_planning/AStar.cpp

source/armarx/navigation/global_planning/AStar.h

...ion/global_planning/optimization/OrientationOptimizer.cpp

...ation/global_planning/optimization/OrientationOptimizer.h

source/armarx/navigation/server/Navigator.cpp

source/armarx/navigation/server/Navigator.h

...arx/navigation/server/introspection/ArvizIntrospector.cpp

source/armarx/navigation/util/Visualization.cpp

source/armarx/navigation/util/Visualization.h