fix geodesic for SO(2)

include/aikido/statespace/SO2.hpp

@@ -18,44 +18,7 @@ class SO2 : virtual public StateSpace
 {
 public:
   /// State in SO(2), a planar rotation.
-  class State : public StateSpace::State
-  {
-  public:
-    /// Constructs the identity element.
-    State();
-
-    ~State() = default;
-
-    /// Constructs a state from a rotation angle.
-    ///
-    /// \param _angle rotation angle
-    explicit State(double _angle);
-
-    /// Gets state as a rotation angle.
-    ///
-    /// \return rotation angle
-    double getAngle() const;
-
-    /// Sets state to a rotation angle.
-    ///
-    /// \param _angle rotation angle
-    void setAngle(double _angle);
-
-    /// Gets state as an Eigen transformation.
-    ///
-    /// \return Eigen transformation
-    Eigen::Rotation2Dd getRotation() const;
-
-    /// Sets state it an Eigen transformation.
-    ///
-    /// \param _rotation Eigen transformation
-    void setRotation(const Eigen::Rotation2Dd& _rotation);
-
-  private:
-    double mAngle;
-
-    friend class SO2;
-  };
+  class State;
 
   using StateHandle = SO2StateHandle<State>;
   using StateHandleConst = SO2StateHandle<const State>;
@@ -68,81 +31,112 @@ class SO2 : virtual public StateSpace
 
   /// Helper function to create a \c ScopedState.
   ///
-  /// \return new \c ScopedState
+  /// \return new \c ScopedState.
   ScopedState createState() const;
 
-  /// Gets state as a rotation angle.
+  /// Returns state as a rotation angle in (-pi, pi].
   ///
-  /// \param _state input state
-  /// \return rotation angle
-  double getAngle(const State* _state) const;
+  /// \param[in] state State.
+  double toAngle(const State* state) const;
 
-  /// Sets state to a rotation angle.
+  /// Sets state from a rotation angle in (-inf, inf).
   ///
-  /// \param _state input state
-  /// \param _angle rotation angle
-  void setAngle(State* _state, double _angle) const;
+  /// \param[out] state State corresponding to angle.
+  /// \param[in] angle Rotation angle in (-inf, inf).
+  void fromAngle(State* state, double angle) const;
 
-  /// Gets state as an Eigen transformation.
+  /// Returns state as an Eigen rotation.
   ///
-  /// \param _state input state
-  /// \return Eigen transformation
-  Eigen::Rotation2Dd getRotation(const State* _state) const;
+  /// \param[in] state State.
+  Eigen::Rotation2Dd toRotation(const State* state) const;
 
-  /// Sets state it an Eigen transformation.
+  /// Sets state from an Eigen rotation.
   ///
-  /// \param _state input state
-  /// \param _rotation Eigen transformation
-  void setRotation(State* _state, const Eigen::Rotation2Dd& _rotation) const;
+  /// \param[out] state State corresponding to rotation.
+  /// \param[in] rotation Eigen rotation.
+  void fromRotation(State* state, const Eigen::Rotation2Dd& rotation) const;
 
   // Documentation inherited.
   std::size_t getStateSizeInBytes() const override;
 
   // Documentation inherited.
-  StateSpace::State* allocateStateInBuffer(void* _buffer) const override;
+  StateSpace::State* allocateStateInBuffer(void* buffer) const override;
 
   // Documentation inherited.
-  void freeStateInBuffer(StateSpace::State* _state) const override;
+  void freeStateInBuffer(StateSpace::State* state) const override;
 
   // Documentation inherited.
   void compose(
-      const StateSpace::State* _state1,
-      const StateSpace::State* _state2,
-      StateSpace::State* _out) const override;
+      const StateSpace::State* state1,
+      const StateSpace::State* state2,
+      StateSpace::State* out) const override;
 
   // Documentation inherited.
-  void getIdentity(StateSpace::State* _out) const override;
+  void getIdentity(StateSpace::State* out) const override;
 
   // Documentation inherited.
   void getInverse(
-      const StateSpace::State* _in, StateSpace::State* _out) const override;
+      const StateSpace::State* in, StateSpace::State* out) const override;
 
   // Documentation inherited.
   std::size_t getDimension() const override;
 
   // Documentation inherited.
   void copyState(
-      const StateSpace::State* _source,
-      StateSpace::State* _destination) const override;
+      const StateSpace::State* source,
+      StateSpace::State* destination) const override;
 
   /// Exponential mapping of Lie algebra element to a Lie group element. The
   /// tangent space is parameterized a rotation angle.
   ///
-  /// \param _tangent element of the tangent space
-  /// \param[out] _out corresponding element of the Lie group
+  /// \param[in] tangent Element of the tangent space.
+  /// \param[out] out Corresponding element of the Lie group.
   void expMap(
-      const Eigen::VectorXd& _tangent, StateSpace::State* _out) const override;
+      const Eigen::VectorXd& tangent, StateSpace::State* out) const override;
 
   /// Log mapping of Lie group element to a Lie algebra element. The tangent
   /// space is parameterized as a rotation angle.
   ///
-  /// \param _in element of this Lie group
-  /// \param[out] _tangent corresponding element of the tangent space
+  /// \param[in] in Element of this Lie group.
+  /// \param[out] tangent Corresponding element of the tangent space.
   void logMap(
-      const StateSpace::State* _in, Eigen::VectorXd& _tangent) const override;
+      const StateSpace::State* in, Eigen::VectorXd& tangent) const override;
+
+  /// Print the angle represented by the state.
+  void print(const StateSpace::State* state, std::ostream& os) const override;
+};
+
+class SO2::State final : public StateSpace::State
+{
+public:
+  /// Constructs a state from a rotation angle.
+  ///
+  /// \param[in] angle Rotation angle in (-inf, inf).
+  explicit State(double angle = 0.0);
+
+  ~State() = default;
+
+  /// Returns state as a rotation angle in (-pi, pi].
+  double toAngle() const;
+
+  /// Sets state from a rotation angle.
+  ///
+  /// \param[in] angle Rotation angle in (-inf, inf).
+  void fromAngle(double angle);
+
+  /// Returns state as an Eigen rotation.
+  Eigen::Rotation2Dd toRotation() const;
+
+  /// Sets state from an Eigen rotation.
+  ///
+  /// \param[in] rotation Eigen rotation.
+  void fromRotation(const Eigen::Rotation2Dd& rotation);
+
+private:
+  /// Angle bounded in (-pi, pi] representing the SO(2) state.
+  double mAngle;
 
-  /// Print the angle represented by the state
-  void print(const StateSpace::State* _state, std::ostream& _os) const override;
+  friend class SO2;
 };
 
 } // namespace statespace

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

@@ -27,36 +27,32 @@ class SO2StateHandle : public statespace::StateHandle<SO2, _QualifiedState>
   {
   }
 
-  /// Gets state as a rotation angle.
-  ///
-  /// \return rotation angle
-  double getAngle() const
+  /// Returns angle corresponding to the state.
+  double toAngle() const
   {
-    return this->getStateSpace()->getAngle(this->getState());
+    return this->getStateSpace()->toAngle(this->getState());
   }
 
-  /// Sets state to a rotation angle.
+  /// Sets state given a rotation angle.
   ///
-  /// \param _angle rotation angle
-  void setAngle(double _angle)
+  /// \param[in] angle rotation angle.
+  void fromAngle(double angle)
   {
-    return this->getStateSpace()->setAngle(this->getState(), _angle);
+    return this->getStateSpace()->fromAngle(this->getState(), angle);
   }
 
-  /// Gets state as an Eigen transformation.
-  ///
-  /// \return Eigen transformation
-  Eigen::Rotation2Dd getRotation() const
+  /// Returns the Eigen transformation corresponding to state.
+  Eigen::Rotation2Dd toRotation() const
   {
-    return this->getStateSpace()->getRotation(this->getState());
+    return this->getStateSpace()->toRotation(this->getState());
   }
 
-  /// Sets state it an Eigen transformation.
+  /// Sets state given an Eigen transformation.
   ///
-  /// \param _rotation Eigen transformation
-  void setRotation(const Eigen::Rotation2Dd& _rotation)
+  /// \param[in] rotation Eigen transformation.
+  void fromRotation(const Eigen::Rotation2Dd& rotation)
   {
-    return this->getStateSpace()->setRotation(this->getState(), _rotation);
+    return this->getStateSpace()->fromRotation(this->getState(), rotation);
   }
 };
 

src/constraint/uniform/SO2UniformSampler.cpp

@@ -51,7 +51,7 @@ statespace::StateSpacePtr SO2UniformSampleGenerator::getStateSpace() const
 bool SO2UniformSampleGenerator::sample(statespace::StateSpace::State* _state)
 {
   const double angle = mDistribution(*mRng);
-  mSpace->setAngle(static_cast<statespace::SO2::State*>(_state), angle);
+  mSpace->fromAngle(static_cast<statespace::SO2::State*>(_state), angle);
   return true;
 }
 

src/distance/SO2Angular.cpp

@@ -25,9 +25,9 @@ double SO2Angular::distance(
     const aikido::statespace::StateSpace::State* _state2) const
 {
   // Difference between angles
-  double diff = mStateSpace->getAngle(
+  double diff = mStateSpace->toAngle(
                     static_cast<const statespace::SO2::State*>(_state1))
-                - mStateSpace->getAngle(
+                - mStateSpace->toAngle(
                       static_cast<const statespace::SO2::State*>(_state2));
   diff = std::fmod(std::fabs(diff), 2.0 * M_PI);
   if (diff > M_PI)