Fixed bug in trajectory joining when robot actually stops.