I have added gears and an AS5600 rotary encoder to one of my InMoov shoulders. For shoulder moves I am applying incrementing positions every 20 ms to my shoulder servo. When requesting shoulder positions I record the requested position and the measured position for each of the 20 ms steps.
I would like to use the measured position vs. the requested position to detect a blocking of the shoulder move.

The result is that the servo initially shows a large lag until it starts to move (about 0.7 s before I measure a change in the shoulder position). At the end of the move (after requesting the final target position) the servo still keeps moving for about 2 secs until it arrives at (or close to) the final position. This makes it more or less impossible to detect  a blocking and avoid a wrong blocking detection.

Thought I might ask if somebody has experienced similar issues and what could provide a better way to make the shoulder move in a more timely fashion and to allow blocking detection.

My initial ideas:
1) instead of many small linear increments I could request positions using a power function
2) I could calculate the expected measured positions using an inverse power function
3) a combination of above methods
4) run repeated moves and let the robot "learn" and improve (do not know where to start with this)
5) use pychrono (physics engine) to get more realistic expected measured positions (looks however above my abilities)

I am aware that the shoulder start position, move direction, current bicep, rotate and omoplate positions will have an impact on the shoulder move progress. For a start I would be happy to have a solution for a very limited scenario (e.g. all servos at rest position).