search

JeroenDM / moveit_opw_kinematics_plugin

An attempt at writing a MoveIt! plugin for opw_kinematics.
http://wiki.ros.org/moveit_opw_kinematics_plugin
Apache License 2.0
7 stars 7 forks source link

extend solution space to > +-180° #24

Closed simonschmeisser closed 5 years ago

simonschmeisser commented 5 years ago

opw only returns solutions within +-180° and cannot be extended easily as it has no notion of joint limits. We now check for all solutions from opw if any joint is still > min_limit for -n*2pi and vice versa

JeroenDM commented 5 years ago

An important feature, thanks!

I quickly wanted to add a test for this but didn't realize searchPositionIK uses a callback to return multiple solutions that I never used, so it is not going to be quick... :(

Another thought I had was to write it in a separate function and reuse this in the getPositionIK. Even if we do not use it in getPositionFK, it is probably cleaner to do it in a separate function because searchPositionIK is getting out of hand (> 100 lines).

simonschmeisser commented 5 years ago

I quickly wanted to add a test for this but didn't realize searchPositionIK uses a callback to return multiple solutions that I never used, so it is not going to be quick... :(

actually (or afaik) this callback just has a misleading name and is a validity callback, ie it allows you to add additional conditions that need to be fulfilled for a solution to be considered valid. We use this to check for collision free sollutions (there might be alternative solutions with a bigger distance to the seed position that are collision free while the closest solution is in collision)(another use case is that MoveIt might have tighter joint limits than the URDF, we check those in the callback until #25 )

simonschmeisser commented 5 years ago

I extracted the code into a separate function but do understand the second part of your comment unfortunately :|

JeroenDM commented 5 years ago

I quickly wanted to add a test for this but didn't realize searchPositionIK uses a callback to return multiple solutions that I never used, so it is not going to be quick... :(

actually (or afaik) this callback just has a misleading name and is a validity callback, ie it allows you to add additional conditions that need to be fulfilled for a solution to be considered valid. We use this to check for collision free sollutions (there might be alternative solutions with a bigger distance to the seed position that are collision free while the closest solution is in collision)(another use case is that MoveIt might have tighter joint limits than the URDF, we check those in the callback until #25 )

Ah, ok, I completely missed the part of the code where the callback is checked. I'm sorry.

It sounds like a nice feature for some cases. I'm curious to know what you gain with this, compared to running the callback on the solution returned by searchPositionIK in the higher-level code.

The code is already very readable now I think. Further cleaning of the long functions would not be related to this pull request in my opinion.

simonschmeisser commented 5 years ago

It sounds like a nice feature for some cases. I'm curious to know what you gain with this, compared to running the callback on the solution returned by searchPositionIK in the higher-level code.

There is currently (AFAIK) no way to get more than one ik solution from higher level code (is that true?) so if we do the collision checking in higher-level code we will just get the solution closest to the seed position. This solution might be in collision, while another solution might be collision free. That's why we use the callback to have it continue "searching"

JeroenDM commented 5 years ago

There is currently (AFAIK) no way to get more than one ik solution from higher level code (is that true?)

Oooooooh, now I see, glad you mentioned it. This was meant to be implemented here as discussed (with myself...) in #4. Recently this functionality is also officially described in the base class. And I agree with the comment made there: TODO(dave): This dual behavior is confusing and should be changed in a future refactor of this API.

This was originally the whole reason I implemented this plugin, but because I never ended up using it I completely forgot about it...

Edit: It is implemented but I need to add joint limit checking and solution extension to this function too.

simonschmeisser commented 5 years ago

Ah, ok, then it was the RobotState API missing those functions ...

JeroenDM commented 5 years ago

If it's ok with you I will squash-merge this and start working on tests in a new branch. Do you have a suggestion for an existing robot to tests the extended joint solutions on?

JeroenDM commented 5 years ago

In response to my own question, the fanuc robot already used in the tests has a range from -6.28 to 6.28 on the last joint. A good starting point.

JeroenDM commented 5 years ago

I hope it's ok I formatted the code, those where more changes than I expected.

simonschmeisser commented 5 years ago

I still need to integrate clang-format into my workflow, those formatting changes are from older PRs of mine I guess.

We did some manual testing here so I'm fine with merging it now and I'm somewhat sorry for not adding testcases myself because of ... ... :wink: