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

* Make types play nice by changing old VFP code, but problem has now been pushed deeper into AIKIDO.

* Propograte const-ness to more of AIKIDO, but not done yet.

* Even more const-ing. Builds more, but still not all the way.

* Add const-ness to some OMPL stuff. Bumped issues back to VFP code.

* Make VFP Offset changes build. Needed to muck around with Robot class.

* Yet more const-ing to make tests build.

* Tests now build as well.

* Clean up createDistanceMetricFor_impl code.

* Add file left out from last commit

* Clean up JointStateSpaceHelpers code.

* Run `make format`.

* Respond to Brian's comments.

* Update CHANGELOG.md.

CHANGELOG.md

@@ -17,6 +17,7 @@
   * Fixed const correctness of StateHandle::getState(): [#419](https://github.com/personalrobotics/aikido/pull/419)
   * Fixed hidden compose function (in-place version): [#421](https://github.com/personalrobotics/aikido/pull/421)
   * Added clone functionality to StateSpace: [#422](https://github.com/personalrobotics/aikido/pull/422)
+  * Used const StateSpaces everywhere: [#429](https://github.com/personalrobotics/aikido/pull/429)
 
 * Constraint
 

include/aikido/constraint/CartesianProductProjectable.hpp

@@ -19,19 +19,19 @@ class CartesianProductProjectable : public Projectable
   ///        should match the number of subspaces in _stateSpace.
   ///        i-th constraint applies to i-th subspace.
   CartesianProductProjectable(
-      std::shared_ptr<statespace::CartesianProduct> _stateSpace,
+      std::shared_ptr<const statespace::CartesianProduct> _stateSpace,
       std::vector<ProjectablePtr> _constraints);
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   bool project(
       const statespace::StateSpace::State* _s,
       statespace::StateSpace::State* _out) const override;
 
 private:
-  std::shared_ptr<statespace::CartesianProduct> mStateSpace;
+  std::shared_ptr<const statespace::CartesianProduct> mStateSpace;
   std::vector<ProjectablePtr> mConstraints;
 };
 

include/aikido/constraint/CartesianProductSampleable.hpp

@@ -20,17 +20,17 @@ class CartesianProductSampleable : public Sampleable
   ///        should match the number of subspaces in _stateSpace.
   ///        i-th constraint applies to i-th subspace.
   CartesianProductSampleable(
-      std::shared_ptr<statespace::CartesianProduct> _stateSpace,
+      std::shared_ptr<const statespace::CartesianProduct> _stateSpace,
       std::vector<std::shared_ptr<Sampleable>> _constraints);
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
-  std::shared_ptr<statespace::CartesianProduct> mStateSpace;
+  std::shared_ptr<const statespace::CartesianProduct> mStateSpace;
   std::vector<std::shared_ptr<Sampleable>> mConstraints;
 };
 

include/aikido/constraint/CartesianProductTestable.hpp

@@ -20,10 +20,10 @@ class CartesianProductTestable : public Testable
   ///        should match the number of subspaces in _stateSpace.
   ///        i-th constraint applies to i-th subspace.
   CartesianProductTestable(
-      std::shared_ptr<statespace::CartesianProduct> _stateSpace,
-      std::vector<TestablePtr> _constraints);
+      std::shared_ptr<const statespace::CartesianProduct> _stateSpace,
+      std::vector<ConstTestablePtr> _constraints);
 
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   bool isSatisfied(
       const aikido::statespace::StateSpace::State* _state,
@@ -34,8 +34,8 @@ class CartesianProductTestable : public Testable
   std::unique_ptr<TestableOutcome> createOutcome() const override;
 
 private:
-  std::shared_ptr<statespace::CartesianProduct> mStateSpace;
-  std::vector<TestablePtr> mConstraints;
+  std::shared_ptr<const statespace::CartesianProduct> mStateSpace;
+  std::vector<ConstTestablePtr> mConstraints;
 };
 
 } // namespace constraint

include/aikido/constraint/CyclicSampleable.hpp

@@ -20,14 +20,14 @@ class CyclicSampleable : public Sampleable
   explicit CyclicSampleable(SampleablePtr _sampleable);
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
   SampleablePtr mSampleable;
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
 };
 
 } // namespace constraint

include/aikido/constraint/Differentiable.hpp

@@ -28,7 +28,7 @@ class Differentiable
   virtual ~Differentiable() = default;
 
   /// Gets the StateSpace that this constraint operates on.
-  virtual statespace::StateSpacePtr getStateSpace() const = 0;
+  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;
 
   /// Returns a vector of constraints' types, i-th element correspoinding to
   /// the type of i-th constraint.

include/aikido/constraint/DifferentiableIntersection.hpp

@@ -16,7 +16,7 @@ class DifferentiableIntersection : public Differentiable
 public:
   DifferentiableIntersection(
       std::vector<DifferentiablePtr> _constraints,
-      statespace::StateSpacePtr _stateSpace);
+      statespace::ConstStateSpacePtr _stateSpace);
 
   // Documentation inherited.
   std::size_t getConstraintDimension() const override;
@@ -34,7 +34,7 @@ class DifferentiableIntersection : public Differentiable
   std::vector<ConstraintType> getConstraintTypes() const override;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   void getValueAndJacobian(
@@ -44,7 +44,7 @@ class DifferentiableIntersection : public Differentiable
 
 private:
   std::vector<DifferentiablePtr> mConstraints;
-  std::shared_ptr<aikido::statespace::StateSpace> mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
 };
 
 } // namespace constraint

include/aikido/constraint/DifferentiableSubspace.hpp

@@ -17,14 +17,14 @@ class DifferentiableSubspace : public Differentiable
   /// \param _constraint Constraint being applied.
   /// \param _index Subspace of _stateSpace to apply _constraint.
   DifferentiableSubspace(
-      std::shared_ptr<statespace::CartesianProduct> _stateSpace,
+      std::shared_ptr<const statespace::CartesianProduct> _stateSpace,
       DifferentiablePtr _constraint,
       std::size_t _index);
 
   virtual ~DifferentiableSubspace() = default;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::vector<ConstraintType> getConstraintTypes() const override;
@@ -48,7 +48,7 @@ class DifferentiableSubspace : public Differentiable
       Eigen::MatrixXd& _jac) const override;
 
 private:
-  std::shared_ptr<statespace::CartesianProduct> mStateSpace;
+  std::shared_ptr<const statespace::CartesianProduct> mStateSpace;
   DifferentiablePtr mConstraint;
   std::size_t mIndex;
 };

include/aikido/constraint/FiniteSampleable.hpp

@@ -36,13 +36,13 @@ class FiniteSampleable : public Sampleable
   virtual ~FiniteSampleable();
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
   std::vector<statespace::StateSpace::State*> mStates;
 };
 

include/aikido/constraint/NewtonsMethodProjectable.hpp

@@ -33,14 +33,14 @@ class NewtonsMethodProjectable : public Projectable
       statespace::StateSpace::State* _out) const override;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
 private:
   DifferentiablePtr mDifferentiable;
   std::vector<double> mTolerance;
   int mMaxIteration;
   double mMinStepSize;
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
 
   bool contains(const statespace::StateSpace::State* _s) const;
 };

include/aikido/constraint/Projectable.hpp

@@ -14,7 +14,7 @@ class Projectable
   virtual ~Projectable() = default;
 
   /// Gets the StateSpace that this constraint operates on.
-  virtual statespace::StateSpacePtr getStateSpace() const = 0;
+  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;
 
   /// Projection _s to _out. Returns false if projection cannot be done.
   /// \param _s state to be projected.

include/aikido/constraint/RejectionSampleable.hpp

@@ -30,13 +30,13 @@ class RejectionSampleable : public Sampleable
       int _maxTrialPerSample);
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
   SampleablePtr mSampleable;
   TestablePtr mTestable;
   int mMaxTrialPerSample;

include/aikido/constraint/Sampleable.hpp

@@ -29,7 +29,7 @@ class Sampleable
   virtual ~Sampleable() = default;
 
   /// Gets the StateSpace that this constraint operates on.
-  virtual statespace::StateSpacePtr getStateSpace() const = 0;
+  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;
 
   /// Creates a SampleGenerator for sampling from this constraint.
   virtual std::unique_ptr<SampleGenerator> createSampleGenerator() const = 0;
@@ -49,7 +49,7 @@ class SampleGenerator
   static constexpr int NO_LIMIT = std::numeric_limits<int>::max();
 
   /// Gets the StateSpace that this SampleGenerator samples from.
-  virtual statespace::StateSpacePtr getStateSpace() const = 0;
+  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;
 
   /// Returns one sample from this constraint; returns true if succeeded.
   virtual bool sample(statespace::StateSpace::State* _state) = 0;

include/aikido/constraint/Satisfied.hpp

@@ -20,10 +20,10 @@ class Satisfied : public constraint::Differentiable,
 
   /// Constructor.
   /// \param _space StateSpace in which this constraint operates.
-  explicit Satisfied(statespace::StateSpacePtr _space);
+  explicit Satisfied(statespace::ConstStateSpacePtr _space);
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   /// Returns \c 0.
   std::size_t getConstraintDimension() const override;
@@ -68,7 +68,7 @@ class Satisfied : public constraint::Differentiable,
       Eigen::MatrixXd& _out) const override;
 
 private:
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
 };
 
 } // namespace constraint

include/aikido/constraint/SequentialSampleable.hpp

@@ -19,15 +19,14 @@ class SequentialSampleable : public Sampleable
       const std::vector<ConstSampleablePtr>& sampleables);
 
   // Documentation inherited.
-  // TODO (avk): const-correctness after planner API is merged.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   // Documentation inherited.
   std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
 private:
   /// StateSpace in which the constraint operates.
-  statespace::StateSpacePtr mStateSpace;
+  statespace::ConstStateSpacePtr mStateSpace;
 
   /// Sequence of sampleables.
   const std::vector<ConstSampleablePtr> mSampleables;

include/aikido/constraint/Testable.hpp

@@ -30,7 +30,7 @@ class Testable
       TestableOutcome* outcome = nullptr) const = 0;
 
   /// Returns StateSpace in which this constraint operates.
-  virtual statespace::StateSpacePtr getStateSpace() const = 0;
+  virtual statespace::ConstStateSpacePtr getStateSpace() const = 0;
 
   /// Return an instance of a TestableOutcome derivative class that corresponds
   /// to this constraint class. Ensures that correct outcome object is passed

include/aikido/constraint/TestableIntersection.hpp

@@ -20,8 +20,9 @@ class TestableIntersection : public Testable
   /// \param _stateSpace StateSpace this constraint operates in.
   /// \param _constraints Set of constraints.
   TestableIntersection(
-      statespace::StateSpacePtr _stateSpace,
-      std::vector<TestablePtr> _constraints = std::vector<TestablePtr>());
+      statespace::ConstStateSpacePtr _stateSpace,
+      std::vector<ConstTestablePtr> _constraints
+      = std::vector<ConstTestablePtr>());
 
   // Documentation inherited.
   bool isSatisfied(
@@ -33,18 +34,18 @@ class TestableIntersection : public Testable
   std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   /// Add a Testable to the conjunction.
   /// \param constraint a constraint in the same StateSpace as the
   ///        TestableIntersection was initialize with.
-  void addConstraint(TestablePtr constraint);
+  void addConstraint(ConstTestablePtr constraint);
 
 private:
-  statespace::StateSpacePtr mStateSpace;
-  std::vector<TestablePtr> mConstraints;
+  statespace::ConstStateSpacePtr mStateSpace;
+  std::vector<ConstTestablePtr> mConstraints;
 
-  void testConstraintStateSpaceOrThrow(const TestablePtr& constraint);
+  void testConstraintStateSpaceOrThrow(const ConstTestablePtr& constraint);
 };
 
 } // namespace constraint

include/aikido/constraint/dart/CollisionFree.hpp

@@ -35,7 +35,7 @@ class CollisionFree : public Testable
   /// \param _collisionDetector collision detector used to test for collision
   /// \param _collisionOptions options passed to \c _collisionDetector
   CollisionFree(
-      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      statespace::dart::ConstMetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
       ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
       std::shared_ptr<::dart::collision::CollisionDetector> _collisionDetector,
       ::dart::collision::CollisionOption _collisionOptions
@@ -45,7 +45,7 @@ class CollisionFree : public Testable
           std::make_shared<::dart::collision::BodyNodeCollisionFilter>()));
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
   /// \copydoc Testable::isSatisfied()
   /// \note Outcome is expected to be an instance of CollisionFreeOutcome.
@@ -85,7 +85,8 @@ class CollisionFree : public Testable
 private:
   using CollisionGroup = ::dart::collision::CollisionGroup;
 
-  aikido::statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
+      mMetaSkeletonStateSpace;
   ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   std::shared_ptr<::dart::collision::CollisionDetector> mCollisionDetector;
   ::dart::collision::CollisionOption mCollisionOptions;

include/aikido/constraint/dart/FrameDifferentiable.hpp

@@ -62,10 +62,10 @@ class FrameDifferentiable : public Differentiable
   std::vector<ConstraintType> getConstraintTypes() const override;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
 private:
-  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
   ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   ::dart::dynamics::ConstJacobianNodePtr mJacobianNode;
   DifferentiablePtr mPoseConstraint;

include/aikido/constraint/dart/FramePairDifferentiable.hpp

@@ -57,10 +57,10 @@ class FramePairDifferentiable : public Differentiable
   std::vector<ConstraintType> getConstraintTypes() const override;
 
   // Documentation inherited.
-  statespace::StateSpacePtr getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
 private:
-  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
   ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   ::dart::dynamics::ConstJacobianNodePtr mJacobianNode1;
   ::dart::dynamics::ConstJacobianNodePtr mJacobianNode2;

include/aikido/constraint/dart/FrameTestable.hpp

@@ -45,14 +45,14 @@ class FrameTestable : public Testable
   std::unique_ptr<TestableOutcome> createOutcome() const override;
 
   // Documentation inherited
-  std::shared_ptr<statespace::StateSpace> getStateSpace() const override;
+  statespace::ConstStateSpacePtr getStateSpace() const override;
 
 private:
-  statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
   ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
   ::dart::dynamics::ConstJacobianNodePtr mFrame;
   TestablePtr mPoseConstraint;
-  std::shared_ptr<statespace::SE3> mPoseStateSpace;
+  std::shared_ptr<const statespace::SE3> mPoseStateSpace;
 };
 
 } // namespace dart