Как спланировать путь из точки 1 в точку 2, чтобы он оставался в указанной области?

Question

По сути, это проблема планирования траектории. Как показано на рисунке ниже, как я могу гарантировать, что при переходе от точки (x,y,z) к другой точке тора я не выхожу из этой области тора?(Область вне тора недопустима для конца эффектор робота)