Add scaling factor to speeds, until it is fixed in gazebo
Tune some default params
Towards a better visualization of trajectories