Code fixes in the RViz enviroment and cartesian planning