Wait till robot does not move anymore and then start new trajectory