Fix issues discovered during demo. #442

sniyaz · 2018-06-06T02:20:41Z

This PR adresses issues related to the new Planner. These issues were discovered while trying to use the new code with the soda demo.

Summary of Changes

Change Problem derivative classes relative to the demo to use ScopedState internally to prevent dangling pointers.
Add a ConfigurationToConfigurationPlanner in the DART planner space that takes a MetaSkeleton.
Add ConfigurationToConfiguration_to_ConfigurationToConfiguration adapter that turns the standard ConfigurationToConfiguration planner into the DART one.
Remove start states from DART Problem classes. This is because the start state should be taken form the MetaSkeleton, and it's why 2 and 3 are necessary.
Make direction passed to ConfigurationToEndEffectorOffset constructor a boost::optional. If it's empty, getDirection returns the current direction of the end effector body node (needed to make planning straight with VFP possible in the demo).
Add getTrajectoryPostProcessor method to ConcreteRobot class. We need this in libherb.
ConfigurationToConfiguration_to_ConfigurationToTSR has been fixed by taking an RNG in the constructor and using this when creating the sampler. The plan() method now checks whether the problem's bodynode is in the skeleton the MetaSkeleton corresponds to (in cases where the hand is not in the MetaSkeleton itself, such as with HERB).

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

… also use ScopedState internally.

…urn bodyNode direction if passed direction was zero vector.

…at problems can store a cloned, scoped version of the passed state.

…for moving straight).

…ptr RNG.

…fset problem.

sniyaz · 2018-06-29T21:44:13Z

I fixed the nits, but I'm trying to fix a build issue with Clan/OSX on Travis.

brianhou · 2018-06-29T22:47:32Z

Hmm, it looks like a potentially weird dependency issue with dart/nlopt/libccd? Here's the error message (which is repeated several times):

CMake Error in src/common/CMakeLists.txt:
Imported target "dart-optimizer-nlopt" includes non-existent path
"/usr/local/Cellar/libccd/2.0_2/include"
in its INTERFACE_INCLUDE_DIRECTORIES. Possible reasons include:

The path was deleted, renamed, or moved to another location.

An install or uninstall procedure did not complete successfully.

The installation package was faulty and references files it does not
provide.

sniyaz · 2018-07-01T09:27:04Z

Yeah, after some googling a CMake file in DART may be the issue? Did we change which version of DART gets used in the OSX test environment?

jslee02

The macOS failure seems not related to the changes of this PR.

Could you please check if the master branch also fails to build on macOS? If so, let's create an issue to track it and merge this PR as it is.

jslee02 · 2018-07-01T17:02:28Z

I believe the macOS build failure is due to that the bottles of dartsim haven't been updated after libccd updated. This should be fixed once Homebrew/homebrew-core#29620 is merged.

brianhou · 2018-07-02T17:29:24Z

include/aikido/planner/Planner.hpp

@@ -44,6 +49,9 @@ class Planner
 protected:
  /// State space associated with this planner.
  statespace::ConstStateSpacePtr mStateSpace;
+
+  /// RNG the planner uses.
+  std::unique_ptr<common::RNG> mRng;


If we're taking in a raw RNG pointer, do we need to store this as a unique_ptr?

I think we could store is as either, since this is hidden from the user. I picked a unique pointer for convenience, since I create a default RNG if one isn't given, and unique pointers handle memory management once the Planner object is destructed (if my understadning of C++ is correct, which it might not be 😅). Otherwise I think we'd need a custom destructor, since we'd malloc a new RNG.

brianhou · 2018-07-02T17:36:40Z

src/planner/dart/ConfigurationToConfiguration.cpp

+  : Problem(stateSpace, std::move(constraint))
+  , mMetaSkeletonStateSpace(stateSpace)
+  , mMetaSkeleton(std::move(metaSkeleton))
+  , mStartState(std::move(mMetaSkeletonStateSpace->createState()))


@jslee02 Are these explicit std::moves for mStartState and mGoalState necessary? Based on my limited understanding of rvalues, those expressions might already be rvalues and therefore std::move is unnecessary?

Yeah, I agree with this. @jslee02 can you confirm?

brianhou · 2018-07-02T17:39:20Z

src/planner/dart/ConfigurationToConfiguration.cpp

+  // instance variable to avoid dangling pointers.
+  if (mMetaSkeleton)
+  {
+    auto startState = mMetaSkeletonStateSpace->createState();


Could this just be

if (mMetaSkeleton) mMetaSkeletonStateSpace->getState(mMetaSkeleton.get(), mStartState);

It seems that mStartState is already a MetaSkeletonStateSpace::ScopedState, so I'm not sure why mStateSpace needs to be used here.

brianhou · 2018-07-02T17:44:14Z

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.hpp

+namespace dart {
+
+class ConfigurationToConfiguration_to_ConfigurationToConfiguration
+    : public PlannerAdapter<aikido::planner::


Nit: I think aikido:: is unnecessary here?

brianhou · 2018-07-02T17:45:40Z

src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp

@@ -57,30 +63,27 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan(
  auto ik = InverseKinematics::create(endEffectorBodyNode);
  ik->setDofs(mMetaSkeleton->getDofs());

-  // Convert TSR constraint into IK constraint
+  // Get the start state form the MetaSkeleton, since this is a DART planner.


Typo: form -> from

brianhou · 2018-07-02T17:55:37Z

include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

@@ -16,27 +17,85 @@ namespace dart {
 class ConfigurationToEndEffectorOffset : public Problem


We should augment this documentation here with the types of constructors that are supported.

brianhou · 2018-07-02T17:56:41Z

src/planner/dart/ConfigurationToEndEffectorOffset.cpp

+  // instance variable to avoid dangling pointers.
+  if (mMetaSkeleton)
+  {
+    auto startState = mMetaSkeletonStateSpace->createState();


Same as above?

brianhou · 2018-07-02T18:00:38Z

include/aikido/robot/ConcreteRobot.hpp

+  // limits, as well as the passed constraint.
+  /// \param[in] metaSkeleton The MetaSkeleton whose limits must be respected.
+  std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
+  getTrajectoryPostProcessor(


I'm fine with this for now, but in light of the discussion from #443 it seems that maybe it doesn't make sense to have this here? I think we should probably set up the postprocessor in the concrete robot constructor, similar to how we set up the metaplanner.

gilwoolee · 2018-07-02T22:13:21Z

include/aikido/planner/dart/ConfigurationToConfiguration.hpp

+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      const statespace::dart::MetaSkeletonStateSpace::State* goalState,
+      constraint::ConstTestablePtr constraint);


Can we allow nullptr for constraint and make it default to Satisfied if nullptr is passed?

It looks like the default Problem class handles this:

aikido/src/planner/Problem.cpp

Line 17 in fe09cbb

if (!mConstraint)

@sniyaz That's true. I think all we need to do here is just to pass nullptr as the default value to constraint.

gilwoolee · 2018-07-02T22:14:44Z

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.hpp

+                                ConfigurationToConfigurationPlanner>
+{
+public:
+  ConfigurationToConfiguration_to_ConfigurationToConfiguration(


Please add docstring to this. :)

gilwoolee · 2018-07-02T22:15:12Z

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp

@@ -10,12 +10,14 @@ namespace planner {
 namespace dart {

 class ConfigurationToConfiguration_to_ConfigurationToTSR
-    : public PlannerAdapter<ConfigurationToConfigurationPlanner,
+    : public PlannerAdapter<aikido::planner::


Nit: same as @brianhou 's comment above, aikido::planner wouldn't be necessary here.

gilwoolee · 2018-07-02T22:15:43Z

include/aikido/planner/dart/ConfigurationToTSR.hpp

  ///
  /// \param[in] stateSpace State space.
+  /// param[in] metaSkeleton MetaSkeleton that getStartState will return the


Nit: typo. \param[in]

gilwoolee · 2018-07-02T22:18:23Z

src/planner/dart/ConfigurationToEndEffectorOffset.cpp

  , mDistance(signedDistance)
 {
-  if (direction.squaredNorm() == 0)
+  if (mDirection.get().squaredNorm() == 0)


Shouldn't this be <= 1e-3 or something that's not exactly zero?

I think this is only here because normalizing a zero vector throws an error. I don't think it's wrong to pass in a small-magnitude vector and use that resulting direction, so I think comparing exactly to zero is fine. However, I think it might be better to use .isZero() rather than .squaredNorm() == 0.

gilwoolee · 2018-07-02T22:18:37Z

src/planner/dart/ConfigurationToEndEffectorOffset.cpp

+  , mDirection(direction)
+  , mDistance(signedDistance)
+{
+  if (mDirection.get().squaredNorm() == 0)


Same question as above.

…ationToConfiguration.

jslee02

Left a few nit comments. Almost there. 😄

jslee02 · 2018-07-07T22:44:09Z

include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

+  /// end-effector BodyNode's current direction.
+  ///
+  /// \param[in] stateSpace State space.
+  /// param[in] metaSkeleton MetaSkeleton that getStartState will return the


Nit: \param[in]

jslee02 · 2018-07-07T22:44:22Z

include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

@@ -47,26 +111,34 @@ class ConfigurationToEndEffectorOffset : public Problem
  /// while maintaining the current orientation.
  ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;

-  /// Returns the start state.
+  /// Return the start state to plan from, either set on construction or


Nit: Returns

jslee02 · 2018-07-07T22:47:27Z

include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      double signedDistance,
+      constraint::ConstTestablePtr constraint);


Nit: Let's pass nullptr to constraint as the default value for the same reason.

jslee02 · 2018-07-07T22:48:03Z

include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

+  /// Returns the direction of motion specified in the world frame. Note that
+  /// if no direction was passed, the current direction of the end effector body
+  /// node is returned.
+  const Eigen::Vector3d getDirection() const;


Nit: No point to return const value.

jslee02 · 2018-07-07T22:48:23Z

include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp

  ///
  /// \param[in] stateSpace State space.
+  /// param[in] metaSkeleton MetaSkeleton that getStartState will return the


Nit: \param[in]

jslee02 · 2018-07-07T22:48:47Z

include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp

      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      const Eigen::Isometry3d& goalPose,
+      constraint::ConstTestablePtr constraint);


Nit: Let's pass nullptr to constraint as the default value for the same reason.

jslee02 · 2018-07-07T22:49:09Z

include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp

      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
      const Eigen::Isometry3d& goalPose,
      constraint::ConstTestablePtr constraint);


Nit: Let's pass nullptr to constraint as the default value for the same reason.

jslee02 · 2018-07-07T22:49:43Z

include/aikido/planner/dart/ConfigurationToTSR.hpp

      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      std::size_t maxSamples,
+      constraint::dart::ConstTSRPtr goalTSR,
+      constraint::ConstTestablePtr constraint);


Nit: Let's pass nullptr to constraint as the default value for the same reason.

jslee02 · 2018-07-07T22:49:52Z

include/aikido/planner/dart/ConfigurationToTSR.hpp

      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
      std::size_t maxSamples,
      constraint::dart::ConstTSRPtr goalTSR,
      constraint::ConstTestablePtr constraint);


Nit: Let's pass nullptr to constraint as the default value for the same reason.

jslee02 · 2018-07-07T22:50:03Z

include/aikido/planner/dart/ConfigurationToTSR.hpp

@@ -46,22 +68,30 @@ class ConfigurationToTSR : public Problem
  /// Returns the maximum number of TSR samples to plan to.
  std::size_t getMaxSamples() const;

-  /// Returns the start state.
+  /// Return the start state to plan from, either set on construction or


Nit: Returns

brianhou · 2018-07-11T20:03:26Z

It looks like the only failures are due to the pending Homebrew PR, so I'm going to merge this. Thanks for all the changes!

This reverts commit f05e40b.

* Test with COnfitoConf and seems to work! * Add getTrajectoryPostProcessor() method to ConcreteRobot class. * Make ConfigurationToEndEffectorOffset and ConfigurationToTSR problems also use ScopedState internally. * HACK: Make ConfigurationToEndEffectorOffset getDirection() method return bodyNode direction if passed direction was zero vector. * Fix ConfigurationToConfiguration_to_ConfigurationToTSR adapter now that problems can store a cloned, scoped version of the passed state. * HACK: Fix hack for returning current BN direction in Offset problem (for moving straight). * HACK: Get around HERB hand BN not being in arm metaSkeleton, and nullptr RNG. * HACK: Update hacks to work with MAGI. * HACK: Got left out of last commit. * HACK: Cap samples in DART TSR adapter. Use better random seed. * Make getTrajectoryPostProcessor take needed args. * Make getTrajectoryPostProcessor take constraint checking params. * Do BN check in TSR adapter correctly by using the skeleton. * Add RNG to ConfigurationToConfiguration_to_ConfigurationToTSR. * Remove startState from DART problems. Need to fix tests. * Add DART ConfigurationToConfigurationPlanner. * Add new adapter that turns non-DART ConfTOConf planner into a DART one. * Update tests for removing startState from DART problems. * Run make format on last commit. * Fix tests from adding RNG to TSR adapter. * Fix naming ambiguity that made TSR adapter not build. * Use boost:none instead of zero vector to get straight direction in offset problem. * Make Offset problem take optional as reference. * Clean up Offset problem comments and restore zero vector check. * Remove TSR adapter hack to prevent inf loops. * HACK BODY NODE FALLS OFF * HACK HACK HACK. * HACK HACK JH * HACK: IKJDSHFKJDSHFKJDSHFKDSHFLJDHFSKJHDSFKJDHF * Revert "HACK: IKJDSHFKJDSHFKJDSHFKDSHFLJDHFSKJHDSFKJDHF" This reverts commit 40482d3. * Revert "HACK HACK JH" This reverts commit 42cd655. * Revert "HACK HACK HACK." This reverts commit 85eee66. * Revert "HACK BODY NODE FALLS OFF" This reverts commit 40b1207. * Update ConfigurationToEndEffectorOffset to use multiple constructors. * Update getStartState in ConfigurationToEndEffectorOffset. * Update ConfigurationToEndEffectorPose to use two constructors and restore getStartState. Fix state passing in ConfigurationToEndEffectorOffset. * Update ConfigurationToTSR to use multiple constructors and restore getStartState. * Update TSR adapter to just use getStartState(). * Pass start state to VFP. * Revert "Update TSR adapter to just use getStartState()." This reverts commit 976b443. * UpdateDART planning problems to not return dangling pointers with getStateState(). * Draft dart/ConfigurationToConfiguration.hpp * Draft dart/ConfigurationToConfiguration.cpp * Make adapter use right ConfigToConfig Problem. * Trim fat in new getTrajectoryPostProcessor method. * Fix tests with what we have now. * Run make format. * Make Planner take RNG. * Add rng arg to SingleProblemPlanner and ConfigurationToConfigurationPlanner constructors to enable future OMPL planners to take RNG. * Use delegate RNG in TSR adapter. Make tests build again. * Move getEndEffectorDirection into robot/util * Create planner/dart/util and put getEndEffectorDirection there instead. * Change Planner to take raw RNG pointer, and getRng to return this raw pointer. * Respond to nits. * Fix RNG destruction in Planner class. * Repond to latest review comments. * Remove some std::moves to fix RVO build issue with OSX. * Nab one std::move missed by the last commit.. * Use getState in DART planning problems. * Remove uneeded aikido::'s in ConfigurationToConfiguration_to_ConfigurationToConfiguration. * Run make format. * Respond to all but one of Gilwoo's nits. * Respond to some more nits.

This reverts commit f05e40b.

evil-sherdil added 25 commits May 30, 2018 20:03

Test with COnfitoConf and seems to work!

e24c163

Add getTrajectoryPostProcessor() method to ConcreteRobot class.

411aba5

Make ConfigurationToEndEffectorOffset and ConfigurationToTSR problems…

05fac7c

… also use ScopedState internally.

HACK: Make ConfigurationToEndEffectorOffset getDirection() method ret…

497e474

…urn bodyNode direction if passed direction was zero vector.

Fix ConfigurationToConfiguration_to_ConfigurationToTSR adapter now th…

54f23a5

…at problems can store a cloned, scoped version of the passed state.

HACK: Fix hack for returning current BN direction in Offset problem (…

349d294

…for moving straight).

HACK: Get around HERB hand BN not being in arm metaSkeleton, and null…

4ffcdaa

…ptr RNG.

HACK: Update hacks to work with MAGI.

c7d2c34

HACK: Got left out of last commit.

0ac8554

HACK: Cap samples in DART TSR adapter. Use better random seed.

f17d866

Make getTrajectoryPostProcessor take needed args.

689e718

Make getTrajectoryPostProcessor take constraint checking params.

c8c5873

Do BN check in TSR adapter correctly by using the skeleton.

fff03e9

Add RNG to ConfigurationToConfiguration_to_ConfigurationToTSR.

08fc93b

Remove startState from DART problems. Need to fix tests.

c283296

Add DART ConfigurationToConfigurationPlanner.

7dca09c

Add new adapter that turns non-DART ConfTOConf planner into a DART one.

feed4d8

Update tests for removing startState from DART problems.

894beeb

Run make format on last commit.

1b51253

Fix tests from adding RNG to TSR adapter.

cc661e1

Fix naming ambiguity that made TSR adapter not build.

ea2ad17

Use boost:none instead of zero vector to get straight direction in of…

3458ef7

…fset problem.

Make Offset problem take optional as reference.

a939cc6

Clean up Offset problem comments and restore zero vector check.

c63b74e

Remove TSR adapter hack to prevent inf loops.

714c26f

sniyaz changed the title ~~Fixes discovered during DEMO.~~ Fix issues discovered during demo. Jun 6, 2018

sniyaz requested review from gilwoolee, jslee02 and brianhou June 6, 2018 02:21

sniyaz mentioned this pull request Jun 6, 2018

DEMO: Add getTrajectoryPostProcessor method to ConcreteRobot class. #440

Closed

5 tasks

Remove some std::moves to fix RVO build issue with OSX.

437b63e

jslee02 previously approved these changes Jul 1, 2018

View reviewed changes

brianhou suggested changes Jul 2, 2018

View reviewed changes

gilwoolee reviewed Jul 2, 2018

View reviewed changes

gilwoolee suggested changes Jul 2, 2018

View reviewed changes

Nab one std::move missed by the last commit..

e138d30

sniyaz dismissed jslee02’s stale review via e138d30 July 3, 2018 23:45

evil-sherdil added 4 commits July 7, 2018 17:08

Use getState in DART planning problems.

a88a8c1

Remove uneeded aikido::'s in ConfigurationToConfiguration_to_Configur…

ecb5239

…ationToConfiguration.

Run make format.

0d4be11

Respond to all but one of Gilwoo's nits.

aa9554e

jslee02 previously approved these changes Jul 7, 2018

View reviewed changes

gilwoolee previously approved these changes Jul 9, 2018

View reviewed changes

Respond to some more nits.

204cd21

sniyaz dismissed stale reviews from gilwoolee and jslee02 via 204cd21 July 10, 2018 23:34

jslee02 approved these changes Jul 10, 2018

View reviewed changes

sniyaz added this to the Aikido 0.3.0 milestone Jul 10, 2018

brianhou approved these changes Jul 10, 2018

View reviewed changes

brianhou merged commit f05e40b into master Jul 11, 2018

brianhou deleted the sniyaz/new_demo branch July 11, 2018 20:03

Ponzel added a commit that referenced this pull request Jul 17, 2018

Revert "Fix issues discovered during demo. (#442)"

e20a570

This reverts commit f05e40b.

aditya-vk mentioned this pull request Jul 18, 2018

Introducing setter methods in problem classes #449

Closed

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

Revert "Fix issues discovered during demo. (#442)"

b459174

This reverts commit f05e40b.

		@@ -16,27 +17,85 @@ namespace dart {
		class ConfigurationToEndEffectorOffset : public Problem

Fix issues discovered during demo. #442

Fix issues discovered during demo. #442

Conversation

sniyaz commented Jun 6, 2018 • edited Loading

sniyaz commented Jun 29, 2018

brianhou commented Jun 29, 2018

sniyaz commented Jul 1, 2018

jslee02 left a comment

Choose a reason for hiding this comment

jslee02 commented Jul 1, 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

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 Jul 7, 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

jslee02 left a comment • 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

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

brianhou commented Jul 11, 2018

sniyaz commented Jun 6, 2018 •

edited

Loading

jslee02 commented Jul 1, 2018 •

edited

Loading

jslee02 Jul 7, 2018 •

edited

Loading

jslee02 left a comment •

edited

Loading