(this is a cross post from https://stackoverflow.com/q/64393373/2682209 as I didn't know the robotics community before.)
I'm trying to use some inverse kinematics algorithm to perform a path planning task. I take my path as a kinematic chain with joined segments and define the start and target. As the curvature of the path shall be restricted, the maximum bending angles at each joint must be limited.
Additionally, the orientation of the start segment as well as the orientation of the final segment shall be well defined within some tolerance margins.
I started out to implement the quite simple FABRIK algorithm (youtube resp. paper). I think, I also added the angular constraints respecting the inner joint angles correctly, but I fail in getting the fixed angles of the start- and the end-segment correct.
I started out with the kinematic chain unfolded in a curve like fashion, bridging the angular difference form start angle to target angle. From there on, I use the FABRIK algorithm and restrict it's joint bending to maintain the given limitations.
This way, the angular constraints are an invariant of the algorithm and are never violated during the run. In quite some settings, this works out OK.
Unfortunately however, this algorithm keeps running in deadlock situations it can't escape from like shown in this picture:
The upper panels shows the start configuration, the lower panel the situation after 18 iterations in a deadlock situation with no more progress.
The purple path should take a turn from the left bubble to the right bubble entering the right bubble from the left to the right (like sketched in red).
This behavior must be due to the fact, that the head-segment (the last before the target) must not turn round, as it must maintain the target angular constraint invariant all the time.
any idea, how to overcome this issue and how to do inverse kinematics with no only postional, but also orientational target values.