inconsistent tcp_frame in model and ikfast

[jhu-gh]

I came across the following behavior recently when i was using this package and not sure what's going on under the hood, was thinking maybe should ask here.

In my code, I did the following:
descartes_core::RobotModelPtr model (new descartes_moveit::IkFastMoveitStateAdapter());
const std::string robot_description = "robot_description";
const std::string group_name = "manipulator";
const std::string world_frame = "base_link";
const std::string tcp_frame = "ee_link";
bool ret = model->initialize(robot_description, group_name, world_frame, tcp_frame);

but somehow in the terminal, it printed: initialized with IKFast tool frame 'tool0' and base frame 'base_link', which means, the tool frame used to initialize ik solver is 'tool0' instead of 'ee_link'. Then, I looked at the source code, realized ikfast_tool_frame is set here, which tries to get the ikfast_tool_frame from rosparam server, however, user is not asked to set that param during initialization. My questions are:

1. should set ikfast_tool_frame param on ros server be integrated into robot model initialization? like somewhere here?

2. I also checked my forward kinematics, if I didn't manually change the default_tool_frame here, I would get incorrect (not what i want) forward kinematics too. should fk and ik initialization somehow disentangled?

I'm using Ubuntu 18 and ROS melodic.

Thanks.

[avwhite]

I am not sure I 100% percent understand exactly what your issue is, but I know that I spend some time dealing with work object and tool transforms in relation to Descartes and Moveit. I can't remember all the details, but I created this issue: #241 at the time.

Maybe this has nothing to do with what you are talking about, but maybe it can help you?

[gavanderhoorn]

I haven't checked, but please be aware that IKFast plugins are generated code. You cannot change the links or locations of frames after the code has been generated.

I'm not entirely sure how things are integrated here, just wanted to draw attention to this fact.

[jhu-gh]

I am not sure I 100% percent understand exactly what your issue is, but I know that I spend some time dealing with work object and tool transforms in relation to Descartes and Moveit. I can't remember all the details, but I created this issue: #241 at the time.

Maybe this has nothing to do with what you are talking about, but maybe it can help you?

Thanks for your pointer. You mentioned that you used a "IK solver plugin generated by the moveit utility", did you mean "descartes/descartes_moveit/" this folder? which also echoes with @gavanderhoorn 's comment, that IKFast plugins are generated code. Now I'm wondering if I'm using this package the right way. What I did is the following:

1. git clone this descartes repo to my ros workspace and build
2. based on the tutorial, wrote my own code
3. launch files under ur5_moveit_config (generated by moveit setup assistant) so that "robot_description" and move_group are defined
4. run my own code to plan a semi-constrainted path

Maybe I missed something.

@gavanderhoorn It would be great if you can clarify if "IKFast plugins" refers to the descartes_moveit folder or specific files in this package. Thanks for your time.

Also i just noticed this package, maybe it'll make my life using descartes easier.

[avwhite]

You mentioned that you used a "IK solver plugin generated by the moveit utility", did you mean "descartes/descartes_moveit/" this folder?

No. The stuff in there is the "moveit adaptor". It allows descartes to tap into moveit, to get information about the robot, the planning scene, and the IK solver.

Moveit itself can use different IK solvers under the hood via IK plugins. It provides a uniform interface to these IK solvers which Descartes can use via the MoveitStateAdaptor (in the descartes/descartes_moveit directory). When creating a moveit config for a robot, you can choose which IK solver plugin to use. The standard one is called KDL. It is a numeric solver. Due to the way it works it gives one solution at a time.

The standard interface Moveit exposes for IK solvers return only one solution (probably inspired by KDL). The problem is that for descartes to work well, it needs all IK solutions IKFast does this (really quickly), but through the moveit interface you only get one. You can see how descartes obtains multiple IK solutions from moveit here:

descartes/descartes_moveit/src/moveit_state_adapter.cpp

Line 182 in 26197a9

bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std::vector<double> >& joint_poses) const

by essentially calling the IK solver many times.

The problem is that this is slow (and maybe not guaranteed to give you all solutions?). You could use IKFast through this moveit state adaptor, but then you end up calling IKFast multiple times only to throw away most of the work (and maybe not even get all solutions?).

This is why the ikfast_moveit_state_adaptor exists. It uses the moveit state adaptor for everything except IK. For IK it obtains the IKFast IK solver instance from moveit and then interacts with it directly, foregoing most of the standardized moveit IK interface. At least this is my understanding.

The problem with IKFast is that it is not actually an IK solver. It is a tool for generating an IK solver for a specific robot. Basically it needs the IK parameters at compile time. So you need a different IKFast plugin for each robot. This is the generated code we are talking about. Not the moveit state adaptor (or the ikfast version). If you are using IKFast, you should have a package called something like robot_name_ikfast_plugin and set up moveit to use the IKFast IK solver in the setup assistant.

In order for moveit to load and use IKFast (remember, moveit can still use it, even if the interface it exposes does not work that well with it). Moveit has its own wrapper code around IKFast (which is also generated by some moveit tool).

In descartes the moveit state adaptor (to this day I believe) compensates for the workobject and tool frames (using the descartes parameters). Newer versions of the moveit wrapper code around IKFast also compensates for workobject and tool frames(using the frames set up in moveit). This was my problem. I am not sure if it is related to yours. But maybe this whole explanation makes things clearer.

In the end I am using both moveit and descartes, so I just ended up setting everything up to use the base frames and do the compensation myself. Maybe not the optimal solution, but at least it works...

[jhu-gh]

You mentioned that you used a "IK solver plugin generated by the moveit utility", did you mean "descartes/descartes_moveit/" this folder?

No. The stuff in there is the "moveit adaptor". It allows descartes to tap into moveit, to get information about the robot, the planning scene, and the IK solver.

Moveit itself can use different IK solvers under the hood via IK plugins. It provides a uniform interface to these IK solvers which Descartes can use via the MoveitStateAdaptor (in the descartes/descartes_moveit directory). When creating a moveit config for a robot, you can choose which IK solver plugin to use. The standard one is called KDL. It is a numeric solver. Due to the way it works it gives one solution at a time.

The standard interface Moveit exposes for IK solvers return only one solution (probably inspired by KDL). The problem is that for descartes to work well, it needs all IK solutions IKFast does this (really quickly), but through the moveit interface you only get one. You can see how descartes obtains multiple IK solutions from moveit here:

descartes/descartes_moveit/src/moveit_state_adapter.cpp

Line 182 in 26197a9

bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std::vector<double> >& joint_poses) const

by essentially calling the IK solver many times.

The problem is that this is slow (and maybe not guaranteed to give you all solutions?). You could use IKFast through this moveit state adaptor, but then you end up calling IKFast multiple times only to throw away most of the work (and maybe not even get all solutions?).

This is why the ikfast_moveit_state_adaptor exists. It uses the moveit state adaptor for everything except IK. For IK it obtains the IKFast IK solver instance from moveit and then interacts with it directly, foregoing most of the standardized moveit IK interface. At least this is my understanding.

The problem with IKFast is that it is not actually an IK solver. It is a tool for generating an IK solver for a specific robot. Basically it needs the IK parameters at compile time. So you need a different IKFast plugin for each robot. This is the generated code we are talking about. Not the moveit state adaptor (or the ikfast version). If you are using IKFast, you should have a package called something like robot_name_ikfast_plugin and set up moveit to use the IKFast IK solver in the setup assistant.

In order for moveit to load and use IKFast (remember, moveit can still use it, even if the interface it exposes does not work that well with it). Moveit has its own wrapper code around IKFast (which is also generated by some moveit tool).

In descartes the moveit state adaptor (to this day I believe) compensates for the workobject and tool frames (using the descartes parameters). Newer versions of the moveit wrapper code around IKFast also compensates for workobject and tool frames(using the frames set up in moveit). This was my problem. I am not sure if it is related to yours. But maybe this whole explanation makes things clearer.

In the end I am using both moveit and descartes, so I just ended up setting everything up to use the base frames and do the compensation myself. Maybe not the optimal solution, but at least it works...

Really appreciate your explanation, it has clarified many things for me!

So I followed the moveit tutorial installed openRave, and generated IKFast plugin for my robot, during the process, it did ask me to specify the base link and end effector link, as expected, at the end, IKFast plugin modified my kinematics.yaml to use the IKFast plugin, instead of the default kinematics solver selected in setup assistant, which would be empty file if kinematics solver is left unchosen.

correct me if I'm wrong, but what i've realized so far is that user has to be consistent in generating IKFast plugin and initializing a descartes_core::RobotModelPtr in terms of world_frame and tcp_frame names. If you happen to use "base_link" and "tool0" in generating IKFast plugin, then, you'll get correct FK/IK results, otherwise, ikfast_moveit_state_adapter will use the default tool_frame and base_frame names, unless "ikfast_base_frame" and "ikfast_tool_frame" are set using rosparam by the user, because afaik, IKFast plugin doesn't set those two ros params.

I'm also using both moveit and descartes so far, and I guess I'll just use ros param to avoid changing anything in descartes package. Again, thanks for your time, I've learned a lot.

[avwhite]

I have just refreshed my mind a bit on the details. Sorry if I am just repeating myself. At this point I am basically just documenting what I have found for myself.

The ikfast moveit state adapter deals with two sets of transforms. The ikfast_base_frame and ikfast_tool_frame needs to be consistent with the ones chosen when generating the ikfast solver. These should probably always be the base frame and flange of the robot. These have the default values of base_link and tool0 and can be overwritten with the rosparams ikfast_base_frame and ikfast_tool_frame.

The frames passed in when initializing the RobotModel (concretely you would be using IkFastMoveitStateAdapter which inherits from it via MoveitStateAdapter) are different. These are supposed to be the frames you are going to actually use for planning. When using the ikfast adapter it will calculate the transforms between these frames and the ikfast frames above on initialization via this function:

descartes/descartes_moveit/src/ikfast_moveit_state_adapter.cpp

Line 138 in 26197a9

bool descartes_moveit::IkFastMoveitStateAdapter::computeIKFastTransforms()

and then correct for them like for instance in this line:

descartes/descartes_moveit/src/ikfast_moveit_state_adapter.cpp

Line 74 in 26197a9

Eigen::Isometry3d tool_pose = world_to_base_.frame_inv * pose * tool0_to_tip_.frame;

One caveat here is the following code in the MoveitStateAdapter initialization which also gets called when initializing IkFastMoveitStateAdapter:

descartes/descartes_moveit/src/moveit_state_adapter.cpp

Line 115 in 26197a9

if (tool_frame_ != link_names.back())

Not entirely sure what is going on here, but it is checking the tool frame you are trying to initialize the descartes model with matches something in moveit. As long as you are using the IkFastMoveitStateAdapter, this is not actually required you can comment it out as long as you don't use MoveitStateAdapter directly. The code in IkFastMoveitStateAdapter works for any frame which is fixed with respect to the tool frame set up in the ikfast plugin.

The problem (the one I had) arises because newer version of the IKFast code generated via moveit also tries to do the compensation which IkFastMoveitStateAdapter does (moveit puts a thin wrapper around ikfast which IKFastMoveitStateAdapter also uses). AFAIK it does this based on which frames you choose as base frames and tool frames in moveit. In moveit it is not as simple as just selecting two frames.

The first thing I tried was to have moveit do the compensation by disabling it in descartes (initializing with robot base and flange frames, or just modifying the descartes code so it doesn't do the correction). But setting these frames in moveit didn't work that great for me. For instance, I could not really get it to use a frame which only shared an ancestor with the robot base as the base frame. It insisted that it should be a direct ancestor. Maybe I did something wrong.

Then I tried to have Descartes do it, but ended up running into the caveat mentioned above. As also described above, I found out you could just comment out that check, and it will still work. With this approach there is no compensation when calling moveit directly, so you have to compensate manually there.

Then I wanted to be able to change the tool frame at runtime. At that point I basically gave up, set everything to match the robot base and flange frame and created my own wrapper around both moveit and descartes which handles the compensation (Actually, looking at it again, it seems I have both descartes and moveit handling the base frame adjustment, but not the tool. For moveit I am doing it via: move_group.setPoseReferenceFrame I am not going to spend more time (re-)figuring out why this works right now, but it appears to do).

In summary, you might be better off just having everything consistently set to the robot base and the robot flange as specified when generating the ikfast plugin. Then handle any transformations manually. Using the built in features of moveit and descartes for this is confusing, especially when you need them both to work at the same time.

P.S: Some other tips:

I have made some modifications to descartes. You can see them here: melodic-devel...devOdico:odico-modifications#diff-56d9568a793dd8cf991c8ca4e9abcb6e9ddd797e2b17564ff8216954e0045f28R90 Note that everything here is done in the laziest way possible to get it to work in exactly my use case.

Explanations:

1. If your robot has an axis which runs for more than 360 degrees, IKFast only gives one solution. It is pretty easy to add in the other ones after the fact. You can probably find it in the linked code (note that it looks like it tries to do it for both joint 4 and 6, but I later disabled it for joint 4. So that code only adds extra solutions for joint 6).
2. I have added some code where you can set a planning scene and changed the collision checking code to use this so it also checks against collision objects in the scene. Note that this does not support a dynamic planning scene. It is basically a snapshot which only changes when the set planning scene function is called.
3. The check is commented out as mentioned above.

[jhu-gh]

Now i understand your problem more, and realized that I did miss some information.
This

The ikfast_base_frame and ikfast_tool_frame needs to be consistent with the ones chosen when generating the ikfast solver. These should probably always be the base frame and flange of the robot. These have the default values of base_link and tool0 and can be overwritten with the rosparams ikfast_base_frame and ikfast_tool_frame.

and this

When using the ikfast adapter it will calculate the transforms between these frames and the ikfast frames above on initialization via this function:

have helped to clarify my problems in using descartes. I didn't notice that tool0_to_tip and world_to_base are calculated and compensated in this package. whereas your problem is more complicated because you have world -> object.

I think

In summary, you might be better off just having everything consistently set to the robot base and the robot flange as specified when generating the ikfast plugin. Then handle any transformations manually.

is the correct practice to follow to use this package. Thanks!