Planning adapters #437

brianhou · 2018-05-24T05:47:25Z

Re-implementation of #425.

Main changes:

This PR adds a MetaSkeleton argument to the constructor of all "DART planners" in planner::dart. All DART planners now also define getMetaSkeletonStateSpace and getMetaSkeleton to make adapters possible. These methods are currently re-implemented in each planner, but they could (should?) be moved into a dart::SingleProblemPlanner class that all DART planners inherit from.
This PR introduces the dart::DartPlannerAdapter class for converting planners that don't contain a MetaSkeleton (i.e. SnapConfigurationToConfigurationPlanner) into their corresponding DART planner (in this case, dart::ConfigurationToConfigurationPlanner). This correspondence isn't enforced anywhere, but maybe some fancy template code could do so?
The dart::PlannerAdapter class is essentially the same single-problem adapter from [WIP] Planning adapters #425, but only operates on DART planners that contain a MetaSkeleton. We can't adapt a planner that doesn't contain a MetaSkeleton into one that should.
Preliminary implementation of the adapter from ConfigurationToConfiguration to ConfigurationToTSR. It does a few const_casts, which we should look hard at to make sure that they're absolutely necessary.

The pipeline to use a SnapConfigurationToConfigurationPlanner as a dart::ConfigurationToTSRPlanner now takes an extra step:

auto snapDelegate = std::make_shared<SnapConfigurationToConfigurationPlanner>(metaSkeletonStateSpace, interpolator);
auto delegate = std::make_shared<DartPlannerAdapter<SnapConfigurationToConfigurationPlanner, dart::ConfigurationToConfigurationPlanner>>(snapDelegate, metaSkeleton);
auto planner = std::make_shared<ConfigurationToConfiguration_to_ConfigurationToTSR>(delegate);

The new intermediate step is necessary to adapt the non-DART SnapConfigurationToConfigurationPlanner into a DART ConfigurationToConfigurationPlanner.

Issues raised during this PR:

Should all Planners have an RNG? This is needed to create a bounds sampler for the IKSampleable, so it might be convenient to include this with all Planners in case an adapter needs it.

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

Add ConfigurationToTSRPlanner. Add DART version of ConfigurationToConfigurationPlanner.

…rationToTSR adapter.

codecov · 2018-05-24T07:08:04Z

Codecov Report

Merging #437 into master will decrease coverage by 0.33%.
The diff coverage is 38.88%.

@@            Coverage Diff            @@
##           master    #437      +/-   ##
=========================================
- Coverage   80.03%   79.7%   -0.34%     
=========================================
  Files         235     243       +8     
  Lines        6002    6050      +48     
=========================================
+ Hits         4804    4822      +18     
- Misses       1198    1228      +30

Impacted Files	Coverage Δ
...do/constraint/dart/InverseKinematicsSampleable.hpp	`100% <ø> (ø)`	⬆️
...do/planner/dart/ConfigurationToEndEffectorPose.hpp	`0% <ø> (ø)`	⬆️
...lanner/SnapConfigurationToConfigurationPlanner.hpp	`100% <ø> (ø)`	⬆️
...do/planner/ConfigurationToConfigurationPlanner.hpp	`100% <ø> (ø)`	⬆️
...rc/constraint/dart/InverseKinematicsSampleable.cpp	`84.94% <ø> (ø)`	⬆️
include/aikido/planner/dart/ConfigurationToTSR.hpp	`0% <ø> (ø)`	⬆️
...orFieldConfigurationToEndEffectorOffsetPlanner.hpp	`100% <ø> (ø)`	⬆️
...r/dart/ConfigurationToEndEffectorOffsetPlanner.hpp	`100% <ø> (ø)`	⬆️
include/aikido/planner/SequenceMetaPlanner.hpp	`0% <ø> (ø)`	⬆️
...rc/planner/dart/ConfigurationToEndEffectorPose.cpp	`0% <ø> (ø)`	⬆️
... and 20 more

sniyaz

This looks good! My main feedback is that it would be nice to have a base class for all of the DART planners, since we copy getMetaSkeletonStateSpace() and getMetaSkeleton() a lot. We could save this for a future PR too, depending on what others think.

We also don't enforce the adaptors at all- I don't think anything prevents you from using the wrong adaptor (i.e. PlannerAdaptor on a planner that doesn't contain a MetaSkeleton). Maybe we could add a flag for whether something is a DART planner to enforce this? This should probably be in a future PR though.

sniyaz · 2018-05-24T21:43:02Z

include/aikido/planner/dart/DartPlannerAdapter.hpp

+namespace planner {
+namespace dart {
+
+/// Adapts a non-DART DelegatePlanner to solve the single problem that


I think the name of this class should be a little more clear about what it does- maybe ConvertToDartPlannerAdapter?

I have also the similar impression, but couldn't come up with a better name. 😓

Planner_to_DartPlanner? PlannerAdapter could be called DartPlanner_to_DartPlanner if you think that's better?

sniyaz · 2018-05-24T21:59:06Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

+  while (generator->canSample())
+  {
+    // Sample from TSR
+    {


Out of curiosity, why is this in braces?

Adding the braces creates a temporary scope, so all of the local variables in that scope will be destructed when it ends. A std::lock_guard releases the mutex when it's destructed.

jslee02 · 2018-05-25T18:31:41Z

All DART planners now also define getMetaSkeletonStateSpace and getMetaSkeleton to make adapters possible. These methods are currently re-implemented in each planner, but they could (should?) be moved into a dart::SingleProblemPlanner class that all DART planners inherit from.

👍

This correspondence isn't enforced anywhere, but maybe some fancy template code could do so?

This sounds good to me. We could save this for furture though. In that case, it'd be good to create an issue to track this.

jslee02

Overall looks good to me. One high level comment and minor comments that are made in the code.

I think we need dart::ConfigurationToConfiguration to distinguish it from non-DART config-to-config problem so that dart::ConfigurationToConfigurationPlanner's solvable problem type becomes dart::ConfigurationToConfiguration but not non-DART config-to-config.

jslee02 · 2018-05-25T18:53:21Z

include/aikido/planner/dart/ConfigurationToTSR.hpp

      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
-      const statespace::StateSpace::State* startState,
+      std::size_t maxSamples,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,


Nit: Grouping the goal related arguments would make sense so that the argument order becomes startState -> maxSamples -> goalTSR or startState -> goalTSR -> maxSamples.

jslee02 · 2018-05-25T18:54:55Z

include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp

+  // Note: SolvableProblem is defined in SingleProblemPlanner.
+
+  /// Return this planner's MetaSkeletonStateSpace.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace();


Nit: This function should be const since it returns a pointer to a const object.

jslee02 · 2018-05-25T20:48:19Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

+    throw std::invalid_argument("MetaSkeleton has 0 degrees of freedom.");
+
+  auto skeleton = mMetaSkeleton->getDof(0)->getSkeleton();
+  for (size_t i = 1; i < mMetaSkeleton->getNumDofs(); ++i)


Nit: std::size_t

jslee02 · 2018-05-25T20:51:31Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

+  for (std::size_t i = 0; i < mMetaSkeleton->getNumBodyNodes(); ++i)
+  {
+    if (mMetaSkeleton->getBodyNode(i)->getName()
+        == problem.getEndEffectorBodyNode()->getName())


I think the equality check should be done over the pointer instead of their names. A MetaSkeleton could have multiple body nodes of the same name.

My thought was that BodyNode that the ConfigurationToTSR problem was constructed with could be from a different Skeleton than the one that the delegate planner's MetaSkeleton refers to. This might happen if we try multiple planners in parallel and therefore have several Skeleton copies that are being used. In this case, I think the BodyNode pointers would not be the same, but the names would be.

That makes sense.

MetaSkeleton has a function that returns all the BodyNodes that has the queried name. How about we use that function and prints a warning if it's more than one?

jslee02 · 2018-05-25T20:53:53Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

+  InverseKinematicsSampleable ikSampleable(
+      mMetaSkeletonStateSpace,
+      mMetaSkeleton,
+      std::const_pointer_cast<TSR>(problem.getGoalTSR()),


Nit: Could you make a note here saying this const-casting should be removed once InverseKinematicsSampleable is changed to take const constraints?

jslee02 · 2018-05-25T20:57:47Z

include/aikido/planner/dart/detail/DartPlannerAdapter-impl.hpp

@@ -0,0 +1,35 @@
+#include "aikido/planner/dart/DartPlannerAdapter.hpp"
+
+using aikido::statespace::dart::MetaSkeletonStateSpace;


Technically, -impl.hpp is also a header file so using using namespace ~ can pollute the namespaces. Please remove this.

…anner.

brianhou · 2018-05-25T23:24:59Z

include/aikido/planner/dart/ConfigurationToConfigurationPlanner.hpp

@@ -2,7 +2,7 @@
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

 #include "aikido/planner/ConfigurationToConfiguration.hpp"
-#include "aikido/planner/SingleProblemPlanner.hpp"
+#include "aikido/planner/dart/DartSingleProblemPlanner.hpp"


Can we rename dart::DartSingleProblemPlanner to dart::SingleProblemPlanner? We already have the dart namespace, so I think repeating it is unnecessary 🙂

[skip ci]

Remove DartPlannerAdapter class in favor of supporting conversion from non-DART planners to DART planners in PlannerAdapter class.

…github.com/personalrobotics/aikido into enhancement/brianhou/metaskeleton-planners Conflicts: include/aikido/planner/dart/detail/DartPlannerAdapter-impl.hpp

jslee02

Looks good to me! Just one nitpick.

jslee02 · 2018-05-29T23:24:25Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

+  // Create an IK solver with MetaSkeleton DOFs
+  auto matchingNodes = mMetaSkeleton->getBodyNodes(
+      problem.getEndEffectorBodyNode()->getName());
+  if (matchingNodes.size() == 0)


Nit: Checking emptiness is generally faster or equal to comparing the size. So let's change this to if (!matchingNodes.empty()).

* Add MetaSkeleton to ConfigurationToEndEffectorOffsetPlanner. * Add more DART planners. Add ConfigurationToTSRPlanner. Add DART version of ConfigurationToConfigurationPlanner. * Add max samples to ConfigurationToTSR problem. * Implement single-problem PlannerAdapter class. * Implement single-problem DartPlannerAdapter class. * Add initial implementation of ConfigurationToConfiguration_to_ConfigurationToTSR adapter. * Format code. * Update ConfigurationToEndEffectorPose to MetaSkeletonStateSpace. * Write DartSingleProblemPlanner. Change ConfigurationToConfigurationPlanner. * Update the other two DART planners to use DartSingleProblemPlanner. * Rename dart::DartSingleProblemPlanner to dart::SingleProblemPlanner. * Fix formatting. [skip ci] * Update PlannerAdapter. Remove DartPlannerAdapter class in favor of supporting conversion from non-DART planners to DART planners in PlannerAdapter class. * Respond to nits from JS. * Make SequenceMetaPlanner use CompositePlanner constructor. * Respond to final nit from JS. * Update CHANGELOG * Add that line back in CHANGELOG.md

brianhou added 7 commits May 23, 2018 17:46

Add MetaSkeleton to ConfigurationToEndEffectorOffsetPlanner.

045af5b

Add more DART planners.

db39e95

Add ConfigurationToTSRPlanner. Add DART version of ConfigurationToConfigurationPlanner.

Add max samples to ConfigurationToTSR problem.

56bf0aa

Implement single-problem PlannerAdapter class.

0af77cf

Implement single-problem DartPlannerAdapter class.

8ef4840

Add initial implementation of ConfigurationToConfiguration_to_Configu…

f7138f6

…rationToTSR adapter.

Format code.

949d523

brianhou added help wanted pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. labels May 24, 2018

brianhou added this to the Aikido 0.3.0 milestone May 24, 2018

brianhou requested review from jslee02 and sniyaz May 24, 2018 05:47

brianhou mentioned this pull request May 24, 2018

[WIP] Planning adapters #425

Closed

5 tasks

Update ConfigurationToEndEffectorPose to MetaSkeletonStateSpace.

d84ad5f

sniyaz reviewed May 24, 2018

View reviewed changes

jslee02 suggested changes May 25, 2018

View reviewed changes

evil-sherdil added 2 commits May 25, 2018 19:00

Write DartSingleProblemPlanner. Change ConfigurationToConfigurationPl…

4078529

…anner.

Update the other two DART planners to use DartSingleProblemPlanner.

ac6bcac

brianhou commented May 25, 2018

View reviewed changes

brianhou and others added 6 commits May 25, 2018 16:58

Rename dart::DartSingleProblemPlanner to dart::SingleProblemPlanner.

eb61f17

Fix formatting.

a51bb9e

[skip ci]

Update PlannerAdapter.

fba0860

Remove DartPlannerAdapter class in favor of supporting conversion from non-DART planners to DART planners in PlannerAdapter class.

Respond to nits from JS.

5b61b44

Merge branch 'enhancement/brianhou/metaskeleton-planners' of https://…

aa5f107

…github.com/personalrobotics/aikido into enhancement/brianhou/metaskeleton-planners Conflicts: include/aikido/planner/dart/detail/DartPlannerAdapter-impl.hpp

Make SequenceMetaPlanner use CompositePlanner constructor.

3bbe3f8

jslee02 previously approved these changes May 29, 2018

View reviewed changes

Respond to final nit from JS.

883ce73

sniyaz dismissed jslee02’s stale review via 883ce73 May 30, 2018 22:06

jslee02 previously approved these changes May 30, 2018

View reviewed changes

Update CHANGELOG

6d31683

sniyaz dismissed jslee02’s stale review via 6d31683 May 30, 2018 23:46

Add that line back in CHANGELOG.md

74897e2

sniyaz merged commit f7ebc5b into master May 30, 2018

sniyaz deleted the enhancement/brianhou/metaskeleton-planners branch May 30, 2018 23:50

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Planning adapters #437

Planning adapters #437

brianhou commented May 24, 2018 •

edited by sniyaz

Loading

codecov bot commented May 24, 2018 •

edited

Loading

sniyaz left a comment

sniyaz May 24, 2018

jslee02 May 25, 2018

brianhou May 25, 2018 •

edited

Loading

sniyaz May 24, 2018

brianhou May 24, 2018

jslee02 commented May 25, 2018

jslee02 left a comment

jslee02 May 25, 2018

jslee02 May 25, 2018

jslee02 May 25, 2018

jslee02 May 25, 2018

brianhou May 25, 2018

jslee02 May 25, 2018

jslee02 May 25, 2018

jslee02 May 25, 2018

brianhou May 25, 2018

jslee02 left a comment

jslee02 May 29, 2018

		@@ -0,0 +1,35 @@
		#include "aikido/planner/dart/DartPlannerAdapter.hpp"

		using aikido::statespace::dart::MetaSkeletonStateSpace;

Planning adapters #437

Planning adapters #437

Conversation

brianhou commented May 24, 2018 • edited by sniyaz Loading

codecov bot commented May 24, 2018 • edited Loading

Codecov Report

sniyaz left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brianhou May 25, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jslee02 commented May 25, 2018

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

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

jslee02 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brianhou commented May 24, 2018 •

edited by sniyaz

Loading

codecov bot commented May 24, 2018 •

edited

Loading

brianhou May 25, 2018 •

edited

Loading