Add wrapper for OMPL path simplifier #164
Conversation
| /// \param _dmetric A valid distance metric defined on the StateSpace | ||
| /// \param _sampler A Sampleable that can sample states from the | ||
| /// StateSpace. Warning: Many OMPL planners internally assume this sampler | ||
| /// samples uniformly. Care should be taken when using a non-uniform sampler. |
There was a problem hiding this comment.
Minor Nit: I don't know if this is actually true. As far as I know, most randomized algorithms require that the distribution has non-zero probability of sampling all configurations (i.e. have support over all of configuration space). There is nothing special about the uniform distribution. In fact, what it means to be "uniform" depends on your choice of measure.
@siddhss5 Is what I said above true? 😅
There was a problem hiding this comment.
Yes! The only requirement is that the sampler densely sample configuration space.
| /// \param _boundsProjector A Projectable that projects a state back within | ||
| /// valid bounds defined on the StateSpace | ||
| /// \param _maxPlanTime The maximum time to allow the planner to search for a | ||
| /// solution |
There was a problem hiding this comment.
Nit: Specify units (i.e. seconds).
There was a problem hiding this comment.
Removed this parameter actually. Forgot to take it out of the comment as well. But will add a similar parameter for the timeout as suggested in your other comments.
| /// \param _maxPlanTime The maximum time to allow the planner to search for a | ||
| /// solution | ||
| /// \param _maxDistanceBtwValidityChecks The maximum distance (under dmetric) between | ||
| /// validity checking two successive points on a tree extension |
There was a problem hiding this comment.
Nit: Is "tree extension" meaningful for a shortcutting algorithm?
| constraint::TestablePtr _boundsConstraint, | ||
| constraint::ProjectablePtr _boundsProjector, | ||
| double _maxDistanceBtwValidityChecks, | ||
| trajectory::InterpolatedPtr _originalTraj); |
There was a problem hiding this comment.
I'd prefer to return an std::unique_ptr<Interpolated> rather than a shared_ptr. It's easy to convert from a unique_ptr to a shared_ptr, but the converse is impossible.
src/planner/ompl/Planner.cpp
Outdated
|
|
||
| // Step 3: Use the OMPL methods to simplify the path | ||
| ::ompl::geometric::PathSimplifierPtr simplifier; | ||
| simplifier.reset(new ::ompl::geometric::PathSimplifier(si)); |
There was a problem hiding this comment.
It is supfluous to use a shared_ptr here. Just declare a local variable with type PathSimplifier.
src/planner/ompl/Planner.cpp
Outdated
| // Step 3: Use the OMPL methods to simplify the path | ||
| ::ompl::geometric::PathSimplifierPtr simplifier; | ||
| simplifier.reset(new ::ompl::geometric::PathSimplifier(si)); | ||
| bool const shortened = simplifier->shortcutPath(*path, 1, 1, 1.0, 0.005); |
There was a problem hiding this comment.
Make maxSteps, maxEmptySteps, rangeRatio, and snapToVertex parameters of this function.
Also, note that the current hard-coded settings are (in my opinion) nonsensical. Setting maxSteps = 1 means that it will only try to shortcut two randomly selected points before returning. The default values in OMPL are a more reasonable choice, possibly with the modification that rangeRatio = 1.0 and snapToVertex = 0.0.
Another option is to leave maxSteps = 1, but call shortcutPath in a loop until a time limit (or other termination condition) expires. This is the approach I used for OMPLSimplifier in or_ompl.
src/planner/ompl/Planner.cpp
Outdated
| } | ||
| else | ||
| { | ||
| return _originalTraj; |
There was a problem hiding this comment.
Returning _originalPath here is dangerous because it aliases the input argument. This could very easily cause a bug because the user expects _originalTraj to be unmodified.
This is a bit of a moot point because:
- It's not possible to return
_originalTrajonce we change the return value to anunique_ptr. - It's safe to return
returnTrajeven ifshortcutPathreturnedfalse.
src/planner/ompl/Planner.cpp
Outdated
| auto ompl_state = sspace->allocState(_originalTraj->getWaypoint(idx)); | ||
| path->append(ompl_state); | ||
| sspace->freeState(ompl_state); | ||
| } |
There was a problem hiding this comment.
This code is duplicated from planOMPL. Please move it to a helper function.
src/planner/ompl/Planner.cpp
Outdated
| path->getState(idx)); | ||
| // Arbitrary timing | ||
| returnTraj->addWaypoint(idx, st->mState); | ||
| } |
There was a problem hiding this comment.
This code should also be moved to a helper function. See my comment above about what it could be named and where it could live.
There was a problem hiding this comment.
This comment has been addressed.
|
Note: Only the termination condition addressed. Other changes shall be addressed in the next commit! |
There was a problem hiding this comment.
This generally looks great. The only requests I have are:
- Refactor the the code that converts between
ompl::PathGeometricandaikido::path::Interpolatedinto helper functions. - Write unit tests for
simplifyOMPL. - Possibly, if you're feeling generous, write tests for (1).
src/planner/ompl/Planner.cpp
Outdated
| auto ompl_state = sspace->allocState(_originalTraj->getWaypoint(idx)); | ||
| path->append(ompl_state); | ||
| sspace->freeState(ompl_state); | ||
| } |
There was a problem hiding this comment.
This code should also be moved to a helper function.
| std::unique_ptr<trajectory::Interpolated> aikido2ompl(::ompl::geometric::PathGeometric *path, | ||
| statespace::StateSpacePtr _stateSpace, | ||
| statespace::InterpolatorPtr _interpolator); | ||
| } // namespace |
There was a problem hiding this comment.
- Anonymous
namespaces should almost never be used in header files. This is a fine function that we could promote to a newplanning/ompl/Trajectory.hppfile. - Rename this function to match our other conversion functions, e.g. added in Function to convert trajectory_msgs::JointTrajectory into aikido::trajectory::Spline #147. I suggest
toInterpolatedTrajectoryfor this function andtoOmplPathGeometricfor the inverse. @jslee02 Objections? - Take the
PathGeometricargument byconst &instead of non-constpointer, unless there is a strong reason to do otherwise.
There was a problem hiding this comment.
Oops. Did not realize I left this here. Was just playing around with the helper function. Didn't want to commit that yet. Shall make the changes suggested :)
There was a problem hiding this comment.
- I left the helper functions within the file for now. Do you suggest I move them to a new
planner/ompl/Trajectory.hppfile? - When I use a constant pointer and try to static_cast it during the state conversion, it throws an error
static_cast...casts away qualifiers. I wasn't sure how to fix it. I'll take another look.
src/planner/ompl/Planner.cpp
Outdated
| // Set the parameters for termination of simplification process | ||
| ::ompl::time::point const time_before = ::ompl::time::now(); | ||
| ::ompl::time::point time_current; | ||
| ::ompl::time::duration const time_limit = ::ompl::time::seconds(_timeout); |
There was a problem hiding this comment.
Nit: I would slightly prefer to use std::chrono here, instead of the ompl::time wrapper for it.
src/planner/ompl/Planner.cpp
Outdated
| return returnTraj; | ||
| std::pair <std::unique_ptr<trajectory::Interpolated>, bool> returnPair; | ||
| returnPair = std::make_pair(std::move(returnTraj),shorten_success); | ||
| return returnPair; |
There was a problem hiding this comment.
Nit: Why not just return std::make_pair(std::move(returnTraj), shorten_success)?
src/planner/ompl/Planner.cpp
Outdated
| si->getStateSpace()); | ||
|
|
||
| // Step 2: Convert the AIKIDO trajectory to Geometric Path | ||
| ::ompl::geometric::PathGeometric *path = new ::ompl::geometric::PathGeometric(si); |
There was a problem hiding this comment.
This leaks memory. I don't think path needs to be heap allocated, so you could replace this with just:
::ompl::geometric::PathGeometric path { si };
src/planner/ompl/Planner.cpp
Outdated
| //============================================================================= | ||
| } // ns aikido | ||
| } // ns planner | ||
| } // ns ompl |
There was a problem hiding this comment.
Add a unit tests for simplifyOMPL. The obvious tests are: (1) each error condition, (2) shortcut a two-waypoint trajectory for a large number of iterations and verify that it doesn't change, and (3) shortcut a three waypoint trajectory for a large number of iterations and verify that the trajectory gets shorter.
It would also be nice to add tests for the GeometricPath conversion functions, but I feel too guilty to ask about that since I am the one who asked you to implement them. 😬
There was a problem hiding this comment.
Oh that's fine! I shall work on it!
There was a problem hiding this comment.
Added tests for trajectory conversion functions. Converted an interpolated trajectory back and forth and ensured it had not changed. That should suffice, yes?
|
ToDo on this PR:
|
|
ToDo on this PR:
|
@aditya-vk added it, but I asked to remove it because it's always positive as the type is |
|
@jslee02 good point, I missed that until @aditya-vk pointed it out. |
|
| auto s0 = stateSpace->createState(); | ||
| for(size_t idx = 0; idx < traj->getNumWaypoints(); ++idx) | ||
| { | ||
| traj->evaluate(idx, s0); |
There was a problem hiding this comment.
This should be getWaypoint, not evaluate.
There was a problem hiding this comment.
evaluate works only if unit timing, got it! Thanks!
|
|
||
| // Match every point | ||
| auto s0 = stateSpace->createState(); | ||
| for(size_t idx = 0; idx < interpolatedTraj->getDuration(); ++idx) |
There was a problem hiding this comment.
This should be getNumWaypoints, not getDuration.
| auto s0 = stateSpace->createState(); | ||
| for(size_t idx = 0; idx < interpolatedTraj->getDuration(); ++idx) | ||
| { | ||
| interpolatedTraj->evaluate(idx, s0); |
There was a problem hiding this comment.
This should be getWaypoint, not evaluate.
| auto interpolatedTraj = aikido::planner::ompl::toInterpolatedTrajectory( | ||
| omplTraj, interpolator); | ||
|
|
||
| // Match every point |
There was a problem hiding this comment.
Verify that the two trajectories have the same number of waypoints with an ASSERT_EQUAL. Otherwise, the test may SEGFAULT in the loop below.
| auto omplTraj = aikido::planner::ompl::toOMPLTrajectory( | ||
| traj, si); | ||
|
|
||
| // Match every point |
There was a problem hiding this comment.
Verify that the two trajectories have the same number of waypoints with an ASSERT_EQUAL. Otherwise, the test may SEGFAULT in the loop below.
There was a problem hiding this comment.
The first test was checking that both trajectories have same number of waypoints. If they don't have equal waypoints, the tests would fail there and never come here. So this should be okay, yes?
There was a problem hiding this comment.
Nope:
- All tests are independent. GTest will continue to run tests even after one tests fail. This is useful because it often uncovers multiple bugs at once in one Travis run.
- Tests aren't guaranteed to be ordered in any particular way. Or even that all of them are run - you can run a subset of tests by passing commandline arguments to GTest.
- The
EXPECT_macros intentionally allow testcase to continue and mark it as failed once it is done running. OnlyASSERT_macros cause the test to immediately exit.
In general, all tests should be independent and a test should never crash, only gracefully report failure. If a check is critical to avoid a SEGFAULT (e.g. a length check like this), you should use ASSERT_ instead of EXPECT_.
There was a problem hiding this comment.
I understand. Thanks! Will make the change right away!
|
All suggestions addressed. 😄 |
This introduces a way to simplify and shorten paths obtained from the planners used in AIKIDO using OMPL methods.
AIKIDO Planner -> untimed trajectory -> simplifyOMPL -> simplified and shortened untimed trajectory.
-> The feature has been currently introduced same place as the planning methods although really a post-processing step. Is there a better place for this than here, currently?