Merge branch 'master' into bugfix/avk/so2

CHANGELOG.md

@@ -8,6 +8,15 @@
   * Fixed bug in StepSequence iterator: [#320](https://github.com/personalrobotics/aikido/pull/320)
   * Cleaned up doxygen errors: [#357](https://github.com/personalrobotics/aikido/pull/357)
 
+* State Space
+
+  * Refactored JointStateSpace and MetaSkeletonStateSpace: [#278](https://github.com/personalrobotics/aikido/pull/278)
+  * Added methods for checking compatibility between DART objects and state spaces: [#315](https://github.com/personalrobotics/aikido/pull/315)
+  * Added flags to MetaSkeletonStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
+  * Fixed bug in the representation of SO(2) statespace: [#408](https://github.com/personalrobotics/aikido/pull/408)
+  * 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)
+
 * Constraint
 
   * Added SequentialSampleable: [#393](https://github.com/personalrobotics/aikido/pull/393)
@@ -19,13 +28,6 @@
   * Introduced uniform and dart namespaces: [#342](https://github.com/personalrobotics/aikido/pull/342)
   * Removed Barrett-specific hand executors: [#380](https://github.com/personalrobotics/aikido/pull/380)
 
-* State Space
-
-  * Refactored JointStateSpace and MetaSkeletonStateSpace: [#278](https://github.com/personalrobotics/aikido/pull/278)
-  * Added methods for checking compatibility between DART objects and state spaces: [#315](https://github.com/personalrobotics/aikido/pull/315)
-  * Added flags to MetaSkeletonStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
-  * Fixed bug in the representation of SO(2) statespace: [#408](https://github.com/personalrobotics/aikido/pull/408)
-
 * Perception
 
   * Added integrated PoseEstimatorModule: [#336](https://github.com/personalrobotics/aikido/pull/336)

include/aikido/statespace/CartesianProduct.hpp

@@ -16,22 +16,16 @@ class CartesianProduct : public std::enable_shared_from_this<CartesianProduct>,
                          public virtual StateSpace
 {
 public:
-  /// A tuple of states where the i-th state is from the i-th subspace.
-  class State : public StateSpace::State
-  {
-  protected:
-    State() = default;
-    ~State() = default;
-
-    friend class CartesianProduct;
-  };
+  class State;
 
   using StateHandle = CompoundStateHandle<State>;
   using StateHandleConst = CompoundStateHandle<const State>;
 
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   /// Construct the Cartesian product of a vector of subspaces.
   /// \param _subspaces vector of subspaces
   explicit CartesianProduct(std::vector<StateSpacePtr> _subspaces);
@@ -157,6 +151,17 @@ class CartesianProduct : public std::enable_shared_from_this<CartesianProduct>,
   std::size_t mSizeInBytes;
 };
 
+/// A tuple of states where the i-th state is from the i-th subspace.
+class CartesianProduct::State : public StateSpace::State
+{
+protected:
+  friend class CartesianProduct;
+
+  State() = default;
+
+  ~State() = default;
+};
+
 } // namespace statespace
 } // namespace aikido
 

include/aikido/statespace/Rn.hpp

@@ -41,6 +41,8 @@ class R : public virtual StateSpace
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   /// Constructs a \c N dimensional real vector space only when the dimension is
   /// can be known in compile time.
   ///

include/aikido/statespace/SE2.hpp

@@ -56,6 +56,8 @@ class SE2 : public virtual StateSpace
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   using Isometry2d = State::Isometry2d;
 
   /// Constructs a state space representing SE(2).

include/aikido/statespace/SE3.hpp

@@ -56,6 +56,8 @@ class SE3 : public virtual StateSpace
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   using Isometry3d = State::Isometry3d;
 
   /// Constructs a state space representing SE(3).

include/aikido/statespace/SO2.hpp

@@ -26,6 +26,8 @@ class SO2 : virtual public StateSpace
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   /// Constructs a state space representing SO(2).
   SO2() = default;
 

include/aikido/statespace/SO3.hpp

@@ -54,6 +54,8 @@ class SO3 : virtual public StateSpace
   using ScopedState = statespace::ScopedState<StateHandle>;
   using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
+  using StateSpace::compose;
+
   using Quaternion = State::Quaternion;
 
   /// Constructs a state space representing SO(3).

include/aikido/statespace/StateHandle.hpp

@@ -1,31 +1,38 @@
 #ifndef AIKIDO_STATESPACE_STATEHANDLE_HPP_
 #define AIKIDO_STATESPACE_STATEHANDLE_HPP_
 
+#include <type_traits>
+
 namespace aikido {
 namespace statespace {
 
 /// Wrap a State with its StateSpace to provide convenient accessor methods.
 /// The template parameter \c _QualifiedState is necessary to support both
 /// \c const and non-<tt>const</tt> states.
 ///
-/// \tparam _StateSpace type of \c StateSpace this state is a member of
-/// \tparam _QualifiedState type of \c State being wrapped
+/// \tparam _StateSpace Type of \c StateSpace this state is a member of
+/// \tparam _QualifiedState Type of \c State being wrapped
 template <class _StateSpace, class _QualifiedState>
 class StateHandle
 {
 public:
   using StateSpace = _StateSpace;
-  using State = typename StateSpace::State;
   using QualifiedState = _QualifiedState;
 
+  using State = typename StateSpace::State;
+  using ConstState =
+      typename std::conditional<std::is_const<QualifiedState>::value,
+                                QualifiedState,
+                                const QualifiedState>::type;
+
   /// Constructs a nullptr handle.
   StateHandle();
 
   /// Wrap state, which must be form the provided StateSpace.
   ///
-  /// \param _space state space that created \c _state
-  /// \param _state state created by \c _space
-  StateHandle(const StateSpace* _space, QualifiedState* _state);
+  /// \param space State space that created \c state.
+  /// \param state State created by \c space.
+  StateHandle(const StateSpace* space, QualifiedState* state);
 
   StateHandle(const StateHandle&) = default;
   StateHandle(StateHandle&&) = default;
@@ -36,27 +43,42 @@ class StateHandle
   /// Implicitly convert to a \c State pointer.
   operator QualifiedState*() const;
 
-  /// Reset StateHandle to nullptr.
+  /// Resets StateHandle to nullptr.
   void reset();
 
-  /// Reset the state, which must be from the provided StateSpace.
+  /// Resets the state, which must be from the provided StateSpace.
   ///
-  /// \param _space state space that created \c _state
-  /// \param _state state created by \c _space
-  void reset(const StateSpace* _space, QualifiedState* _state);
+  /// \param space State space that created \c state.
+  /// \param state State created by \c space.
+  void reset(const StateSpace* space, QualifiedState* state);
 
-  /// Gets the State.
+  /// Returns the State. This function is enabled only if QualifiedState is a
+  /// non-const State type.
   ///
   /// \return state wrapped by this handle
-  QualifiedState* getState() const;
+  template <typename Q = QualifiedState>
+  auto getState() ->
+      typename std::enable_if<!std::is_const<Q>::value, Q*>::type;
+  // Note: We don't define non-const function for const State type because it
+  // violates const-correctness.
 
-  /// Gets the state space that created this state.
+  /// Returns the State.
   ///
-  /// \return state space created this state
+  /// \return State wrapped by this handle
+  template <typename Q = QualifiedState>
+  auto getState() const ->
+      typename std::conditional<std::is_const<Q>::value, Q*, const Q*>::type;
+
+  /// Returns the state space that created this state.
+  ///
+  /// \return State space created this state
   const StateSpace* getStateSpace() const;
 
 protected:
+  /// State space of the sate that is managed by this handler.
   const StateSpace* mSpace;
+
+  /// State managed by this handler. This can be either const or non-const type.
   QualifiedState* mState;
 };
 
@@ -65,4 +87,4 @@ class StateHandle
 
 #include "detail/StateHandle-impl.hpp"
 
-#endif // ifndef AIKIDO_STATESPACE_STATEHANDLE_HPP_
+#endif // AIKIDO_STATESPACE_STATEHANDLE_HPP_

include/aikido/statespace/StateSpace.hpp

@@ -32,17 +32,7 @@ class StateSpace
 {
 public:
   /// Base class for all States.
-  class State
-  {
-  protected:
-    /// This is a base class that should only only be used in derived classes.
-    State() = default;
-
-    /// It is unsafe to call this, since it is a non-virtual destructor.  Having
-    /// any virtual function in this class, including this destructor, adds
-    /// sizeof(pointer) overhead for a vtable.
-    ~State() = default;
-  };
+  class State;
 
   using StateHandle = statespace::StateHandle<StateSpace, State>;
   using StateHandleConst = statespace::StateHandle<StateSpace, const State>;
@@ -169,6 +159,18 @@ class StateSpace
   virtual void print(const State* _state, std::ostream& _os) const = 0;
 };
 
+class StateSpace::State
+{
+protected:
+  /// This is a base class that should only only be used in derived classes.
+  State() = default;
+
+  /// It is unsafe to call this, since it is a non-virtual destructor.  Having
+  /// any virtual function in this class, including this destructor, adds
+  /// sizeof(pointer) overhead for a vtable.
+  ~State() = default;
+};
+
 } // namespace statespace
 } // namespace aikido
 

include/aikido/statespace/detail/CartesianProduct-impl.hpp

@@ -1,3 +1,5 @@
+#include "aikido/statespace/CartesianProduct.hpp"
+
 #include <sstream>
 
 namespace aikido {
@@ -20,6 +22,7 @@ class CompoundStateHandle
   /// Construct and initialize to \c nullptr.
   CompoundStateHandle()
   {
+    // Do nothing
   }
 
   /// Construct a handle for \c _state in \c _space.
@@ -29,6 +32,19 @@ class CompoundStateHandle
   CompoundStateHandle(const StateSpace* _space, State* _state)
     : statespace::StateHandle<CartesianProduct, QualifiedState>(_space, _state)
   {
+    // Do nothing
+  }
+
+  /// Gets state by subspace index.
+  ///
+  /// \tparam Space type of \c StateSpace for subspace \c _index
+  /// \param _index in the range [ 0, \c getNumSubspaces() ]
+  /// \return state at \c _index
+  template <class Space = statespace::StateSpace>
+  typename Space::State* getSubState(std::size_t _index)
+  {
+    return this->getStateSpace()->template getSubState<Space>(
+        this->getState(), _index);
   }
 
   /// Gets state by subspace index.
@@ -37,7 +53,7 @@ class CompoundStateHandle
   /// \param _index in the range [ 0, \c getNumSubspaces() ]
   /// \return state at \c _index
   template <class Space = statespace::StateSpace>
-  typename Space::State* getSubState(std::size_t _index) const
+  const typename Space::State* getSubState(std::size_t _index) const
   {
     return this->getStateSpace()->template getSubState<Space>(
         this->getState(), _index);
@@ -50,12 +66,26 @@ class CompoundStateHandle
   /// \param _index in the range [ 0, \c getNumSubspaces() ]
   /// \return state at \c _index
   template <class Space = statespace::StateSpace>
-  typename Space::StateHandle getSubStateHandle(std::size_t _index) const
+  typename Space::StateHandle getSubStateHandle(std::size_t _index)
   {
     return typename Space::StateHandle(
         this->getStateSpace()->template getSubspace<Space>(_index).get(),
         getSubState<Space>(_index));
   }
+
+  /// Gets state by subspace index and wraps it in a \c Space::StateHandle
+  /// helper class.
+  ///
+  /// \tparam Space type of \c StateSpace for subspace \c _index
+  /// \param _index in the range [ 0, \c getNumSubspaces() ]
+  /// \return state at \c _index
+  template <class Space = statespace::StateSpace>
+  typename Space::StateHandleConst getSubStateHandle(std::size_t _index) const
+  {
+    return typename Space::StateHandleConst(
+        this->getStateSpace()->template getSubspace<Space>(_index).get(),
+        getSubState<Space>(_index));
+  }
 };
 
 //==============================================================================

include/aikido/statespace/detail/SE2-impl.hpp

@@ -38,7 +38,7 @@ class SE2StateHandle : public statespace::StateHandle<SE2, _QualifiedState>
   /// Sets value to an Eigen transfomation object.
   ///
   /// \param _transform Eigen transformation
-  void setIsometry(const Eigen::Isometry2d& _transform) const
+  void setIsometry(const Eigen::Isometry2d& _transform)
   {
     return this->getStateSpace()->setIsometry(this->getState(), _transform);
   }

include/aikido/statespace/detail/SE3-impl.hpp

@@ -38,7 +38,7 @@ class SE3StateHandle : public statespace::StateHandle<SE3, _QualifiedState>
   /// Gets value as an Eigen transformation object.
   ///
   /// \return Eigen trasnformation
-  void setIsometry(const Eigen::Isometry3d& _transform) const
+  void setIsometry(const Eigen::Isometry3d& _transform)
   {
     return this->getStateSpace()->setIsometry(this->getState(), _transform);
   }

include/aikido/statespace/detail/ScopedState-impl.hpp

@@ -1,17 +1,21 @@
+#include "aikido/statespace/ScopedState.hpp"
+
 namespace aikido {
 namespace statespace {
 
-template <class _Handle>
-ScopedState<_Handle>::ScopedState(const StateSpace* _space)
+//==============================================================================
+template <class Handle>
+ScopedState<Handle>::ScopedState(const StateSpace* _space)
 {
   this->mSpace = _space;
   mBuffer.reset(new char[_space->getStateSizeInBytes()]);
   this->mState = static_cast<ScopedState::State*>(
       _space->allocateStateInBuffer(mBuffer.get()));
 }
 
-template <class _Handle>
-ScopedState<_Handle>::~ScopedState()
+//==============================================================================
+template <class Handle>
+ScopedState<Handle>::~ScopedState()
 {
   this->mSpace->freeStateInBuffer(this->mState);
 }