Code fixes in the RViz enviroment and cartesian planning

Please test the current version of the plugin and if everything works as expected.
Some test files can be found in the folder waypoints_data.
The results I get by executing Cartesian path with the motoman_sia20d_moveit_config package is the following:
test_risto.yaml = 71.05%
test_risto_2.yaml = 100%
test_risto_3.yaml = 70.48%