I am struggling with this problem for days. I really hope that someone could give me a hint what the problem is.
The robot consists of 5 axes. The first axis rotates around the z-axis and other 4 axes rotate around the y-axis. And the solver basically works.
Here is what I have done so far:
I calculate the manipulability factor with my Jacobian matrix (only translational part, since only the position is tracked here. Actually, I also tried with a combined Jacobian matrix, so not only the translational part but also the rotational part. But jerky motion was there anyway):
Then the damping factor is:
The damping factor is then integrated into the pseudo inverse calculation :
As you can see, this is just a classical pseudo inverse kinematic solver with damped least square method. The manipulability factor according the second (problem) movement is : The manipulability drops in the beginning of the video. But why? As far as I know, this manipulability factor indicates the linear dependence of the axes. To me the axes don't seem to be linearly dependent in the beginning part.
This jerky motion drives me crazy. As you can see in the first animation, the solver seems to work properly. What am I missing here?