Planning robot motion with dynamic constraints