Return of multple IK configurations of goal state.

edit

I find a way of obtaining some of the possible IK solutions for a given goal state, by using a custom implementation of the GroupStateValidityCallbackFn when calling the method setFromIK(...) from a robot_state::RobotState instance.

With this callback function, I can determine if the returned IK solution is valid or not and then try to get another one that fulfills my requirements.

This approach works along with the moveit plugin structure by only using a generic function to obtain an IK solution which is then implemented by the IK solver plugin with which the robot is configured.

The code looks like this:

std::vector<double*> invalid_states;        //Variable holding the discarted IK solutions.

robot_state::RobotState target_state( move_group_interface_instance.getCurrentState() );

while( target_state.setFromIK(joint_model_group, target_pose, NUMBER_OF_ATTEMPS, TIMEOUT, checkIkValidity)) ){
   // Plann for following motions using the return state as start state, and validate whether the IK solutions works or should be discarded
   // if (solution is not valid)
   //    add IK solution to invalid_states vector
}

Then my implementation of the GroupStateValidityCallbackFn:

bool checkIkValidity(robot_state::RobotState* robot_state, const robot_state::JointModelGroup* joint_group, const double* joint_group_variable_values){
        return true/false // Depending on your desition scheme.
    }

Checks if the returned IK solution is too similar to the discarded IK solutions saved on the invalid_states vector.

I realize that this function should be configured depending on the robot kinematics configuration (i.g. max possible IK solutions, timeout, IK solutions similarity measurment...etc) but in any case, it worked for me and might do the trick for someone else too.