Added dp_primitive, go2point_FA, go2point_UA primitives
Merge branch 'reorganization_v5' of https://github.com/labust/labust-ros-pkg into reorganization_v5