Refactor VFP with new Planner API #426

sniyaz · 2018-05-17T00:21:20Z

This PR has changes that use the new Planner API to refactor VFP.

Right now I've only refactored planToEndEffectorOffset by creating the new VectorFieldConfigurationToEndEffectorOffsetPlanner class. While these changes are looked over, I'll refactor planToEndEffectorPose as well.

Summary of changes

Create new ConfigurationToEndEffectorOffsetPlanner class for the ConfigurationToEndEffectorOffset Problem.
Create VectorFieldConfigurationToEndEffectorOffsetPlanner class that extends the above and calls the old planToEndEffectorOffset method in VectorFieldPlanner.

TODOs

We need to figure out to handle the startState provided by ConfigurationToEndEffectorOffset. The old VFP code gets the current configuration from the metaSkeleton, so right now I just ignore this instance variable.
The old VFP code took minDistance and maxDistance for planToEndEffectorOffset. I provide these by taking a tolerance around the signedDistance provided by ConfigurationToEndEffectorOffset, but we should discuss whether this is appropriate (I also need to handle the negative case).

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

sniyaz · 2018-05-18T01:41:36Z

This is blocking on #429 right now. Once that gets merged, I'll rebase the actual Planner API changes from this PR on top of that one, and we should be good! 😄

…Planner.hpp

…y won't build.

dqyi11 · 2018-05-18T23:41:13Z

src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp

+  auto metaskeletonStateSpace
+      = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(mStateSpace);
+  if (!metaskeletonStateSpace)
+    throw std::invalid_argument("mStateSpace is not MetaSkeletonStateSpace!");


It is better to check this in the constructor

For sure! My thinking is that this would require two casts: one in the constructor to check, and one when it's actually used. Are we OK with this?

NVM, this is moot with the discussion below since the type is enforced by the constructor argument now.

dqyi11 · 2018-05-18T23:47:01Z

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

+  /// \param[out] result Information about success or failure.
+  /// \return Trajectory or \c nullptr if planning failed.
+  /// \throw If \c problem is not ConfigurationToEndEffectorOffset.
+  /// \throw If \c result is not ConfigurationToEndEffectorOffset::Result.


It seems that these two validations are not yet added in the implementation.

jslee02 · 2018-05-18T23:44:39Z

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

+
+#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp"
+#include "aikido/planner/vectorfield/VectorFieldPlanner.hpp"


Nit: It's not necessary to include this header here. Please move to the source.

jslee02 · 2018-05-18T23:50:55Z

include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp

+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  explicit ConfigurationToEndEffectorOffsetPlanner(
+      statespace::ConstStateSpacePtr stateSpace);


This planner only works for MetaSkeletonStateSpace. Please change this class to:

take ConstMetaSkeletonStateSpace in the constructor

have a getter getMetaSkeletonStateSpace() that returns the ConstStatespacePtr by static-casting it to ConstMetaSkeletonStateSpacePtr.
to prevent passing incorrect state space in compile time.

jslee02 · 2018-05-18T23:52:47Z

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

+  /// checking.
+  /// \param[in] timelimit timeout in seconds.
+  VectorFieldConfigurationToEndEffectorOffsetPlanner(
+      statespace::ConstStateSpacePtr stateSpace,


Please change to take ConstMetaSkeletonStateSpacePtr for the same reason above.

jslee02 · 2018-05-18T23:54:40Z

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

+      double initialStepSize,
+      double jointLimitTolerance,
+      double constraintCheckResolution,
+      std::chrono::duration<double> timelimit);


Parameters that are not planner specific parameters should move to the problem class. @dqyi11 Could you verify all the parameters are planner specific?

I agree with the current scope of parameters defined in the problem class, which is consistent with the design in prpy. https://github.com/personalrobotics/prpy/blob/master/tests/planning/methods/PlanToEndEffectorOffset.py
All these parameters are planner-specific. Though some of them are used in both vector field planner and CRRT planner, they can be set very differently (for example constraintCheckResolution). distanceTolerance, positionTolerance and angularTolerance are also not strictly aligned in planning end-effector offset between using vector field planner and CRRT planner.

codecov · 2018-05-21T21:54:59Z

Codecov Report

Merging #426 into master will increase coverage by 0.27%.
The diff coverage is 76.92%.

@@            Coverage Diff             @@
##           master     #426      +/-   ##
==========================================
+ Coverage   79.76%   80.03%   +0.27%     
==========================================
  Files         231      235       +4     
  Lines        5986     6002      +16     
==========================================
+ Hits         4775     4804      +29     
+ Misses       1211     1198      -13

Impacted Files	Coverage Δ
...ikido/planner/ConfigurationToEndEffectorOffset.hpp	`100% <ø> (+100%)`	⬆️
src/planner/ConfigurationToEndEffectorOffset.cpp	`73.68% <ø> (+73.68%)`	⬆️
...lanner/ConfigurationToEndEffectorOffsetPlanner.cpp	`100% <100%> (ø)`
...lanner/ConfigurationToEndEffectorOffsetPlanner.hpp	`100% <100%> (ø)`
src/planner/vectorfield/VectorFieldPlanner.cpp	`78.18% <100%> (+3.18%)`	⬆️
...orFieldConfigurationToEndEffectorOffsetPlanner.hpp	`100% <100%> (ø)`
...orFieldConfigurationToEndEffectorOffsetPlanner.cpp	`68.42% <68.42%> (ø)`
... and 3 more

sniyaz · 2018-05-21T21:59:45Z

I think the start state should be the last thing to figure out now.

@jslee02 I had to use a dynamic_pointer_cast to go from ConstStatespacePtr ---> ConstMetaSkeletonStateSpacePtr. Apparently you can't use static_cast to downcast when virtual inheritance is involved? There's definitely a chance I'm wrong on this, so LMK.

dqyi11 · 2018-05-21T22:08:26Z

src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp

+      problem.getConstraint(),
+      problem.getDirection(),
+      // TODO: Need to handle case when negative.
+      problem.getDistance() - mDistanceTolerance,


It is better to check whether that problem.getDistance() - mDistanceTolerance and problem.getDistance() + mDistanceTolerance have the same sign...

…teSpace instance variable to prvent expensive dynamic casting.

sniyaz · 2018-05-22T01:12:41Z

I've handled the startState issue by setting the metaSkeleton to the startState given in the ConfigurationToEndEffectorOffset problem class. This is done by changing the old VFP code to take a startState and setting it in that function.

I've also made the stateSpace and startState arguments to that class constructor more restrictive, since ConfigurationToEndEffectorOffset problems can only be used with MetaSkeletonStateSpace.

jslee02

🎉

sniyaz requested review from dqyi11, jslee02 and brianhou May 17, 2018 00:21

jslee02 mentioned this pull request May 17, 2018

Task list for migrating to new planning API #413

Open

27 tasks

sniyaz mentioned this pull request May 18, 2018

Const changes to support new VFP Planner API refactor. #429

Merged

5 tasks

brianhou changed the base branch from master to VFP_const May 18, 2018 02:35

brianhou changed the base branch from VFP_const to master May 18, 2018 02:35

evil-sherdil added 6 commits May 18, 2018 19:05

Draft ConfigurationToEndEffectorOffsetPlanner.hpp

b2d8175

Draft ConfigurationToEndEffectorOffsetPlanner.cpp

16256d3

Draft VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

6aff8fa

Add instance variables to VectorFieldConfigurationToEndEffectorOffset…

0c73e25

…Planner.hpp

Draft VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp. Probabl…

ebffe9a

…y won't build.

Change these changes building, but there are some type mismatches.

34b843f

sniyaz force-pushed the VFP_refactor branch from e31bd9e to 34b843f Compare May 18, 2018 23:08

Make rebased changes build, run make format.

57f7db0

dqyi11 reviewed May 18, 2018

View reviewed changes

jslee02 reviewed May 18, 2018

View reviewed changes

Update VFP test for offset refactor.

9241491

sniyaz changed the title ~~[WIP] Refactor VFP with new Planner API~~ Refactor VFP with new Planner API May 19, 2018

evil-sherdil added 3 commits May 21, 2018 17:01

Respond to first set of reviews.

8e285c4

Move check for distance validity from old VFP code into new planner.

8bd9c3d

Handle the fact that distance can be negative.

d67d1cc

dqyi11 reviewed May 21, 2018

View reviewed changes

evil-sherdil added 3 commits May 21, 2018 18:22

Make ConfigurationToEndEffectorOffsetPlanner contain mMetaSkeletonSta…

f0a645d

…teSpace instance variable to prvent expensive dynamic casting.

Clean up tiny thing.

09f79b0

Respond to nit.

be62ba8

evil-sherdil added 2 commits May 21, 2018 18:48

Use std::move.

0c3444f

Handle start state.

b1ca2fd

jslee02 previously approved these changes May 22, 2018

View reviewed changes

dqyi11 previously approved these changes May 22, 2018

View reviewed changes

jslee02 mentioned this pull request May 22, 2018

Consider adding VFPlanner constructor that doesn't require start state #434

Open

Update CHANGELOG.md

3865bb0

jslee02 dismissed stale reviews from dqyi11 and themself via 3865bb0 May 22, 2018 22:19

jslee02 merged commit 24618c5 into master May 22, 2018

jslee02 deleted the VFP_refactor branch May 22, 2018 22:19

jslee02 mentioned this pull request May 23, 2018

Fix compilation error in robot/util.cpp #436

Merged

2 tasks

dqyi11 mentioned this pull request Aug 10, 2018

Set start state to state space before creating vector field #456

Merged

5 tasks

gilwoolee pushed a commit that referenced this pull request Jan 21, 2019

Refactor VFP with new Planner API (#426)

7b6bce6

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactor VFP with new Planner API #426

Refactor VFP with new Planner API #426

sniyaz commented May 17, 2018 •

edited

Loading

sniyaz commented May 18, 2018

dqyi11 May 18, 2018

sniyaz May 21, 2018

sniyaz May 21, 2018 •

edited

Loading

dqyi11 May 18, 2018

jslee02 May 18, 2018

jslee02 May 18, 2018

jslee02 May 18, 2018

jslee02 May 18, 2018

dqyi11 May 21, 2018

codecov bot commented May 21, 2018 •

edited

Loading

sniyaz commented May 21, 2018 •

edited

Loading

dqyi11 May 21, 2018

sniyaz commented May 22, 2018 •

edited

Loading

jslee02 left a comment

Refactor VFP with new Planner API #426

Refactor VFP with new Planner API #426

Conversation

sniyaz commented May 17, 2018 • edited Loading

sniyaz commented May 18, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sniyaz May 21, 2018 • edited Loading

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

codecov bot commented May 21, 2018 • edited Loading

Codecov Report

sniyaz commented May 21, 2018 • edited Loading

Choose a reason for hiding this comment

sniyaz commented May 22, 2018 • edited Loading

jslee02 left a comment

Choose a reason for hiding this comment

sniyaz commented May 17, 2018 •

edited

Loading

sniyaz May 21, 2018 •

edited

Loading

codecov bot commented May 21, 2018 •

edited

Loading

sniyaz commented May 21, 2018 •

edited

Loading

sniyaz commented May 22, 2018 •

edited

Loading