I'm trying to write a Matlab function that will enable me to write letters and numbers using my 6dof manipulator (RRRRRR).
I have a function named
gensym that generates points along the path that the manipulator needs to traverse to write a letter. If the letter can't be written without lifting the manipulator off the paper (for example letter "A"), the function will also generate the points above the paper level that the manipulator will use to jump to the next required coordinate.
The problem arises when using
SerialLink.ikine to generate joint coordinates for every point along the path. I am calling
ikine in a for loop for every point and putting the generated joint coordinates in a matrix, then plotting the manipulator position one by one. The problem is,
ikine will give "illogical" results sometimes, so, instead of moving along the path to draw a line,
ikine rotates some joints by angle $\pi$, producing the same end-effector position, but not going along the "straight line" between the two points.
I made a short gif animation to demonstrate the problem, where you can see how the fourth joint rotates along the path for no reason.
How can I fix this? Are there some options to
ikine that will prevent this?
Edit: At first, I thought it was only the fourth joint that rotates by $\pi$, but, here are the actual joint angles from the positions in the animation (each row represents one position from the animation and each column represents corresponding joint rotation):
-1.5036 0.7771 1.7358 0.0000 0.6287 0.0672 1.6435 -0.8231 -1.2862 0.0000 -1.0323 -3.0689 1.6496 -0.8809 -1.1808 0.0000 -1.0799 -3.0628 1.6552 -0.9426 -1.0679 0.0000 -1.1311 -3.0572 -1.4813 1.0092 1.3116 0.0000 0.8207 0.0895 -1.4773 1.0700 1.1996 0.0000 0.8719 0.0935 -1.4751 1.1096 1.1266 0.0000 0.9054 0.0957
If there isn't a way to prevent
ikine from generating such results, is there a way to fix this afterwards using some kind of algorithm?