Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Enabled ActiveDOFs in several planners #318

Merged
merged 11 commits into from
Jul 21, 2016
Merged

Enabled ActiveDOFs in several planners #318

merged 11 commits into from
Jul 21, 2016

Conversation

mkoval
Copy link
Member

@mkoval mkoval commented Jul 21, 2016

This commit sets the ActiveDOFs CollisionOption flag in: OMPL, SnapPlanner, VectorFieldPlanner, and GreedyIkPlanner.

Enabled ActiveDOFs in several planners.
b41d856 
This commit sets the ActiveDOFs CollisionOption flag in: OMPL planners,
SnapPlanner, VectorFieldPlanner, and GreedyIkPlanner.
@mkoval mkoval added the bug label Jul 21, 2016
Michael Koval added 7 commits July 20, 2016 23:55
Fixed DeprecationWarning in CHOMP.
b7d09ab 
Switched from manually calling the constructor of KinBodyStateSaver to
using the CreateKinBodyStateSaver function.
Enabled ActiveDOFs option for CHOMP.
ac4ec92
Add 'spheres' group to enable CHOMP unit tests.
69f54d9
Added CHOMP PlanToConfiguration unit tests.
58313f4
Strip extra groups added by CHOMP.
fe0c2c6
Deleted dead PlanToTSR function in CHOMP.
de25cd0 
This has been commented out for quite some time.
Removed broken PlanToEndEffectorPose from CHOMP.
7613cc3 
This relies on TSR.serialize functionality that no longer exists. It
also never worked reliably.
@mkoval
Copy link
Member Author

mkoval commented Jul 21, 2016

I noticed #319 while writing unit tests for CHOMP. If we resolve that issue, then we can also enable PlanToConfigurationTestCollisionTest on CHOMP.

@cdellin
Copy link
Contributor

cdellin commented Jul 21, 2016

I added a temporary printf in or_fcl's FCLCollisionChecker::SetCollisionOptions method. Note on my machine that the enum value OpenRAVE::CO_ActiveDOFs has value 0x10 = 16.

CHOMP before/after:

calling planning method ...
...
[orcdchomp_mod.cpp:2875 gettraj] timing trajectory ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
[orcdchomp_mod.cpp:2930 gettraj] checking trajectory for collision ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

calling planning method ...
...
RECEIVED OPTIONS 16 (env id 3)!
...
[orcdchomp_mod.cpp:2875 gettraj] timing trajectory ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
[orcdchomp_mod.cpp:2930 gettraj] checking trajectory for collision ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

(I'm not exactly sure what's going on here. During timing trajectory, the module does call OpenRAVE::planningutils::RetimeActiveDOFTrajectory, which I believe sets the CO_ActiveDOFs flag temporarily. But I'm not sure what the last pair calls in both the before and after case are.)

OMPL_RRTConnect before/after:

calling planning method ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
[RRTConnect.cpp:185] RRTConnect: Starting planning with 1 states already in datastructure
[RRTConnect.cpp:292] RRTConnect: Created 5 states (3 start + 2 goal)
[SimpleSetup.cpp:129] Solution found in 0.547832 seconds
RECEIVED OPTIONS 0 (env id 3)!

calling planning method ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
[RRTConnect.cpp:185] RRTConnect: Starting planning with 1 states already in datastructure
[RRTConnect.cpp:292] RRTConnect: Created 5 states (2 start + 3 goal)
[SimpleSetup.cpp:129] Solution found in 0.877408 seconds
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

(I'm not sure what the first two calls are in both before and after. or_ompl currently sets the CO_ActiveDOFs flag internally, and this PR introduces a second state saver around it, so the output otherwise makes senes.)

Snap before/after:

calling planning method ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

calling planning method ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

(Not sure about this one either. Looks like there may be an inner state saver somewhere which enforces the flag to be off, and this PR adds a saver on the outside which turns it on.)

Vectorfield before/after:

calling planning method ...
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

calling planning method ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

(Same as Snap.)

GreedyIK before/after:

calling planning method ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!

calling planning method ...
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 0 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
RECEIVED OPTIONS 16 (env id 3)!
No handlers could be found for logger "prpy.planning.workspace"
RECEIVED OPTIONS 0 (env id 3)!

(Don't ask me about this one!)

Michael Koval added 3 commits July 21, 2016 11:40
Moved CHOMP collision detection to Python.
45565a2
Updated CHOMP unit tests.
7cabafa 
- Disabled PlanToConfigurationCompleteTest because CHOMP is not
  complete and the test currently fails.
- Enabled PlanToConfigurationTestCollisionTest now that CHOMP throws
  more granular exceptions.
Improved CHOMP error messages.
a55151a
@mkoval mkoval mentioned this pull request Jul 21, 2016
Merged
@mkoval
Copy link
Member Author

mkoval commented Jul 21, 2016

@cdellin and I tracked down most of mystery CollisionOptions calls to GetNonAdjacentLinks. The only ones remaining are the repeated calls in GreedyIk, which likely come from the IK filter.

I am content with merging this pull request once @cdellin signs off on it.

@cdellin
Copy link
Contributor

cdellin commented Jul 21, 2016

This looks good to me!

@mkoval mkoval merged commit eba7646 into master Jul 21, 2016
@mkoval mkoval deleted the bugfix/ActiveDOFs branch July 21, 2016 17:10
This was referenced Jul 23, 2016
Closed
Open
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees

@cdellin cdellin

Labels
bug
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@mkoval @cdellin