API to use OMPL Planners within the new planner framework #466

aditya-vk · 2018-10-27T23:01:07Z

This PR enables using the OMPL planners via AIKIDO for any of the ConfigurationToConfiguration problems. This should allow us to plan for exact/approximate solutions for configuration(s)/TSR problems.

There are a few TODOs in the PR, one of which would be part of another PR perhaps(?) since they are in relation to statespaces and contraints.

Before creating a pull request

Document new methods and classes
Format code with make format

Before merging a pull request

Set version target by selecting a milestone on the right side
Summarize this change in CHANGELOG.md
Add unit test(s) for this change

codecov · 2018-10-28T04:29:42Z

Codecov Report

Merging #466 into master will decrease coverage by 0.02%.
The diff coverage is 84.49%.

@@            Coverage Diff             @@
##           master     #466      +/-   ##
==========================================
- Coverage   77.61%   77.58%   -0.03%     
==========================================
  Files         260      262       +2     
  Lines        6588     6669      +81     
==========================================
+ Hits         5113     5174      +61     
- Misses       1475     1495      +20

Impacted Files	Coverage Δ
...nclude/aikido/planner/ompl/GeometricStateSpace.hpp	`100% <ø> (ø)`	⬆️
src/planner/ompl/Planner.cpp	`88.82% <100%> (+0.06%)`	⬆️
src/planner/ompl/GeometricStateSpace.cpp	`100% <100%> (ø)`	⬆️
...r/ompl/OMPLConfigurationToConfigurationPlanner.hpp	`100% <100%> (ø)`
...l/OMPLConfigurationToConfigurationPlanner-impl.hpp	`71.42% <71.42%> (ø)`

jslee02 · 2018-10-28T04:08:10Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+public:
+  /// Constructor.
+  ///
+  /// \param[in] planner The OMPL planner to use.


This planner is not in the constructor arguments. I guess you wanted to describe the template type of this class. Let's move this to the class docstring something like:

/// \tparam PlannerType The OMPL planner to use.

I think this line should be deleted now?

jslee02 · 2018-11-05T04:29:11Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+#include <ompl/base/ScopedState.h>
+#include <ompl/base/SpaceInformation.h>
+#include <ompl/base/goals/GoalRegion.h>
+#include <ompl/geometric/PathSimplifier.h>


Nit: Third party headers should come first and then the project headers (see this)

jslee02 · 2018-11-05T04:32:28Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+
+#include "aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp"
+
+#endif // AIKIDO_PLANNER_SNAPCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_


Suggested change

#endif // AIKIDO_PLANNER_SNAPCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

#endif // AIKIDO_PLANNER_OMPL_OMPLCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

jslee02 · 2018-11-05T04:35:37Z

src/robot/util.cpp

+  //      createTestableBounds(space),
+  //      createProjectableBounds(space),
+  //      timelimit,
+  //      collisionResolution);


Nit: Let's remove old code

jslee02 · 2018-11-05T04:39:06Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+      std::move(dmetric),
+      std::move(sampler),
+      mBoundsConstraint,
+      std::move(mBoundsProjector));


We don't need to have mBoundsProjector as a member variable because it's been moved. It seems we could convert this as a local variable in this constructor implementation.

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

src/robot/util.cpp

jslee02

Looks good to me!

brianhou

Thanks for the PR! I have one major question. The OMPLPlanner constructor takes an interpolator, dmetric, sampler, boundsConstraint, boundsProjector. However, it seems that we're inconsistent with which of those are stored as fields in the OMPLPlanner class, versus which of those are only used as local variables in the constructor to create the OMPL Planner.

My understanding is that this is because we need mInterpolator (to reconstruct an AIKIDO Interpolated) and mBoundsConstraint (to create the TestableIntersection with the problem's constraint). However, it seems that one way to resolve this is to modify GeometricStateSpace and add getters for all of the above. That way, we don't need any of them stored as fields: we can just get them directly from the state information. What do you think?

brianhou · 2018-11-05T18:09:04Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+public:
+  /// Constructor.
+  ///
+  /// \param[in] planner The OMPL planner to use.


I think this line should be deleted now?

brianhou · 2018-11-05T18:11:26Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+  /// Plans a trajectory from start state to goal state by using an interpolator
+  /// to interpolate between them.
+  ///
+  /// The planner returns success if the resulting trajectory satisfies


I don't think this is accurate: this function always returns the trajectory, which is null if planning fails.

brianhou · 2018-11-05T18:12:50Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+  ::ompl::base::PlannerPtr getOMPLPlanner();
+
+  /// Sets interpolator used to produce the output trajectory.
+  void setInterpolator(statespace::ConstInterpolatorPtr interpolator);


Nit: missing documentation for interpolator.

Is this function being used? What's the use case for wanting to change the interpolator after construction, without changing the other fields?

brianhou · 2018-11-05T18:14:32Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

+  statespace::ConstInterpolatorPtr mInterpolator;
+
+private:
+  // TODO (avk): Should we make the problem take std::vector<TestablePtr>?


Why would we want a vector of Testables?

I was also a little uncomfortable with needing to have an mBoundsConstraint to reconstruct the contraint during planning and was wondering if we could have a vector of testables passed in the problem instead of a single testable.

brianhou · 2018-11-05T18:15:54Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+#define AIKIDO_PLANNER_OMPL_DETAIL_OMPLCONFIGURATIONTOCONFIGURATION_IMPL_HPP_
+
+#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp"
+


I think we try to avoid empty lines in the includes?

brianhou · 2018-11-05T18:25:03Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+  const auto metaskeletonStatespace
+      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(mStateSpace);
+
+  if (!metaskeletonStatespace


I found this logic somewhat confusing because it wasn't clear to me when all of the conditions would be satisfied. I think this might be clearer:

if (metaSkeletonStateSpace) { if (!sampler) ... if (!mBoundsConstraint) ... if (!boundsProjector) ... } else { if (!sampler || !boundsConstraint || !boundsProjector) { throw ... } } auto sspace = ...

brianhou · 2018-11-05T18:25:48Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+  // Geometric State space
+  auto sspace = ompl_make_shared<GeometricStateSpace>(
+      mStateSpace,
+      std::make_shared<aikido::statespace::GeodesicInterpolator>(mStateSpace),


Shouldn't this use mInterpolator? (Again, still not clear why that's a field and not just a local variable.)

brianhou · 2018-11-05T18:27:31Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+  // Plan
+  mPlanner->setProblemDefinition(pdef);
+  mPlanner->setup();
+  auto solved = mPlanner->solve(1);


Shouldn't 1 be timelimit or something?

brianhou · 2018-11-05T18:29:31Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+    {
+      // Only states belonging geometric statespaces are supported.
+      assert(
+          dynamic_cast<aikido::planner::ompl::GeometricStateSpace::StateType*>(


Is it possible for a PathGeometric to have non-GeometricStateSpace::StateType? If so, is it possible for a PathGeometric to have a mixture of StateTypes (i.e. does this dynamic_cast need to happen every time)?

brianhou · 2018-11-05T18:32:31Z

tests/planner/ompl/test_OMPLConfigurationToConfigurationPlanner.cpp

+          std::move(boundsProjection),
+          0.1);
+  auto traj = planner->plan(problem);
+  EXPECT_TRUE(true);


aditya-vk · 2018-11-05T19:25:18Z

Really good idea about modifying GeometricStateSpace 👍

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

brianhou

Almost there! I have a few minor nits we should address, but I think we can merge once those are in. Thanks for the great changes!

include/aikido/planner/ompl/GeometricStateSpace.hpp

brianhou · 2018-11-29T21:16:10Z

include/aikido/planner/ompl/GeometricStateSpace.hpp

+  aikido::statespace::ConstInterpolatorPtr getInterpolator() const;
+
+  /// Return the bounds constraint for the statespace. Used to specify
+  /// constraints


This is wrapped weirdly.

brianhou · 2018-11-29T21:16:57Z

include/aikido/planner/ompl/GeometricStateSpace.hpp

+  /// Return the bounds constraint for the statespace. Used to specify
+  /// constraints
+  /// to the OMPL planner.
+  aikido::constraint::TestablePtr getBoundsConstraint() const;


Is there a reason for this not to be ConstTestablePtr to match the statespace and interpolator?

brianhou · 2018-11-29T21:17:51Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

  /// \param[in] stateSpace State space that this planner associated with.
  /// \param[in] rng Random number generator to create the state sampler.
  /// \param[in] interpolator Interpolator used to interpolate between two
  /// states. GeodesicInterpolator is used by default.
  /// \param[in] dmetric A valid distance metric defined on the StateSpace.
  /// Distance metric relevant to the statespace is used by default.
-  /// \param[in] sampler A Sampleable that can sample states from the
-  /// StateSpace. Warning: Many OMPL planners internally assume this sampler


Why did we remove this warning? I think it'd be a good idea to keep (and perhaps denote with a \note to make it show up nicely in the docs).

brianhou · 2018-11-29T21:20:24Z

include/aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp

-  /// otherwise. The reason for the failure is stored in the \c result output
-  /// parameter.
+  /// If successful, the planner returns a valid trajectory. If not, it
+  /// returns a \c nullptr. The corresponding message is stored in result.


I think a combination of the old and new versions is the most descriptive/accurate. How about something like:

If successful, the planner returns a trajectory that satisfies the constraint. If not, it returns a \c nullptr and the reason for the failure is stored in the \c result.

brianhou · 2018-11-29T21:24:38Z

src/planner/ompl/GeometricStateSpace.cpp

+double GeometricStateSpace::getMaxDistanceBetweenValidityChecks() const
+{
+  return mMaxDistanceBetweenValidityChecks;
+}
 }


I think we usually have a newline before the closing braces for the namespaces. Also, // namespace XXX?

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

jslee02

Looks great to me. Just one nitpick.

jslee02 · 2018-11-30T01:20:56Z

include/aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp

+    for (std::size_t idx = 0; idx < path->getStateCount(); ++idx)
+    {
+      const auto* st
+          = static_cast<aikido::planner::ompl::GeometricStateSpace::StateType*>(


Nit: Let's add an assertion for checking the type using dynamic_cast.

Done. For now, I don't much of a preference on this, since we are only working with geometric planners/statespaces. But I guess when we later extend our support, we might be dealing with other state types.

Thanks!

* dirty init for OMPL + AIKIDO under new API * new ompl api header * ompl planner implementation * use the new ompl api and test * update ompl planner implementation; bugfix for replanning * test planner specific params * pass rng * clean up tests, clear() after solving problem * remove vscode files * make format * address JS's comments * address minor comments from brian * testing the new feature Co-Authored-By: aditya-vk <adityavk711@gmail.com> * WIP: Added a bunch of TODOs to address * add getter methods * use the member varaibles from statespace * follow the new geometric statespace constructor * use new API * add new param to constructors in tests * cleanup and add doscstrings * unsigned int vs size_t * cleanup of geometric statespace * add some tests in geomteric statespace * correct for changes in geomteric statespace class * termination condition changed * address travis error * Format code * typo fix Co-Authored-By: aditya-vk <adityavk711@gmail.com> * typo fix Co-Authored-By: aditya-vk <adityavk711@gmail.com> * address comments * style change * update changelog * added an assertion before the static cast

aditya-vk added 11 commits July 9, 2018 19:47

dirty init for OMPL + AIKIDO under new API

3857d2e

merge master into branch

ceca44d

new ompl api header

a5d3895

ompl planner implementation

715e0d1

use the new ompl api and test

9f3281b

update ompl planner implementation; bugfix for replanning

5ff904c

test planner specific params

2412b95

pass rng

f69ee95

clean up tests, clear() after solving problem

2f7cdd3

remove vscode files

6ec8489

make format

ddd2dd6

aditya-vk added this to the Aikido 0.3.0 milestone Oct 27, 2018

aditya-vk requested review from jslee02, brianhou, gilwoolee and sanjibanc October 27, 2018 23:01

jslee02 reviewed Nov 5, 2018

View reviewed changes

address JS's comments

78d93da

jslee02 previously approved these changes Nov 5, 2018

View reviewed changes

brianhou suggested changes Nov 5, 2018

View reviewed changes

aditya-vk dismissed jslee02’s stale review via 41c9f59 November 5, 2018 19:23

address minor comments from brian

41c9f59

brianhou and others added 6 commits November 5, 2018 11:27

testing the new feature

8ce0ce6

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

WIP: Added a bunch of TODOs to address

06c18f9

add getter methods

8b57e02

use the member varaibles from statespace

a4d3367

follow the new geometric statespace constructor

19d1782

use new API

0eea5c9

aditya-vk and others added 10 commits November 5, 2018 19:02

add new param to constructors in tests

b5d8062

Merge branch 'master' into ompl/update

c851a33

cleanup and add doscstrings

d43bb9f

unsigned int vs size_t

d7d59e6

cleanup of geometric statespace

444b413

add some tests in geomteric statespace

dbc2d44

correct for changes in geomteric statespace class

48127ac

termination condition changed

0c0de47

address travis error

916cc76

Format code

f21df91

brianhou suggested changes Nov 29, 2018

View reviewed changes

brianhou and others added 3 commits November 29, 2018 14:13

typo fix

a27a0c4

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

typo fix

fedeace

Co-Authored-By: aditya-vk <adityavk711@gmail.com>

address comments

7897f80

brianhou previously approved these changes Nov 29, 2018

View reviewed changes

aditya-vk added 2 commits November 29, 2018 16:24

style change

a687b9b

update changelog

fafd58c

aditya-vk dismissed brianhou’s stale review via fafd58c November 30, 2018 00:26

jslee02 suggested changes Nov 30, 2018

View reviewed changes

jslee02 reviewed Nov 30, 2018

View reviewed changes

added an assertion before the static cast

56e9281

jslee02 approved these changes Nov 30, 2018

View reviewed changes

aditya-vk merged commit f136924 into master Nov 30, 2018

aditya-vk deleted the ompl/update branch November 30, 2018 04:28

jslee02 mentioned this pull request Nov 30, 2018

Increase minimum required ompl version greater than 0.13.0 #471

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

API to use OMPL Planners within the new planner framework #466

API to use OMPL Planners within the new planner framework #466

aditya-vk commented Oct 27, 2018

codecov bot commented Oct 28, 2018 •

edited

Loading

jslee02 Oct 28, 2018

brianhou Nov 5, 2018

jslee02 Nov 5, 2018

jslee02 Nov 5, 2018

jslee02 Nov 5, 2018

jslee02 Nov 5, 2018

jslee02 left a comment

brianhou left a comment

brianhou Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

aditya-vk Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

aditya-vk Nov 6, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

brianhou Nov 5, 2018

aditya-vk commented Nov 5, 2018

brianhou left a comment

brianhou Nov 29, 2018

brianhou Nov 29, 2018

brianhou Nov 29, 2018

brianhou Nov 29, 2018

brianhou Nov 29, 2018

jslee02 left a comment

jslee02 Nov 30, 2018

aditya-vk Nov 30, 2018


		#include "aikido/planner/ompl/detail/OMPLConfigurationToConfigurationPlanner-impl.hpp"

		#endif // AIKIDO_PLANNER_SNAPCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

	#endif // AIKIDO_PLANNER_SNAPCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
	#endif // AIKIDO_PLANNER_OMPL_OMPLCONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

		#define AIKIDO_PLANNER_OMPL_DETAIL_OMPLCONFIGURATIONTOCONFIGURATION_IMPL_HPP_

		#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp"

API to use OMPL Planners within the new planner framework #466

API to use OMPL Planners within the new planner framework #466

Conversation

aditya-vk commented Oct 27, 2018

codecov bot commented Oct 28, 2018 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

brianhou left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

aditya-vk commented Nov 5, 2018

brianhou left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Oct 28, 2018 •

edited

Loading