I am getting very wrong planned solutions for my robot. The frame for which I am planning does not reach the position specified in the position constraint even though the move_group action succeeds. This is a sporadic issue. It more often produces good plans, (about 25 out of 30 times it is good) but it still produces some bad plans while showing succeeded.
When I look at the final position of the bad plans, I see that some of the joint positions are maxed out to their extreme limits. It is as if the IK solver computed a move for them to move beyond their joint limits and it added all the points in the trajectory that it could without violating the limits.
Note that I am not using pathpilot, but interacting directly with the move_group action using all the default plugins. I have defined a custom gripper and custom groups.
For example, these are the joint positions of my start state:
[-1.964511736136016e-08, -0.2137400359076774, 0.5482885836109729, 5.7849939892502386e-08, -0.383620470680354, 1.24270021880501]
An optimal solution to reach my goal position constraint (PTP, using pilz) would be the following joint states:
[-0.6218598706510868, 1.0406937951301265, 0.7716561220997629, -0.6358653867476644, -1.7664725034207511, 1.1132525213608377]
Another valid, though not optimal solution that involves unnecessary twists and turns of the robot arm is this:
[-0.6172537308277318, 1.0404191342757776, 0.7745574165579882, 2.5100597315065967, 1.7692473976769476, 4.25416103641855]
Here are examples of incorrect ending positions:
[-0.6172542613323999, 1.0404097464475126, 0.7745566315101212, -3.1414999999999997, 1.7692329273338998, -6.283185307179586]
[-0.6218719519863527, 1.0406839918984905, 0.7716555344512751, -0.6358846626272823, -1.7664568711836162, 6.283185307179586]
[-0.6172622388669395, 1.040414551841883, 0.7745510811574023, -3.1415, 1.7692323064490967, -2.029010377560164]
I found this issue with bio_ik that sounds like my same issue, but I am not sure if it is the same implementation that Tormach uses or if the error would be on the za6.
Have you run into this problem at Tormach?
Also, is the default implementation of the za6 tied to bio_ik? And would the tormach implementation be amenable to me tweaking the IK parameters? I would investigate trying other IK solvers available to moveit, but I am not sure how tormach has modified moveit with the global and tcp groups and whether or not it would work to use other solvers.
Thanks!