Merge pull request #406 from florent-lamiraux/devel

Add methods "name" and "neutral" in OperationBase and derived classes

doc/Doxyfile.extra.in

@@ -45,22 +45,6 @@ INLINE_GROUPED_CLASSES = NO
 
 TYPEDEF_HIDES_STRUCT   = NO
 
-# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to
-# determine which symbols to keep in memory and which to flush to disk.
-# When the cache is full, less often used symbols will be written to disk.
-# For small to medium size projects (<1000 input files) the default value is
-# probably good enough. For larger projects a too small cache size can cause
-# doxygen to be busy swapping symbols to and from disk most of the time
-# causing a significant performance penalty.
-# If the system has enough physical memory increasing the cache will improve the
-# performance by keeping more symbols in memory. Note that the value works on
-# a logarithmic scale so increasing the size by one will roughly double the
-# memory usage. The cache size is given by this formula:
-# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0,
-# corresponding to a cache size of 2^16 = 65536 symbols.
-
-SYMBOL_CACHE_SIZE      = 0
-
 # Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be
 # set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given
 # their name and scope. Since this can be an expensive process and often the
@@ -238,12 +222,6 @@ SHOW_NAMESPACES        = YES
 
 SHOW_FILES             = YES
 
-# If the sources in your project are distributed over multiple directories 
-# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy 
-# in the documentation. The default is NO.
-
-SHOW_DIRECTORIES       = YES
-
 # The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
 # by doxygen. The layout file controls the global structure of the generated
 # output files in an output format independent way. To create the layout file
@@ -466,12 +444,6 @@ HTML_COLORSTYLE_SAT    = 100
 
 HTML_COLORSTYLE_GAMMA  = 80
 
-# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes,
-# files or namespaces will be aligned in HTML using tables. If set to
-# NO a bullet list will be used.
-
-HTML_ALIGN_MEMBERS     = YES
-
 # If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
 # documentation will contain sections that can be hidden and shown after the
 # page has loaded. For this to work a browser that supports
@@ -538,11 +510,6 @@ DISABLE_INDEX          = YES
 
 GENERATE_TREEVIEW      = YES
 
-# By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories,
-# and Class Hierarchy pages using a tree view instead of an ordered list.
-
-USE_INLINE_TREES       = YES
-
 # Use this tag to change the font size of Latex formulas included
 # as images in the HTML documentation. The default is 10. Note that
 # when you change the font size after a successful doxygen run you need
@@ -583,7 +550,7 @@ PREDEFINED             += WITH_HPP_FCL
 # contain include files that are not input files but should be processed by
 # the preprocessor.
 
-INCLUDE_PATH          += @PROJECT_SOURCE_DIR@/src  \
+INCLUDE_PATH          = @PROJECT_SOURCE_DIR@/src
 
 #---------------------------------------------------------------------------
 # Configuration::additions related to external references

src/algorithm/finite-differences.hxx

@@ -50,7 +50,7 @@ namespace se3
   {
     using namespace se3::details;
     Eigen::VectorXd fd_increment(model.nv);
-    for(int k = 1; k < model.joints.size(); ++k)
+    for(std::size_t k = 1; k < model.joints.size(); ++k)
     {
       const JointModel & jmodel = model.joints[k];
       FinitDiffEpsVisitor::run(jmodel,FinitDiffEpsVisitor::ArgsType(fd_increment));

src/multibody/liegroup/cartesian-product.hpp

@@ -41,18 +41,36 @@ 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
     /// 4 (quaternion) while the dimension of the tangent space is 3.
     Index nq () const
     {
-      return NQ;
+      return lg1_.nq () + lg2_.nq ();
     }
     /// Get dimension of Lie Group tangent space
     Index nv () const
     {
-      return NV;
+      return lg1_.nv () + lg2_.nv ();
+    }
+
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n;
+      n.resize (nq ());
+      n.head (lg1_.nq ()) = lg1_.neutral ();
+      n.tail (lg2_.nq ()) = lg2_.neutral ();
+      return n;
+    }
+
+    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>
@@ -114,6 +132,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

src/multibody/liegroup/operation-base.hpp

@@ -243,6 +243,11 @@ namespace se3
     Index nq () const;
     /// Get dimension of Lie Group tangent space
     Index nv () const;
+    /// Get neutral element as a vector
+    ConfigVector_t neutral () const;
+
+    /// Get name of instance
+    std::string name () const;
 
     Derived& derived ()
     {

src/multibody/liegroup/operation-base.hxx

@@ -242,6 +242,20 @@ namespace se3 {
   {
     return Derived::nv ();
   }
+
+  template <class Derived>
+  typename LieGroupOperationBase <Derived>::ConfigVector_t
+  LieGroupOperationBase <Derived>::neutral () const
+  {
+    return Derived::neutral ();
+  }
+
+  template <class Derived>
+  std::string LieGroupOperationBase <Derived>::name () const
+  {
+    return Derived::name ();
+  }
+
 } // namespace se3
 
 #endif // __se3_lie_group_operation_base_hxx__

src/multibody/liegroup/special-euclidean.hpp

@@ -75,6 +75,17 @@ namespace se3
       return NV;
     }
 
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n; n.setZero (); n [2] = 1;
+      return n;
+    }
+
+    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,
@@ -196,6 +207,7 @@ namespace se3
     }
   }; // struct SpecialEuclideanOperation<2>
 
+  /// SE(3)
   template<>
   struct SpecialEuclideanOperation<3> : public LieGroupOperationBase <SpecialEuclideanOperation<3> >
   {
@@ -222,6 +234,17 @@ namespace se3
       return NV;
     }
 
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n; n.setZero (); n [6] = 1;
+      return n;
+    }
+
+    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,

src/multibody/liegroup/special-orthogonal.hpp

@@ -69,6 +69,17 @@ namespace se3
       return NV;
     }
 
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n; n.setZero (); n [0] = 1;
+      return n;
+    }
+
+    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 +187,17 @@ namespace se3
       return NV;
     }
 
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n; n.setZero (); n [3] = 1;
+      return n;
+    }
+
+    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,

src/multibody/liegroup/vector-space.hpp

@@ -69,6 +69,21 @@ namespace se3
       return size_;
     }
 
+    ConfigVector_t neutral () const
+    {
+      ConfigVector_t n;
+      if (Size == Eigen::Dynamic)
+        n.resize (size_);
+      n.setZero ();
+      return n;
+    }
+
+    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,

unittest/liegroups.cpp

@@ -130,4 +130,81 @@ BOOST_AUTO_TEST_CASE ( test_vector_space )
   BOOST_CHECK_MESSAGE(error, "Random configuration between infinite bounds should return an error");
 }
 
+BOOST_AUTO_TEST_CASE ( test_size )
+{
+  // R^1: neutral = [0]
+  VectorSpaceOperation <1> vs1;
+  Eigen::VectorXd neutral;
+  neutral.resize (1);
+  neutral.setZero ();
+  BOOST_CHECK (vs1.nq () == 1);
+  BOOST_CHECK (vs1.nv () == 1);
+  BOOST_CHECK (vs1.name () == "R^1");
+  BOOST_CHECK (vs1.neutral () == neutral);
+  // R^2: neutral = [0, 0]
+  VectorSpaceOperation <2> vs2;
+  neutral.resize (2);
+  neutral.setZero ();
+  BOOST_CHECK (vs2.nq () == 2);
+  BOOST_CHECK (vs2.nv () == 2);
+  BOOST_CHECK (vs2.name () == "R^2");
+  BOOST_CHECK (vs2.neutral () == neutral);
+  // R^3: neutral = [0, 0, 0]
+  VectorSpaceOperation <3> vs3;
+  neutral.resize (3);
+  neutral.setZero ();
+  BOOST_CHECK (vs3.nq () == 3);
+  BOOST_CHECK (vs3.nv () == 3);
+  BOOST_CHECK (vs3.name () == "R^3");
+  BOOST_CHECK (vs3.neutral () == neutral);
+  // SO(2): neutral = [1, 0]
+  SpecialOrthogonalOperation <2> so2;
+  neutral.resize (2); neutral [0] = 1; neutral [1] = 0;
+  BOOST_CHECK (so2.nq () == 2);
+  BOOST_CHECK (so2.nv () == 1);
+  BOOST_CHECK (so2.name () == "SO(2)");
+  BOOST_CHECK (so2.neutral () == neutral);
+  // SO(3): neutral = [0, 0, 0, 1]
+  SpecialOrthogonalOperation <3> so3;
+  neutral.resize (4); neutral.setZero ();
+  neutral [3] = 1;
+  BOOST_CHECK (so3.nq () == 4);
+  BOOST_CHECK (so3.nv () == 3);
+  BOOST_CHECK (so3.name () == "SO(3)");
+  BOOST_CHECK (so3.neutral () == neutral);
+  // SE(2): neutral = [0, 0, 1, 0]
+  SpecialEuclideanOperation <2> se2;
+  neutral.resize (4); neutral.setZero ();
+  neutral [2] = 1;
+  BOOST_CHECK (se2.nq () == 4);
+  BOOST_CHECK (se2.nv () == 3);
+  BOOST_CHECK (se2.name () == "SE(2)");
+  BOOST_CHECK (se2.neutral () == neutral);
+  // SE(3): neutral = [0, 0, 0, 0, 0, 0, 1]
+  SpecialEuclideanOperation <3> se3;
+  neutral.resize (7); neutral.setZero ();
+  neutral [6] = 1;
+  BOOST_CHECK (se3.nq () == 7);
+  BOOST_CHECK (se3.nv () == 6);
+  BOOST_CHECK (se3.name () == "SE(3)");
+  BOOST_CHECK (se3.neutral () == neutral);
+  // R^2 x SO(2): neutral = [0, 0, 1, 0]
+  CartesianProductOperation <VectorSpaceOperation <2>,
+                             SpecialOrthogonalOperation <2> > r2xso2;
+  neutral.resize (4); neutral.setZero ();
+  neutral [2] = 1;
+  BOOST_CHECK (r2xso2.nq () == 4);
+  BOOST_CHECK (r2xso2.nv () == 3);
+  BOOST_CHECK (r2xso2.name () == "R^2*SO(2)");
+  BOOST_CHECK (r2xso2.neutral () == neutral);
+  // R^3 x SO(3): neutral = [0, 0, 0, 0, 0, 0, 1]
+  CartesianProductOperation <VectorSpaceOperation <3>,
+                             SpecialOrthogonalOperation <3> > r3xso3;
+  neutral.resize (7); neutral.setZero ();
+  neutral [6] = 1;
+  BOOST_CHECK (r3xso3.nq () == 7);
+  BOOST_CHECK (r3xso3.nv () == 6);
+  BOOST_CHECK (r3xso3.name () == "R^3*SO(3)");
+  BOOST_CHECK (r3xso3.neutral () == neutral);
+}
 BOOST_AUTO_TEST_SUITE_END ()