Safe Trajectory Planning & Control