Incorrect IK solutions

Robots
1

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!

2

@tbh I believe you might have already solve this problem at this time, and while we do not officaly support this low-level functionality, I have some comment that might help your or other users:
bio_ik has a problem of finding deterministic cartesian solutions if your seed and the goal are far apart. In essence, if you need to find a solution that is lies close in the joint space to your current joint state (or the joint state that you treat as a seed for bio_ik solver), then you are all good.

But for bigger distances, it often finds ones that are not the closest ones, this happens especially if the start and end joint positions are far apart or you start or finish the move close to a singuarity pose.

This is only problematic for a PTP moves to a Cartesian goal.

A hack you can do is to plan a linear move to this goal (and do not execute it), and then fetch the joint solution of the last waypoint of this move. Then you can use this solution as your joint space goal (an equivalent of movej(j), but effectively you will move to the Cartesian position you have solved for.

This works because solutions that are gradually found on the continuous linear path are practically always deterministic when you start from a certain configuration at point A, you will end at some determined configuration at point B.

3

@Jakub_Kaminski Thanks for following up with me! I have indeed solved this for our use case and I have also learned a lot since I originally asked, so I’ll share that in hopes of helping others.

When I originally asked the question, I was using the move_group action to plan a trajectory to a cartesian position. I have since switched to calling the compute_ik service directly and then sending a joint_space goal to move_group. This gives me additional control that move_group does not allow and I do not think it has given me an incorrect solution yet.

For example, when calling move_group to plan [and execute] a move, it forces you to use either joint constraints or position constraints and throws an error message if you specify both. However when I call the compute_ik directly, in addition to setting the desired pose of the request, I can also specify other constraints, such as a joint constraint to keep joint_4 within 2 radians of its 0 position and therefore avoid out of bounds positions.

In addition, because of my end effector and particular use case, I often know in advance at what position joint_6 must be in order to achieve my goal. The objects and poses to which I need to go are all either horizontal or vertical. So supplying the radian value for joint_6 to point down for example, with all the other joints seeded to 0 has consistently succeeded in giving me correct ik solutions.

Possibly supplying the seed in this way would also work with move_group, but I prefer to call the ik separately now, anyway.

There is one caveat with linear moves that I will mention. I called the ik service to lookup a pose for the robot for a certain position and then told the robot to do a linear move to that joint-space position from the current position. The joint-space goal given had joint_4 in the middle of its range, at about 0 radians. But the robot was already in a pose where joint_4 was at pi radians, aka the end of its travel. I expected joint_4 to go back to 0, turning -pi radians, but it did not. The end effector did, however, get to the desired position. So in cases like these it will not travel to your specified joint-space position, but it will travel to the identical cartesian-space position and it will still succeed. It seems as though for linear moves specified in joint-space, it converts your joint-space goal to cartesian-space, calculates the individual cartesian-space positions along the line specified and then converts each position into a joint-space pose closest to the current pose.