Commit 95d62eba authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #406 from florent-lamiraux/devel

Add methods "name" and "neutral" in OperationBase and derived classes
parents 79251600 8a7403c9
......@@ -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
......
......@@ -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));
......
......@@ -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
......
......@@ -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 ()
{
......
......@@ -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__
......@@ -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,
......
......@@ -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,
......
......@@ -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,
......
......@@ -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 ()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment