Ask Your Question

MoveIt - Python interface - TRAC-IK -How to request closest IK solution to current joint configuration?

asked 2021-02-19 19:35:22 -0500

cambel07 gravatar image

updated 2021-02-21 05:17:54 -0500


I am trying to use MoveIt's python interface to plan cartesian trajectories with a UR3e robot using ROS-Melodic. I have set up everything and it works fine. I am using it with the TRAC-IK solver.

The problem is that when I get a solution from the planner, the solution can change very drastically from a small motion to an unnecessarily long trajectory. The main difference between trajectories is the goal joint configuration from the goal pose. So my question is, how to request the closes IK solution to the current joint configuration?

I can do that manually by computing the IK solutions using TRAK-IK, computing the closes solution to my current configuration, and then requesting MoveIt to plan using the joint configuration that I choose rather than using a goal pose. When I do that, the resulting trajectory is always similar. However, isn't there a way to request MoveIt to use a specific IK solution over another?

Edit: Video for clarity. From the same initial position, and to the same target pose (Cartesian goal), different IK solutions.

edit retag flag offensive close merge delete


I'm not sure I understand what you're asking. When you say "cartesian trajectories", do you mean one calculated by the cartesian path planning function? Because for 6-DOF robots this should not depend on the IK solution at the goal pose. Also, do you mean "How to request closest IK solution to a given joint configuration?"

fvd gravatar image fvd  ( 2021-02-20 00:07:07 -0500 )edit

Is TracIK configured to use its Distance solve type, instead of the default Speed?

gvdhoorn gravatar image gvdhoorn  ( 2021-02-20 01:12:04 -0500 )edit

@fvd Sorry, about "cartesian trajectories" I meant that I am using the move_group.set_pose_target() method, which offers no option to specify which of the possible IK solutions to use.

And, yes, I also mean to ask "how to request the closest IK solution to a given joint configuration?", it just that I want to get the closest to the current joint configuration of the robot.

I included a video in the question for clarity

@gvdhoorn no, how do I configure TRACK-IK to do that? I checked the code and the logs and it is expecting a rosparam move_group(leftarm in my case)/solve_type, but even if I specify "Distance" it still doesn't seem to recognize it :/

Looking in common namespaces for param name: leftarm/solve_type

Using solve type Speed

cambel07 gravatar image cambel07  ( 2021-02-20 03:00:48 -0500 )edit

Can you add how you define the parameter your question?

fvd gravatar image fvd  ( 2021-02-20 03:32:28 -0500 )edit

how do I configure TRACK-IK to do that?

In the kinematics.yaml of your MoveIt cfg package, like so (manipulator is the group name here, change to whatever you have of course):

  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  solve_type: Distance
gvdhoorn gravatar image gvdhoorn  ( 2021-02-20 07:54:45 -0500 )edit

@gvdhoorn thank you! Silly me, I was publishing a rosparam with that "manipulator/solve_type" but that way didn't work. I wonder why...

Anyways, I just tried using the kinematics.yaml and it worked. TRAC-IK recognizes the solver type and it does seem to work as I wished, the IK solution seems to be the same every time.

cambel07 gravatar image cambel07  ( 2021-02-20 09:20:54 -0500 )edit

@fvd sorry, I don't understand your question. Do you mean the TRAC-IK parameter?

if that's the case I defined the parameter <param name="leftarm/solve_type" type="str" value="Distance" />, I defined the parameter before calling the move_group definition, and I checked that the parameter existed in the parameter server but still for whatever reason TRAC-IK was not able to read from it or something.

cambel07 gravatar image cambel07  ( 2021-02-20 09:25:21 -0500 )edit

@gvdhoorn thank you! Silly me, I was publishing a rosparam with that "manipulator/solve_type" but that way didn't work. I wonder why...

it needs to be in a specific namespace. If it's not there, TracIK will not find it and default to Speed.

gvdhoorn gravatar image gvdhoorn  ( 2021-02-20 09:29:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2021-02-22 10:09:57 -0500

fvd gravatar image

updated 2021-02-22 10:13:04 -0500

A workaround was already proposed in the comments, but for context:

  • In C++, you should be able to obtain multiple IK solutions from the Kinematics API, and then sort through them yourself, as described in this question. You can also see how RobotState uses Kinematics methods here. The need for a function to get the "closest" IK solution from the KinematicsBase API has been brought up in a Github issue.

  • The Python interface is quite limited, especially for moveit_core (the C++-native internals like this). There are quite a lot for the move_group_interface(MoveGroupCommander in Python), and a few for the RobotState and planning_scene_interface. There have been long discussions about this on Github. There is no obstacle to adding Python bindings yourself as needed - except that it is a bit confusing that they are defined in two places (here and here for the move_group_interface).

  • Another option to access C++ functionality in Python is to add a MoveGroupCapability and then call that in your Python code. You can go all the way and add a convenience function to the MoveGroupInterface and expose that to Python as well.

I hope that is a little helpful. As always, if you implement something that might be useful or fix a problem, do consider contributing it upstream. Cheers

edit flag offensive delete link more


Pedantic, but technically, the answer is then still: "right now, this is not supported".

gvdhoorn gravatar image gvdhoorn  ( 2021-02-23 03:23:16 -0500 )edit

The "this is not supported right now, but" is silent :)

fvd gravatar image fvd  ( 2021-02-23 03:34:10 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2021-02-19 19:35:22 -0500

Seen: 306 times

Last updated: Feb 22 '21