[Question] Getting all inverse kinematics solutions

[Thieso]

Hi guys,

I am working with a UR10e robot and am planning a path in cartesian space. If the start and end points are close to each other it works mostly fine but I they are rather far away I have a lot of issues with the inverse kinematics solutions. It keeps finding very extreme positions (some joints close to their limits) and if I add the floor as an obstacle to avoid those solutions, it does not find a solution at all. I have already tried different IK solvers but with no luck.
So my idea was to retrieve all IK solutions and choose one based on my own criteria (something like minimum distance in joint space with some weights on the joints).
My question is now, how can I retrieve these solutions from my Moveit Package to do these weighting?

I am not sure if this is the right place to ask such questions but I would appreciate any help.

Thanks a lot.

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[simonschmeisser]

Ah so you asked ros-industrial/universal_robot#561 as well and had a look at ros-industrial/universal_robot#358 very good!

The officially right place would be answers.ros.org but it's a known issue that questions often lay around unanswered there for a while ...
As you can see in the PR it adds this function:

    virtual bool getPositionIK(const std::vector< geometry_msgs::Pose > &ik_poses, 
                               const std::vector< double > &ik_seed_state,
                               std::vector< std::vector< double > > &solutions,
                               kinematics::KinematicsResult &result,
                               const kinematics::KinematicsQueryOptions &options) const;

You just need to call this and work with the results in solutions. There is no convenience API in MoveIt currently so you need to call this on the IK solver directly. It can be reached via RobotModel

[felixvd]

@Thieso Did that answer your question?

[Thieso]

Ah yes sorry for the late response. This seems like a good explanation.
What I am doing is first to use the RobotModel to get the JointModelGroup, from that get call getSolverInstance() to get the solver and the solver can then be used to call getPositionIK().
Thanks for the explanation

[simonschmeisser]

You're welcome! Please feel very invited to add a short tutorial or a pull request (PR) for a more convenient way if you know a good place to put it

[altairve]

@Thieso Following your idea of getting to KinematicsBase class through getSolverInstance() , I could not find the exact function definition provided by @simonschmeisser . The closest I could find in the documentation is:

virtual bool kinematics::KinematicsBase::getPositionIK	(const geometry_msgs::Pose & ik_pose,
                                                         const std::vector< double > & ik_seed_state,
                                                         std::vector< double > & 	solution,
                                                         moveit_msgs::MoveItErrorCodes & error_code,
                                                         const kinematics::KinematicsQueryOptions & options =
                                                                                  kinematics::KinematicsQueryOptions() 
)const [pure virtual]

But the solution vector size is only six and does not contain more than one IK solutions, even though the IK solver (cached_ik_kinematics_plugin) is printing on the console that it has found 34 IK solutions. Any help would be much appreciated. Thanks.

[aarsht7]

@altairve did you found any way around to get all possible solutions?

[simonschmeisser]

@altairve linked to a seriously outdated version of the documentation (groovy from 2012). The overload that returns all solutions was added later and might not have been implemented by all ik plugins. All relevant information is provided here so please invest some more effort.

[aarsht7]

Hello @simonschmeisser I tried to have a look at the universal_robot repo from your previous comment but I do not find the same code in main repo as the PR commits in the mentioned link as you have mentioned. I am using custom IK fast moveit plugin. I do have similar getPositionIK function in my plugin file but I can not find any moveit tutorial on this topic if there exist any. I am not sure how to utilize the function properly. If you have any example, it would be really helpful.