Merge branch 'master' into feature/perceptionmodule

aikido/include/aikido/statespace/CartesianProduct.hpp

@@ -145,6 +145,12 @@ class CartesianProduct
   void logMap(const StateSpace::State *_in,
               Eigen::VectorXd &_tangent) const override;
 
+  /// Print the contents of each substate contained in the state
+  /// as a list with each substate enclosed in brackets and including its
+  /// index
+  /// Format: [0: ...] [1: ...] ... [n: ...]
+  void print(const StateSpace::State *_state, std::ostream &_os) const override;
+
 private:
   std::vector<StateSpacePtr> mSubspaces;
   std::vector<std::size_t> mOffsets;

aikido/include/aikido/statespace/Rn.hpp

@@ -99,6 +99,10 @@ class Rn : public virtual StateSpace
   void logMap(const StateSpace::State *_in,
               Eigen::VectorXd &_tangent) const override;
 
+  /// Print the n-dimensional vector represented by the state
+  /// Format: [x_1, x_2, ..., x_n]
+  void print(const StateSpace::State *_state, std::ostream &_os) const override;
+
 private:
   /// Gets the mutable value stored in a Rn::State. This is
   /// used internally to implement the public \c getValue member functions.

aikido/include/aikido/statespace/SE2.hpp

@@ -124,6 +124,9 @@ class SE2 : public virtual StateSpace
   /// \param[out] _tangent corresponding element of the tangent space
   void logMap(const StateSpace::State *_state,
               Eigen::VectorXd &_tangent) const override;
+
+  /// Print the state. Format: [x, y, theta]
+  void print(const StateSpace::State *_state, std::ostream &_os) const override;
 };
 
 }  // namespace statespace

aikido/include/aikido/statespace/SE3.hpp

@@ -124,6 +124,11 @@ class SE3 : public virtual StateSpace
   /// \param[out] _tangent corresponding element of the tangent space
   void logMap(const StateSpace::State *_in,
               Eigen::VectorXd &_tangent) const override;
+
+  /// Print the quaternion followed by the translation
+  /// Format: [q.w, q.x, q.y, q.z, x, y, z] where is the quaternion
+  /// representation of the rotational component of the state
+  void print(const StateSpace::State *_state, std::ostream &_os) const override;
 };
 
 } // namespace statespace

aikido/include/aikido/statespace/SO2.hpp

@@ -140,6 +140,8 @@ class SO2 : virtual public StateSpace
   void logMap(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;
 };
 
 }  // namespace statespace

aikido/include/aikido/statespace/SO3.hpp

@@ -120,6 +120,10 @@ class SO3 : virtual public StateSpace
   /// \param[out] _tangent corresponding element of the tangent space
   void logMap(const StateSpace::State *_in,
               Eigen::VectorXd &_tangent) const override;
+
+  /// Print the quaternion represented by the state. 
+  /// Format: [w, x, y, z]
+  void print(const StateSpace::State *_state, std::ostream &_os) const override;
 };
 
 } // namespace statespace

aikido/include/aikido/statespace/StateSpace.hpp

@@ -159,6 +159,11 @@ class StateSpace
   /// \param _state element of this Lie group
   /// \param[out] _tangent corresponding element of the tangent space
   virtual void logMap(const State *_in, Eigen::VectorXd &_tangent) const = 0;
+
+  /// Print the state to the output stream
+  /// \param _state The element to print
+  /// \param _os The stream to print to
+  virtual void print(const State *_state, std::ostream &_os) const = 0;
 };
 
 using StateSpacePtr = std::shared_ptr<StateSpace>;

aikido/src/statespace/CartesianProduct.cpp

@@ -174,5 +174,17 @@ void CartesianProduct::logMap(const StateSpace::State *_in,
   }
 }
 
+//=============================================================================
+void CartesianProduct::print(const StateSpace::State *_state,
+                             std::ostream &_os) const {
+
+  auto state = static_cast<const State *>(_state);
+  for (size_t i = 0; i < mSubspaces.size(); ++i) {
+    _os << "[ " << i << ":";
+    getSubspace<>(i)->print(getSubState<>(state, i), _os);
+    _os << " ] ";
+  }
+}
+
 }  // namespace statespace
 }  // namespace aikido

aikido/src/statespace/Rn.cpp

@@ -152,5 +152,14 @@ void Rn::logMap(const StateSpace::State *_in,
   _tangent = getValue(in);
 }
 
+//=============================================================================
+void Rn::print(const StateSpace::State *_state, std::ostream &_os) const 
+{
+    auto val = getValue(static_cast<const State*>(_state));
+
+    Eigen::IOFormat cleanFmt(3, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");
+    _os << val.format(cleanFmt);
+}
+
 }  // namespace statespace
 }  // namespace aikido

aikido/src/statespace/SE2.cpp

@@ -154,5 +154,19 @@ void SE2::logMap(const StateSpace::State* _in,
   _tangent(0) = rotation.angle();
 }
 
+//=============================================================================
+void SE2::print(const StateSpace::State *_state, std::ostream &_os) const
+{
+    auto state = static_cast<const State*>(_state);
+    auto transform = getIsometry(state);
+
+    Eigen::IOFormat cleanFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ",",
+                             ",", "", "", "[", "]");
+    Eigen::Rotation2Dd rotation = Eigen::Rotation2Dd::Identity();
+    rotation.fromRotationMatrix(transform.rotation());
+    _os << Eigen::Vector3d(transform.translation()[0],
+                           transform.translation()[1],
+                           rotation.angle()).format(cleanFmt);
+}
 }  // namespace statespace
 }  // namespace aikido

aikido/src/statespace/SE3.cpp

@@ -147,5 +147,21 @@ void SE3::logMap(const StateSpace::State *_in,
   _tangent = dart::math::logMap(transform);
 }
 
+//=============================================================================
+void SE3::print(const StateSpace::State *_state, std::ostream &_os) const
+{
+    auto state = static_cast<const State*>(_state);
+    auto transform = getIsometry(state);
+    auto t = transform.translation();
+
+    Eigen::VectorXd vals(7);
+    Eigen::Quaterniond quat(transform.rotation());
+    vals << quat.w(), quat.x(), quat.y(), quat.z(), t(0), t(1), t(2);
+
+    Eigen::IOFormat cleanFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ",",
+                             ",", "", "", "[", "]");
+    _os << vals.format(cleanFmt);
+}
+
 }  // namespace statespace
 }  // namespace aikido

aikido/src/statespace/SO2.cpp

@@ -163,5 +163,12 @@ void SO2::logMap(const StateSpace::State *_in,
   _tangent(0) = getAngle(in);
 }
 
+//=============================================================================
+void SO2::print(const StateSpace::State *_state, std::ostream &_os) const
+{
+    auto state = static_cast<const State*>(_state);
+    _os << "[" << getAngle(state) << "]";
+}
+
 }  // namespace statespace
 }  // namespace aikido

aikido/src/statespace/SO3.cpp

@@ -156,5 +156,17 @@ void SO3::logMap(const StateSpace::State *_in,
   _tangent = dart::math::logMap(rotMat);
 }
 
+//=============================================================================
+void SO3::print(const StateSpace::State *_state, std::ostream &_os) const
+{
+    auto state = static_cast<const State*>(_state);
+
+    Eigen::IOFormat cleanFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ",",
+                             ",", "", "", "[", "]");
+    auto quat = getQuaternion(state);
+    _os << Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()).format(cleanFmt);
+
+}
+
 }  // namespace statespace
 }  // namespace aikido

aikido/tests/statespace/test_CartesianProduct.cpp

@@ -3,11 +3,13 @@
 #include <aikido/statespace/Rn.hpp>
 #include <aikido/statespace/SO2.hpp>
 #include <aikido/statespace/SO3.hpp>
+#include <aikido/statespace/SE2.hpp>
 
 using aikido::statespace::CartesianProduct;
 using aikido::statespace::Rn;
 using aikido::statespace::SO2;
 using aikido::statespace::SO3;
+using aikido::statespace::SE2;
 
 TEST(CartesianProduct, Compose)
 {
@@ -155,3 +157,32 @@ TEST(CartesianProduct, CopyState)
   auto out3 = dest.getSubStateHandle<SO3>(2).getQuaternion();
   EXPECT_TRUE(out3.isApprox(quat));
 }
+
+TEST(CartesianProduct, PrintState)
+{
+  CartesianProduct space({
+      std::make_shared<SO2>(),
+      std::make_shared<Rn>(3),
+      std::make_shared<SO3>(),
+      std::make_shared<SE2>(),
+//      std::make_shared<SE3>(),
+  });
+
+  auto source = space.createState();
+
+  double angle = M_PI;
+  auto rv = Eigen::Vector3d(3, 4, 5);
+  auto quat =
+      Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
+  auto pose1 = Eigen::Isometry2d::Identity();
+  pose1.rotate(Eigen::Rotation2Dd(M_PI_2));
+  pose1.translation() << 2, 3;
+
+  source.getSubStateHandle<SO2>(0).setAngle(angle);
+  source.getSubStateHandle<Rn>(1).setValue(rv);
+  source.getSubStateHandle<SO3>(2).setQuaternion(quat);
+  source.getSubStateHandle<SE2>(3).setIsometry(pose1);
+
+  std::cout.precision(3);
+  space.print(source, std::cout);
+}

aikido/tests/statespace/test_Rn.cpp

@@ -88,3 +88,11 @@ TEST(Rn, CopyState)
   rvss.copyState(source, dest);
   EXPECT_TRUE(dest.getValue().isApprox(source.getValue()));
 }
+
+TEST(Rn, PrintState)
+{
+  Rn rvss(4);
+  auto source = rvss.createState();
+  source.setValue(Eigen::Vector4d(0, 1, 2, 3));
+  rvss.print(source, std::cout);
+}

aikido/tests/statespace/test_SE2.cpp

@@ -97,3 +97,10 @@ TEST(SE2, LogMap)
   EXPECT_TRUE(out.isApprox(Eigen::Vector3d(M_PI / 6, 4, 6)));
 }
 
+TEST(SE2, PrintState)
+{
+  SE2 se2;
+  auto state = se2.createState();
+  Eigen::Isometry2d pose1 = Eigen::Isometry2d::Identity();
+  se2.print(state, std::cout);
+}

aikido/tests/statespace/test_SE3.cpp

@@ -95,3 +95,10 @@ TEST(SE3, LogMap)
   EXPECT_TRUE(out.isApprox(twist));
 }
 
+TEST(SE3, PrintState)
+{
+  SE3 se3;
+  auto state = se3.createState();
+  state.setIsometry(Eigen::Isometry3d::Identity());
+  se3.print(state, std::cout);
+}

aikido/tests/statespace/test_SO2.cpp

@@ -100,3 +100,10 @@ TEST(SO2, CopyState)
   EXPECT_DOUBLE_EQ(source.getAngle(), dest.getAngle());
 }
 
+TEST(SO2, PrintState)
+{
+  SO2 so2;
+  auto source = so2.createState();
+  source.setAngle(M_PI);
+  so2.print(source, std::cout);
+}

aikido/tests/statespace/test_SO3.cpp

@@ -93,3 +93,13 @@ TEST(SO3, CopyState)
   so3.copyState(source, dest);
   EXPECT_TRUE(source.getQuaternion().isApprox(dest.getQuaternion()));
 }
+
+TEST(SO3, PrintState)
+{
+  SO3 so3;
+  auto source = so3.createState();
+  auto quat = Eigen::Quaterniond(
+      Eigen::AngleAxisd(1.8 * M_PI, Eigen::Vector3d::UnitX()));
+  source.setQuaternion(quat);
+  so3.print(source, std::cout);
+}