diff --git a/src/multibody/liegroup/cartesian-product.hpp b/src/multibody/liegroup/cartesian-product.hpp index 83f62c0c2f22f4d82bcdf027926751bb8b9ea6e2..ade8c1c0e1924f588576adbc57b135992eeb922f 100644 --- a/src/multibody/liegroup/cartesian-product.hpp +++ b/src/multibody/liegroup/cartesian-product.hpp @@ -41,6 +41,9 @@ namespace se3 typedef CartesianProductOperation<LieGroup1, LieGroup2> LieGroupDerived; SE3_LIE_GROUP_TYPEDEF_TEMPLATE; + CartesianProductOperation () : lg1_ (), lg2_ () + { + } /// Get dimension of Lie Group vector representation /// /// For instance, for SO(3), the dimension of the vector representation is @@ -55,6 +58,12 @@ namespace se3 return NV; } + std::string name () const + { + std::ostringstream oss; oss << lg1_.name () << "*" << lg2_.name (); + return oss.str (); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1, @@ -114,6 +123,9 @@ namespace se3 return LieGroup1::isSameConfiguration(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), prec) + LieGroup2::isSameConfiguration(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), prec); } + private: + LieGroup1 lg1_; + LieGroup2 lg2_; }; // struct CartesianProductOperation } // namespace se3 diff --git a/src/multibody/liegroup/operation-base.hpp b/src/multibody/liegroup/operation-base.hpp index 0776e71256a3825d3413c493b884678dadbbe9c7..928cda2be514659fc382b520f6396264e34f69e6 100644 --- a/src/multibody/liegroup/operation-base.hpp +++ b/src/multibody/liegroup/operation-base.hpp @@ -244,6 +244,9 @@ namespace se3 /// Get dimension of Lie Group tangent space Index nv () const; + /// Get name of instance + std::string name () const; + Derived& derived () { return static_cast <Derived&> (*this); diff --git a/src/multibody/liegroup/operation-base.hxx b/src/multibody/liegroup/operation-base.hxx index 180c55f38fbcbcb24c9d0049ee0be29343a87193..0c56ef41def3b6e7b941edb2a98d318c55b3e00d 100644 --- a/src/multibody/liegroup/operation-base.hxx +++ b/src/multibody/liegroup/operation-base.hxx @@ -242,6 +242,13 @@ namespace se3 { { return Derived::nv (); } + + template <class Derived> + std::string LieGroupOperationBase <Derived>::name () const + { + return Derived::name (); + } + } // namespace se3 #endif // __se3_lie_group_operation_base_hxx__ diff --git a/src/multibody/liegroup/special-euclidean.hpp b/src/multibody/liegroup/special-euclidean.hpp index 3b5fa63ce9670e579582da70eb85897663f17ca9..38c7f837524144f0cb87f855357f68ffa2224bfb 100644 --- a/src/multibody/liegroup/special-euclidean.hpp +++ b/src/multibody/liegroup/special-euclidean.hpp @@ -75,6 +75,11 @@ namespace se3 return NV; } + std::string name () const + { + return std::string ("SE(2)"); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1, @@ -222,6 +227,11 @@ namespace se3 return NV; } + std::string name () const + { + return std::string ("SE(3)"); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1, diff --git a/src/multibody/liegroup/special-orthogonal.hpp b/src/multibody/liegroup/special-orthogonal.hpp index e786d49fbbf3ac785860f8f84480137b05b767fa..14f67993d92178cec686308f8a45c63232e18ff7 100644 --- a/src/multibody/liegroup/special-orthogonal.hpp +++ b/src/multibody/liegroup/special-orthogonal.hpp @@ -69,6 +69,11 @@ namespace se3 return NV; } + std::string name () const + { + return std::string ("SO(2)"); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1, @@ -176,6 +181,11 @@ namespace se3 return NV; } + std::string name () const + { + return std::string ("SO(3)"); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1, diff --git a/src/multibody/liegroup/vector-space.hpp b/src/multibody/liegroup/vector-space.hpp index 0ad9464f4ec3ba5b4ed7d514e846d9f97b58caf9..7ad61b81d9ce2478013be90950ba9a7a32a206c8 100644 --- a/src/multibody/liegroup/vector-space.hpp +++ b/src/multibody/liegroup/vector-space.hpp @@ -69,6 +69,12 @@ namespace se3 return size_; } + std::string name () const + { + std::ostringstream oss; oss << "R^" << size_; + return oss.str (); + } + template <class ConfigL_t, class ConfigR_t, class Tangent_t> static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1,