tests with visualizer show that probably there is some problem with the ROS nodes, the Q matrix contains only some values, TODO solve this