Оптимизация инверсной кинематики.

[xrou]

Всем привет.
По статье на Хабре https://habr.com/ru/post/332164/ сделал манипулятор на инверсивной кинематике по алгоритму Градиентный спуск.
Все работает, все круто. Сделал сортировку разноцветных шаров по соответствующего цвета корзинам - работает, но вылез один нюанс.
Манипулятор закидывая один шарик может сильно изогнутся и если следующий шарик близко, манипулятор не разгибается - подходит к нему с согнутыми суставами что не очень красиво,
а и иногда возникает ситуация когда сгиб оказывается ниже шарика и части манипулятора проходят сквозь пол.
Вобщем мне нужно добавить к алгоритму способность потдерживать оптимальное расположение суставов. А оптимальным расположением будет перевернутая латинская U.
Т.е. к каждому шарику манипулятор должен подходить сверху, ну или стремиться к этому.

У меня есть идея: после каждого отпущенного шарик ставить манипулятор в вертикальное положение, но это конский костыль, хочется всеже научить алгоритм работать правильно.