Refactor DART StateSpaces #278

brianhou · 2017-12-08T07:02:50Z

This PR refactors AIKIDO's DART state spaces to decouple the state space from the underlying DART Joint or MetaSkeleton, as suggested by @mkoval and @psigen in #249.

This will be a hugely backwards-incompatible API change. Any code that uses MetaSkeletonStateSpace::getMetaSkeleton, JointStateSpace::getJoint, or MetaSkeletonStateSpaceSaver will need to be fixed.

Quick summary of changes:

JointStateSpace::getJoint has been removed and JointStateSpace::getProperties has been added.
MetaSkeletonStateSpace::getMetaSkeleton has been removed and MetaSkeletonStateSpace::getProperties has been added. MetaSkeletonStateSpace::Properties also holds information about how to map (joint index, joint dof index) to metaskeleton dof index, rather than re-computing each time we need to convert between states and positions.
MetaSkeletonStateSpaceSaver has been renamed to MetaSkeletonStateSaver. I think this makes more sense, since it was most importantly used to save the current MetaSkeleton position (which would no longer be in a MetaSkeletonStateSpace).
Many constraints (e.g. constraint::CollisionFree, constraint::InverseKinematicsSampleable) now take in an additional MetaSkeleton argument. This is necessary so they have a concrete skeleton that they can test with.
Conversions between SplineTrajectory and ROS JointTrajectory are somewhat different. The joint names in the JointTrajectory now come from the DOF names, rather than the Joint names. I thought this was simpler, but could be convinced to figure out how to use Joint names instead if this is the wrong thing to do. (It seems that each joint in a ROS JointTrajectory can only take on one value, so maybe it's closer to what we mean by DOF?)

Note: one of the VectorFieldPlanner tests is currently failing, but I figured it's not really worth debugging since the improved VectorFieldPlanner will be ready soon and will need to be ported to the new MetaSkeletonStateSpace anyway.

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

This decouples the state space from the underlying DART Joint that was used to generate the state space. Joint properties (e.g. position limits) are maintained in a separate JointStateSpace::Properties field, which is not tied to the original DART Joint. This also changes the interfaces for the JointStateSpace::getState and JointStateSpace::setState methods to take in a DART Joint.

Since MetaSkeletonStateSpaces won't have an internal MetaSkeleton, having a MetaSkeletonStateSpaceSaver doesn't make sense.

This decouples the state space from the underlying DART MetaSkeleton that was used to generate the state space. Properties are maintained in a separate MetaSkeletonStateSpace::Properties field. This changes the interfaces for the MetaSkeletonStateSpace::getState and MetaSkeletonStateSpace::setState methods to take in a DART MetaSkeleton. This also changes the interfaces for many of the constraints and executors to take in an additional DART MetaSkeleton argument. The VectorFieldPlanner code is in a separate commit so it can be easily reverted when the improved VectorFieldPlanner is implemented.

One of the VectorFieldPlanner tests is currently failing, but I figured it's not worth debugging since the improved VectorFieldPlanner will be ready soon. This is in a separate commit so it can be easily reverted when the improved implementation is ready.

codecov · 2017-12-08T07:38:11Z

Codecov Report

Merging #278 into master will decrease coverage by 0.32%.
The diff coverage is 73.5%.

@@            Coverage Diff             @@
##           master     #278      +/-   ##
==========================================
- Coverage   79.75%   79.42%   -0.33%     
==========================================
  Files         201      202       +1     
  Lines        5768     5841      +73     
==========================================
+ Hits         4600     4639      +39     
- Misses       1168     1202      +34

Impacted Files	Coverage Δ
...lude/aikido/constraint/FramePairDifferentiable.hpp	`100% <ø> (ø)`	⬆️
include/aikido/statespace/dart/SO3Joint.hpp	`100% <ø> (ø)`	⬆️
include/aikido/statespace/dart/SO2Joint.hpp	`100% <ø> (ø)`	⬆️
include/aikido/statespace/StateSpace.hpp	`100% <ø> (ø)`	⬆️
...do/planner/vectorfield/BodyNodePoseVectorField.hpp	`100% <ø> (ø)`	⬆️
...ner/vectorfield/MoveEndEffectorPoseVectorField.hpp	`100% <ø> (ø)`	⬆️
include/aikido/constraint/FrameDifferentiable.hpp	`100% <ø> (ø)`	⬆️
include/aikido/statespace/dart/WeldJoint.hpp	`100% <ø> (ø)`	⬆️
...r/vectorfield/MoveEndEffectorOffsetVectorField.hpp	`100% <ø> (ø)`	⬆️
include/aikido/constraint/CollisionFree.hpp	`100% <ø> (ø)`	⬆️
... and 41 more

mkoval

This looks great. Nice work @brianhou! I generally think this is ready-to-merge once you address the merge conflicts with devel, which seem to be caused by the recent merge of #286.

Two random musings:

It's annoying that we often pass around a MetaSkeletonStateSpace and a concrete MetaSkeleton as a pair, e.g. to implement constraints. If this continues to be an issue, it may be worthwhile to wrap this in a helper class that is effectively an std::pair that has an API similar to the old implementation of MetaSkeletonStateSpace.
At some point we'll likely want to parallelize constraint evaluations. That will require cloning a constraint onto a different MetaSkeleton, e.g. creating two PoseConstraints that refer to the different MetaSkeletons that both instantiate the same MetaSkeletonStateSpace. This will be tricky to implement for constraints that wrap other constraints - but I have some ideas here.

mkoval · 2017-12-08T18:22:36Z

include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp

-  /// \return Degree of freedom
-  const dart::dynamics::DegreeOfFreedom* dof() const;
+  /// \return Degree of freedom name
+  const std::string dof() const;


const is not meaningful here. Also, I suggest naming this "DOF name" - since it is now a string rather than a DegreeOfFreedom object itself.

mkoval · 2017-12-08T18:23:50Z

include/aikido/statespace/dart/JointStateSpace.hpp

+    const Eigen::VectorXd getVelocityLowerLimits() const;
+
+    /// Return the vector of velocity upper limits.
+    const Eigen::VectorXd getVelocityUpperLimits() const;


All of these const return values are not meaningful. Consider returning by const & instead.

mkoval · 2017-12-08T18:23:59Z

include/aikido/statespace/dart/JointStateSpace.hpp

+    std::size_t getNumDofs() const;
+
+    /// Return the names of DOFs in the joint.
+    std::vector<std::string> getDofNames() const;


Return by const &?

mkoval · 2017-12-08T18:24:07Z

include/aikido/statespace/dart/JointStateSpace.hpp

+    const std::string getName() const;
+
+    /// Return the type of the joint.
+    const std::string getType() const;


Return by const &?

mkoval · 2017-12-08T18:24:10Z

include/aikido/statespace/dart/JointStateSpace.hpp

+    explicit Properties(::dart::dynamics::Joint* _joint);
+
+    /// Return the name of the joint.
+    const std::string getName() const;


Return by const &?

mkoval · 2017-12-08T18:33:39Z

include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp

+    /// Mapping from Joint index and Joint DOF index to MetaSkeleton DOF index
+    std::unordered_map<std::size_t,
+                       std::unordered_map<std::size_t, std::size_t>>
+        mIndexMap;


Just a suggestion: If the docstring is correct, maybe this should be a:

std::unordered_map<std::pair<std::size_t, std::size_t>, std::size_t>

This was because pairs don't come with a hash function and I was being sloppy... I'll fix it.

mkoval · 2017-12-08T18:48:53Z

src/statespace/dart/MetaSkeletonStateSpace.cpp

+    for (std::size_t idof = 0; idof < joint->getNumDofs(); ++idof)
+    {
+      const auto dof = joint->getDof(idof);
+      const auto dofName = dof->getName();


Nit: const auto&?

mkoval · 2017-12-08T18:52:01Z

src/statespace/dart/MetaSkeletonStateSpace.cpp

+std::size_t MetaSkeletonStateSpace::Properties::getDofIndex(
+    const std::string& dofName) const
+{
+  std::vector<std::size_t> indices;


Nit: Call indices.reserve(mDofNames.size()) here.

mkoval · 2017-12-12T03:34:37Z

include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp

  using CartesianProduct::State;
  using CartesianProduct::ScopedState;

  /// Constructs a state space for a DART \c MetaSkeleton.
  ///
  /// \param _metaskeleton target \c MetaSkeleton
-  MetaSkeletonStateSpace(::dart::dynamics::MetaSkeletonPtr _metaskeleton);
+  MetaSkeletonStateSpace(::dart::dynamics::MetaSkeleton* _metaskeleton);


Mark this as explicit. It should have been before, too! 😱

mkoval · 2017-12-12T03:34:48Z

include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp

-  /// \return \c MetaSkeleton associated with this state space
-  ::dart::dynamics::MetaSkeletonPtr getMetaSkeleton() const;
+  /// \return MetaSkeleton properties associated with this state space
+  const Properties getProperties() const;


Nit: const is not meaningful.

Return const & rather than const.

…eSpaces." This reverts commit 934ee62.

Conflicts: src/planner/vectorfield/VectorFieldPlanner.cpp tests/constraint/test_CollisionFree.cpp tests/planner/ompl/OMPLTestHelpers.hpp

…ces.

brianhou · 2018-01-07T04:35:06Z

I've updated this PR with the new VectorFieldPlanner implementation, so there are no longer any conflicts with master. Can someone (@gilwoolee or @jslee02) take one last pass over this before I merge?

jslee02

Per this change, all the DART state spaces take DART objects (meta-skeleton and joints) only to retrieve data from the DART objects but not to change them. So it would be good to take DART objects as const variable. I've made this change in this branch so feel free to merge if it looks good to you.

Also, DART state space assumes the passing-in DART object pointer is not nullptr. I think we should either throw an exception for nullptr or take it as a const reference.

gilwoolee

I made a few small comments but other than that it looks good. :)

gilwoolee · 2018-01-09T20:47:36Z

include/aikido/planner/vectorfield/VectorFieldUtil.hpp

@@ -31,8 +31,8 @@ namespace vectorfield {
 bool computeJointVelocityFromTwist(
    Eigen::VectorXd& jointVelocity,
    const Eigen::Vector6d& desiredTwist,
-    aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace,
-    dart::dynamics::BodyNodePtr bodyNode,
+    const dart::dynamics::MetaSkeletonPtr stateSpace,


Need to change the name to metaSkeleton.

gilwoolee · 2018-01-09T20:49:48Z

include/aikido/statespace/dart/JointStateSpace.hpp


 protected:
-  ::dart::dynamics::Joint* mJoint;
+  Properties mProperties;


Can we make it const?

gilwoolee · 2018-01-09T20:56:39Z

src/constraint/CollisionFree.cpp

  , mCollisionDetector(std::move(_collisionDetector))
  , mCollisionOptions(std::move(_collisionOptions))
 {
-  if (!mStatespace)
-    throw std::invalid_argument("_statespace is nullptr.");
+  if (!mMetaSkeletonStateSpace)


TODO: check whether _metaSkeletonStateSpace and _metaskeleton are compatible.

gilwoolee · 2018-01-09T20:57:40Z

src/constraint/FrameDifferentiable.cpp

 {
+  if (!mMetaSkeletonStateSpace)


TODO: check compatibility between _metaSkeletonStateSpace and _metaskeleton.

gilwoolee · 2018-01-09T21:37:46Z

src/constraint/FramePairDifferentiable.cpp

 {
+  if (!mMetaSkeletonStateSpace)


Check compatibility between statespace and metaskeleton.

gilwoolee · 2018-01-09T21:44:10Z

src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp

@@ -13,6 +13,7 @@ namespace vectorfield {
 //==============================================================================
 MoveEndEffectorOffsetVectorField::MoveEndEffectorOffsetVectorField(
    aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace,
+    dart::dynamics::MetaSkeletonPtr metaskeleton,


Check for compatibility between statespace and metaskeleton.

gilwoolee · 2018-01-09T21:45:01Z

src/planner/vectorfield/VectorFieldPlanner.cpp

@@ -192,6 +195,7 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorPose(
    const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
+    dart::dynamics::MetaSkeletonPtr metaskeleton,


Check for compatibility.

* Update changelog; Move #278 to 0.3.0 * Pass DART joint as const variable to state spaces * Use BallJoint::convertToPositions()

brianhou · 2018-01-10T19:26:30Z

@jslee02 Merging master (with the TrajectoryMarker introduced in #288) causes the build to fail. Can you investigate? Part of the issue is because MetaSkeletonStateSpaceSaver was changed to MetaSkeletonStateSaver. Also, it seems that the MetaSkeleton within TrajectoryMarker shouldn't be const since we change it in TrajectoryMarker::updatePoints.

* Refactor JointStateSpace to remove Joint. This decouples the state space from the underlying DART Joint that was used to generate the state space. Joint properties (e.g. position limits) are maintained in a separate JointStateSpace::Properties field, which is not tied to the original DART Joint. This also changes the interfaces for the JointStateSpace::getState and JointStateSpace::setState methods to take in a DART Joint. * Rename MetaSkeletonStateSpaceSaver to MetaSkeletonStateSaver. Since MetaSkeletonStateSpaces won't have an internal MetaSkeleton, having a MetaSkeletonStateSpaceSaver doesn't make sense. * Refactor MetaSkeletonStateSpace. This decouples the state space from the underlying DART MetaSkeleton that was used to generate the state space. Properties are maintained in a separate MetaSkeletonStateSpace::Properties field. This changes the interfaces for the MetaSkeletonStateSpace::getState and MetaSkeletonStateSpace::setState methods to take in a DART MetaSkeleton. This also changes the interfaces for many of the constraints and executors to take in an additional DART MetaSkeleton argument. The VectorFieldPlanner code is in a separate commit so it can be easily reverted when the improved VectorFieldPlanner is implemented. * Refactor VectorFieldPlanner to work with new MetaSkeletonStateSpaces. One of the VectorFieldPlanner tests is currently failing, but I figured it's not worth debugging since the improved VectorFieldPlanner will be ready soon. This is in a separate commit so it can be easily reverted when the improved implementation is ready. * Minor formatting fixes. * Add hash function for std::pair. * Address Mike's review comments. Return const & rather than const. * Address more of Mike's review comments. * Revert "Refactor VectorFieldPlanner to work with new MetaSkeletonStateSpaces." This reverts commit 934ee62. * Refactor new VectorFieldPlanner to work with new MetaSkeletonStateSpaces. * Update CHANGELOG.md * Pass DART objects as const to state spaces (#297) * Update changelog; Move #278 to 0.3.0 * Pass DART joint as const variable to state spaces * Use BallJoint::convertToPositions() * Address Gilwoo's review comments. * Update TrajectoryMarker per DART state space refactor * Fix code style

brianhou added 4 commits December 7, 2017 14:43

Rename MetaSkeletonStateSpaceSaver to MetaSkeletonStateSaver.

4ea924a

Since MetaSkeletonStateSpaces won't have an internal MetaSkeleton, having a MetaSkeletonStateSpaceSaver doesn't make sense.

brianhou requested review from mkoval, psigen and gilwoolee December 8, 2017 07:02

Merge branch 'master' into enhancement/brianhou/dart-statespaces

49228e3

brianhou and others added 2 commits December 8, 2017 01:45

Minor formatting fixes.

8b46852

Merge branch 'master' into enhancement/brianhou/dart-statespaces

78a2487

brianhou mentioned this pull request Dec 8, 2017

Add methods for converting trajectories between different (but equivalent) state spaces #249

Closed

5 tasks

jslee02 mentioned this pull request Dec 8, 2017

Fix const correctness in multiple places #282

Closed

mkoval approved these changes Dec 12, 2017

View reviewed changes

jslee02 added this to the Aikido 0.3.0 milestone Dec 15, 2017

brianhou added 7 commits January 6, 2018 18:53

Add hash function for std::pair.

03ad841

Address Mike's review comments.

a5b7d14

Return const & rather than const.

Address more of Mike's review comments.

845adf5

Revert "Refactor VectorFieldPlanner to work with new MetaSkeletonStat…

5c88794

…eSpaces." This reverts commit 934ee62.

Merge branch 'master' into enhancement/brianhou/dart-statespaces

2bd0e50

Conflicts: src/planner/vectorfield/VectorFieldPlanner.cpp tests/constraint/test_CollisionFree.cpp tests/planner/ompl/OMPLTestHelpers.hpp

Refactor new VectorFieldPlanner to work with new MetaSkeletonStateSpa…

6ea4961

…ces.

Update CHANGELOG.md

779e323

jslee02 self-requested a review January 8, 2018 18:58

Merge branch 'master' into enhancement/brianhou/dart-statespaces

ed3dcdc

jslee02 added a commit that referenced this pull request Jan 9, 2018

Update changelog; Move #278 to 0.3.0

342605f

jslee02 reviewed Jan 9, 2018

View reviewed changes

gilwoolee suggested changes Jan 9, 2018

View reviewed changes

jslee02 and others added 2 commits January 10, 2018 09:19

Merge branch 'master' into enhancement/brianhou/dart-statespaces

e3d4396

Pass DART objects as const to state spaces (#297)

925a4b7

* Update changelog; Move #278 to 0.3.0 * Pass DART joint as const variable to state spaces * Use BallJoint::convertToPositions()

brianhou and others added 3 commits January 10, 2018 11:49

Address Gilwoo's review comments.

c82aaea

Update TrajectoryMarker per DART state space refactor

fbe76a8

Fix code style

0fe489c

brianhou merged commit 99f6866 into master Jan 11, 2018

brianhou deleted the enhancement/brianhou/dart-statespaces branch January 11, 2018 23:09

jslee02 mentioned this pull request Jan 12, 2018

Pass MetaSkeleton as const reference to MetaSkeletonStateSpace #298

Open

brianhou mentioned this pull request Feb 22, 2018

Support controlling subsets of a Skeleton #347

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactor DART StateSpaces #278

Refactor DART StateSpaces #278

brianhou commented Dec 8, 2017 •

edited

Loading

codecov bot commented Dec 8, 2017 •

edited

Loading

mkoval left a comment

mkoval Dec 8, 2017

mkoval Dec 8, 2017

mkoval Dec 8, 2017

mkoval Dec 8, 2017

mkoval Dec 8, 2017

mkoval Dec 8, 2017

brianhou Jan 7, 2018

mkoval Dec 8, 2017

mkoval Dec 8, 2017

mkoval Dec 12, 2017

mkoval Dec 12, 2017

brianhou commented Jan 7, 2018

jslee02 left a comment •

edited

Loading

gilwoolee left a comment

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

gilwoolee Jan 9, 2018

brianhou commented Jan 10, 2018

Refactor DART StateSpaces #278

Refactor DART StateSpaces #278

Conversation

brianhou commented Dec 8, 2017 • edited Loading

codecov bot commented Dec 8, 2017 • edited Loading

Codecov Report

mkoval 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

brianhou commented Jan 7, 2018

jslee02 left a comment • edited Loading

Choose a reason for hiding this comment

gilwoolee 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

brianhou commented Jan 10, 2018

brianhou commented Dec 8, 2017 •

edited

Loading

codecov bot commented Dec 8, 2017 •

edited

Loading

jslee02 left a comment •

edited

Loading