search

ros-industrial-consortium / descartes

ROS-Industrial Special Project: Cartesian Path Planner
Apache License 2.0
126 stars 92 forks source link

Robot base and tool offsets corrected for twice when using descartes with IKFast plugin generated by MoveIt. #241

Open avwhite opened 4 years ago

avwhite commented 4 years ago

When generating an IK solver with IKFast, we use the robot base as the "world frame" and the last link of the robot as the "tool frame".

When generating trajectories, we might wish to specify them in relation to some other base frame, and have a tool attached on the robot follow the path. This can be done using the same IK solver, simply by applying the appropriate transform to the input.

The IKFast moveit state adaptor in Descartes does this.

The problem is that the IKFast moveit plugins generated from the IKFast solver by the create_ikfast_moveit_plugin also does this.

So, as far as I can tell, when you use Descartes with an IK solver plugin generated by the moveit utility, using non-identity base and tool frames will not work because the transform gets applied twice.

I have managed to get the expected behavior by not doing the transform on line 74 of descartes_moveit/src/ikfast_moveit_state_adapter.cpp. Instead I just set tool_pose = pose.

I don't know if I am doing something wrong. If I am not it seems to me that something should be changed so this transform can be disabled (or completely removed) from one or both places.

avwhite commented 4 years ago

I have been looking further into this.

It seems like the IKFast plugin (as generated by the moveit tool) will always compensate to plan with respect to the frames which have been configured in moveit. (this makes sense considering it is a moveit plugin).

The descartes ikfast moveit state adapter meanwhile always compensates with respect to the frames the descartes robot model has been initialized with.

I cannot figure out how to configure moveit to use a base frame which is not part of a single "chain" with the robot. This leaves the option of configuring moveit to use the same frames as the raw IKFast solver (so that it does no transformations), and then having descartes handle the transformations. (this is different from the approach above, where I disabled the transformation in descartes, but then I only used a different tool frame which I could figure out how to configure in moveit).

There is just one problem. Descartes will complain and fail to initialize the robot model when using a tool frame different from the IK solver plugin tool frame. This is because, as it also says in the error message, this functionality has not been implemented yet. Except when you use the ikfast moveit state adaptor subclass this functionality actually is implemented, but it still calls its parents class initialization function which creates the error.

Simply commenting out the check in the moveit state adaptor makes everything work, but this will ofcourse break if you don't use IKFast, so the real solution is probably one where the ikfast adaptor subclass does its own initialization of these transforms. Or maybe just implementing it in the base moveit state adaptor class.

So I think I have solved the problem for my use case now, but if I am misunderstanding anything please let me know. I might create a PR which lets the ikfast moveit state adaptor succesfully initialize with a tool frame different from the IKFast solver's.

Maybe descartes should not transform into the IKFast solvers frames, but instead into the frames which have been set up in moveit. Then moveit (via the IKFast plugin) would be responsible from transforming from these frames into the raw IK solvers frames? (of course only when using the moveit state adaptors).

gavanderhoorn commented 4 years ago

A quick comment: the IKFast plugin transforming poses back to the base/tip frame(s) is a recent addition (as in: couple months, I believe https://github.com/ros-planning/moveit/pull/1014), and it's likely that the Descartes integration just hasn't been updated to take this into account.

avwhite commented 4 years ago

Thanks for the comment. That makes a lot of sense. Maybe I should look into how to set up moveit to use a base frame not connected to the kinematic chain of the robot, and then go back to disabling the transformation in descartes.

gavanderhoorn commented 4 years ago

Maybe I should look into how to set up moveit to use a base frame not connected to the kinematic chain of the robot

late, but: this is probably a misunderstanding, but how would MoveIt transform incoming poses if the frame is not connected to the robot?

avwhite commented 4 years ago

What I mean is that if world -> base_link -> link1 -> ... -> tool and also world -> object, I was not able to figure out how to set object to be the base (which coordinates are specified in) and tool to be the tool . There exist a transform between them, but they are not direct descendants of each other. (neither base_link or object are currently moving with respect to world while the system is running, it is more for convenience. I don't know if my current setup would support that)

To be honest I did not try very hard, as I am not sure I really understand what all the options in the moveit setup assistant are for, and what the best practices are. So after I could not get it to work immediately, I opted to go with the simplest moveit settings, and went with the approach of getting descartes to do the extra transformations.