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, as you can see below:
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?
As you can see above, with very high alpha the movement stops around the singularity area (the manipulability value is in that position 0.12 ! At the home configuration the manipulability value is around 15.)
UPDATE 2 -----------------------------
I might find the error, but not sure because this leads to another error... So I changed the unit_vectors of my 2-5 axes so that they rotate around x-axis, instead of y-axis:
the problematic point can be reached now :
But the point that could be reached before is not reachable now....
To me it doesn't make sense that the unit_vectors should be around x-axis. What am I missing here?