Ranking IK Solutions #423

aditya-vk · 2018-05-15T21:23:10Z

This PR introduces ranker for IK solutions.

Looking for discussion on:

Should we make it more general to also be able to rank TSRs with the same (templated) classes?
I prefer having them separate to make it simple and free of potential errors.

Decided to keep them separate

Where do we have the files? It seemed constraint/dart/ might be a good place since it's still a constraint over the IKs which by itself is a constraint. Can also be under planner.

Decided to have them under distance

Naming convention for the classes.
Efficiency and implementation details.

~~WIP: Tests. I started a test for FIFO (which is more or less a random IK ranker like in prpy).~~

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

…onst_correctness

aditya-vk · 2018-07-10T18:53:42Z

Cleaned up! Should be good to go (I think). Will follow up with updates to planToTSR methods once this gets merged :)

The tests are failing for reasons that don't have to do with the code itself.

brianhou · 2018-07-10T20:02:23Z

include/aikido/distance/ConfigurationRanker.hpp

+#ifndef AIKIDO_DISTANCE_CONFIGURATIONRANKER_HPP_
+#define AIKIDO_DISTANCE_CONFIGURATIONRANKER_HPP_
+
+#include "aikido/distance/CartesianProductWeighted.hpp"


Nit: is this include necessary?

brianhou · 2018-07-10T20:03:22Z

include/aikido/distance/ConfigurationRanker.hpp

+
+  /// Returns a vector of configurations ranked in increasing order of costs.
+  /// \param[in] configurations Vector of configurations to rank.
+  std::vector<statespace::CartesianProduct::State*> rankConfigurations(


Let's change this from CartesianProduct::State to MetaSkeletonStateSpace::State, etc.

brianhou · 2018-07-10T20:03:43Z

include/aikido/distance/ConfigurationRanker.hpp

+#include "aikido/distance/CartesianProductWeighted.hpp"
+#include "aikido/distance/DistanceMetric.hpp"
+#include "aikido/distance/defaults.hpp"
+#include "aikido/statespace/CartesianProduct.hpp"


I think this is also unnecessary.

brianhou · 2018-07-10T20:08:39Z

include/aikido/distance/ConfigurationRanker.hpp

+  virtual ~ConfigurationRanker() = default;
+
+  /// Returns the statespace.
+  statespace::ConstStateSpacePtr getStateSpace() const;


This method doesn't seem to be used. I'd prefer to remove it if we don't see a use for it, but I think this should return a ConstMetaSkeletonStateSpacePtr if we decide to keep it.

I'll remove it for now.

brianhou · 2018-07-10T20:09:39Z

include/aikido/distance/NominalConfigurationRanker.hpp

+  ///
+  /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
+  /// \param[in] metaskeleton Metaskeleton of the robot.
+  /// \param[in] nominalConfiguration Nominal Configuration. The current


Nit: Nominal Configuration -> Nominal configuration.

Can we also provide a nullptr default?

brianhou · 2018-07-10T20:11:31Z

include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp

+  /// specified DART \c MetaSkeleton positions.
+  ///
+  /// \param[in] positions DART \c MetaSkeleton positions
+  ScopedState getStateFromPositions(Eigen::VectorXd& positions) const;


In that case, let's add it as a separate/future PR instead so this one is just about the IK rankers.

brianhou · 2018-07-10T20:12:31Z

src/distance/ConfigurationRanker.cpp

+    throw std::invalid_argument("MetaSkeleton is nullptr.");
+
+  mDistanceMetric = createDistanceMetricFor(
+      std::dynamic_pointer_cast<statespace::CartesianProduct>(


Are both of these casts necessary? Does createDistanceMetricFor not work for const statespace::CartesianProduct?

The const-cast should not be necessary.

brianhou · 2018-07-10T20:14:26Z

src/distance/ConfigurationRanker.cpp

+}
+
+//==============================================================================
+std::vector<statespace::CartesianProduct::State*>


It seems that we're returning raw pointers to the CartesianProduct::States, rather than CartesianProduct::ScopedStates. What would happen if the input configurations are destructed before the ranking results are accessed?

brianhou · 2018-07-10T20:18:55Z

src/distance/JointAvoidanceConfigurationRanker.cpp

+JointAvoidanceConfigurationRanker::JointAvoidanceConfigurationRanker(
+    ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace,
+    ConstMetaSkeletonPtr metaSkeleton)
+  : ConfigurationRanker(metaSkeletonStateSpace, metaSkeleton)


Should this std::move the MetaSkeletonStateSpace and MetaSkeleton?

brianhou · 2018-07-10T20:21:39Z

This looks pretty good overall! Thanks for the changes.

@jslee02 would you mind looking over the State stuff to make sure we're using them correctly? Thanks!

aditya-vk · 2018-07-10T22:31:12Z

Addressed the comments except for the State-handling. I'll wait for @jslee02 before modifications. I assumed that the way we'd be using the ranker we wouldn't be destroying the input states but that's a possibility in any case that needs taken care of.

brianhou · 2018-07-10T23:12:45Z

I think it's reasonable to do something like return ranker.rankConfigurations(states);, which I think would lose the states.

jslee02 · 2018-07-11T20:28:47Z

I'm not sure if I can review this PR soon (hopefully late this week?). Please go ahead if this is blocked by me. 😅

jslee02

The build failures on macOS is fixed by Homebrew/homebrew-core#29620.

If I don't forget the AIKIDO's state spate mechanism by the last three weeks, classes should hold states as scoped states unless the lifetime is guaranteed to be longer than the lifetime of the state holder, which I don't think that's the case in this PR.

Caveat: I reviewed this PR quickly so it could include incorrect comments.

jslee02 · 2018-07-15T05:45:22Z

tests/distance/test_JointAvoidanceConfigurationRanker.cpp

+
+static constexpr double EPS = 1e-6;
+
+static BodyNode::Properties create_BodyNodeProperties(const std::string& _name)


Nit: No more leading underscore in parameter name!

jslee02 · 2018-07-15T05:46:19Z

src/distance/NominalConfigurationRanker.cpp

+
+//==============================================================================
+double NominalConfigurationRanker::evaluateConfiguration(
+    statespace::dart::MetaSkeletonStateSpace::State* solution) const


Why solution is not const?

jslee02 · 2018-07-15T05:47:33Z

src/distance/NominalConfigurationRanker.cpp

+namespace distance {
+
+using statespace::dart::ConstMetaSkeletonStateSpacePtr;
+using statespace::CartesianProduct;


Nit: This type alias isn't used

jslee02 · 2018-07-15T05:48:09Z

src/distance/JointAvoidanceConfigurationRanker.cpp

+
+//==============================================================================
+double JointAvoidanceConfigurationRanker::evaluateConfiguration(
+    statespace::dart::MetaSkeletonStateSpace::State* solution) const


Maybe const?

jslee02 · 2018-07-15T05:50:03Z

src/distance/JointAvoidanceConfigurationRanker.cpp

+        std::move(metaSkeletonStateSpace), std::move(metaSkeleton))
+{
+  auto lowerLimits = mMetaSkeleton->getPositionLowerLimits();
+  auto upperLimits = mMetaSkeleton->getPositionUpperLimits();


Nit: const auto& since getPosition(Lower/Upper)Limits() returns const reference.

jslee02 · 2018-07-15T05:53:29Z

src/distance/JointAvoidanceConfigurationRanker.cpp

+  }
+
+  mLowerLimitsState = mMetaSkeletonStateSpace->createState();
+  mUpperLimitsState = mMetaSkeletonStateSpace->createState();


mUpperLimitsState should be scoped state. Otherwise it will be destructed out of this function, and mUpperLimitsState will become dangling pointer.

brianhou · 2018-07-16T22:19:47Z

include/aikido/distance/ConfigurationRanker.hpp

+  /// Destructor
+  virtual ~ConfigurationRanker() = default;
+
+  /// Ranks/Sorts the vector of configurations in increasing order of costs.


Nit: I think sorts is redundant here?

brianhou · 2018-07-16T22:21:10Z

include/aikido/distance/ConfigurationRanker.hpp

+  virtual ~ConfigurationRanker() = default;
+
+  /// Ranks/Sorts the vector of configurations in increasing order of costs.
+  /// \param[in] configurations Vector of configurations to rank.


Nit: I believe the Doxygen convention for this is \param[in,out].

brianhou · 2018-07-16T22:23:41Z

include/aikido/distance/JointAvoidanceConfigurationRanker.hpp

+  std::vector<std::size_t> mUnboundedUpperLimitsIndices;
+
+  /// (Modified) State corresponding to the lower position limits.
+  /// The positions at the indices in mUnboundedLowerLimitsIndices are modified


I think we can change the descriptions for both lower/upper to clarify that the states are modified by evaluateConfiguration to match the input state before computing the distance from the lower limits.

brianhou · 2018-07-16T22:24:31Z

include/aikido/distance/ConfigurationRanker.hpp

+
+namespace aikido {
+namespace distance {
+


Nit: document ConfigurationRanker.

brianhou · 2018-07-16T22:26:25Z

include/aikido/distance/JointAvoidanceConfigurationRanker.hpp

+
+namespace aikido {
+namespace distance {
+


Nit: document JointAvoidanceConfigurationRanker.

I think we should mention here that only joints with limits contribute to the distance to the lower/upper limits.

brianhou · 2018-07-16T22:27:35Z

include/aikido/distance/NominalConfigurationRanker.hpp

+  /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
+  /// \param[in] metaskeleton Metaskeleton of the robot.
+  /// \param[in] nominalConfiguration Nominal configuration. The current
+  /// configuration is considered if set to nullptr.


Nit: clarify that it's the current configuration of the metaSkeleton.

brianhou · 2018-07-16T22:27:56Z

include/aikido/distance/NominalConfigurationRanker.hpp

+  /// Constructor
+  ///
+  /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
+  /// \param[in] metaskeleton Metaskeleton of the robot.


Nit: metaskeleton -> metaSkeleton.

brianhou · 2018-07-16T22:28:07Z

include/aikido/distance/JointAvoidanceConfigurationRanker.hpp

+  /// Constructor
+  ///
+  /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
+  /// \param[in] metaskeleton Metaskeleton of the robot.


Nit: metaskeleton -> metaSkeleton.

brianhou · 2018-07-16T22:28:18Z

include/aikido/distance/ConfigurationRanker.hpp

+  /// Constructor
+  ///
+  /// \param[in] metaSkeletonStateSpace Statespace of the skeleton.
+  /// \param[in] metaskeleton Metaskeleton of the robot.


Nit: metaskeleton -> metaSkeleton.

brianhou · 2018-07-16T22:49:57Z

src/distance/JointAvoidanceConfigurationRanker.cpp

+  auto lowerLimits = mMetaSkeleton->getPositionLowerLimits();
+  auto upperLimits = mMetaSkeleton->getPositionUpperLimits();
+
+  for (std::size_t i = 0; i < mMetaSkeletonStateSpace->getDimension(); ++i)


Quick question: is there ever a case where there are lower limits but not upper limits (or vice versa)? If not, then rather than checking against infs, maybe we can use DegreeOfFreedom::hasPositionLimit. I think it'd look something like this:

auto properties = mMetaSkeletonStateSpace->getProperties(); for (std::size_t i = 0; i < properties.getNumDofs(); ++i) { if (mMetaSkeleton->getDof(i)->hasPositionLimit()) mUnboundedIndices.emplace_back(i); }

I wanted to have it to be general just in case. Looks like dart is checking both upper and lower limits separately when performing that check implying no such assumption is being made if I understand correctly.

Stale

jslee02 · 2018-07-17T05:41:48Z

Hm, for some reason, test_NominalConfigurationRanker fails on macOS.

* init * Fixing const-correctness in state space * update structure * make format * source for base ranking class * make format * add general sorting * make format * preallocate size of ranking vector * nit todo removed * remove unnecessary headers + new strategies * make format * remove redundant virtual keyword * add FIFO to ranking strategy * add source scripts for ranking strategies * assign the base class members first * make format * add sources to read later * correct macro for header-guard * tests for FIFO + cleanup * Fixing const-correctness of StateHandle * Fix implicit conversion of StateHandle to State* * Undo unnecessary changes * Remove incorrect comment * Add clone to StateSpace * Format code * test for the bug * bug fix in adding IK solutions * comment out unused variables * nit comments * Update changelog * Update CHANGELOG.md * WIP: Random restructuring to test stuff * change API * make format * nominal configuration strategy * test for nominal configuration strategy * restructure the code into distance * enable base class and nominal configuration ranker * bring back test for nominal config ranker + check distances * simplify the test script * bring bakc joint avoidance ranker * simplify the code * make format * clean up test scripts * joint avoidance strategy * consider cyclic joints * make format * make tests uniform * update log * simplify and address PR comments * modify joint avoidance. TODO: change to indices of unbounded limits? * modify constructor to take nominal configuration as parameter * make format * add docstring * work with indices * make format * convenience function to get state without creating it explicitly * simplify code * make format * update log * edit docstrings * use cartesianproduct state space * add docstrings * create limits states in ctr; use cartesian sspace. * make format * move helper function to anonymous namespace * rename variables and functions for increased clarity * make format * simplify tests scripts * remove unnecessary clone * make format * take in scoped states as expected during planning * make format * remove function outside scope of the PR * remove getter for statespace; use MetaSkeletonStateSpace * remove const-cast * make format * address concerns over state; cleanup * remove unnecessary header * remove unnecesary header * make ranking efficient; address nit comments * make format * Refactor and fix weird bug.

aditya-vk and others added 30 commits May 10, 2018 22:05

init

1deb4af

Fixing const-correctness in state space

0564808

Merge branch 'master' into ranker

80aad9e

update structure

e6a196a

make format

97adbe5

source for base ranking class

d44d582

make format

e2b4811

add general sorting

a88135f

make format

3e57698

preallocate size of ranking vector

8ee14a8

nit todo removed

6555f44

remove unnecessary headers + new strategies

bfd0bba

make format

bcb89a8

remove redundant virtual keyword

a554a3f

add FIFO to ranking strategy

3605059

add source scripts for ranking strategies

7b943ac

assign the base class members first

e3c9760

make format

603b5a5

add sources to read later

c1ad242

correct macro for header-guard

0daaa76

tests for FIFO + cleanup

f0ce3c3

Merge remote-tracking branch 'origin/master' into bugfix/statespace_c…

a745168

…onst_correctness

Fixing const-correctness of StateHandle

9d1c5d5

Fix implicit conversion of StateHandle to State*

45bd002

Undo unnecessary changes

cd42da9

Remove incorrect comment

0325051

Add clone to StateSpace

5537180

Format code

25be305

Merge branch 'master' into enhance/clone_state

029e9f7

Merge branch 'master' into ranker

d53b50b

make format

4d87a74

aditya-vk removed the pr: work-in-progress Calls for preliminary review of the PR. Remove when PR is ready for complete review. label Jul 8, 2018

brianhou suggested changes Jul 10, 2018

View reviewed changes

aditya-vk added 4 commits July 10, 2018 14:39

remove function outside scope of the PR

99b8ae5

remove getter for statespace; use MetaSkeletonStateSpace

ea26812

remove const-cast

8f3deb1

make format

e41c965

Merge branch 'master' into ranker

e9e7762

jslee02 previously requested changes Jul 15, 2018

View reviewed changes

aditya-vk added 3 commits July 15, 2018 23:12

address concerns over state; cleanup

7327b20

remove unnecessary header

99118d0

remove unnecesary header

2cfec3e

brianhou suggested changes Jul 16, 2018

View reviewed changes

aditya-vk added 2 commits July 16, 2018 17:37

make ranking efficient; address nit comments

66eec69

make format

3f36bf1

brianhou previously approved these changes Jul 17, 2018

View reviewed changes

Refactor and fix weird bug.

84a0041

brianhou dismissed their stale review via 84a0041 July 17, 2018 07:13

brianhou approved these changes Jul 17, 2018

View reviewed changes

brianhou merged commit cfba2f6 into master Jul 17, 2018

brianhou deleted the ranker branch July 17, 2018 14:34

aditya-vk mentioned this pull request Dec 4, 2018

Sampling IKs close to the current configuration #384

Closed


		static constexpr double EPS = 1e-6;

		static BodyNode::Properties create_BodyNodeProperties(const std::string& _name)

Ranking IK Solutions #423

Ranking IK Solutions #423

Conversation

aditya-vk commented May 15, 2018 • edited Loading

aditya-vk commented Jul 10, 2018

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 10, 2018

aditya-vk commented Jul 10, 2018

brianhou commented Jul 10, 2018

jslee02 commented Jul 11, 2018

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

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 commented Jul 17, 2018

aditya-vk commented May 15, 2018 •

edited

Loading

jslee02 left a comment •

edited

Loading