rate set to 200Hz, action server works better, linear PID decrease during trajectory