Changed trajectory_filter_allowed to so the robot moves faster. MoveToPose now works without being jerky.