Commit 834c00bb authored by jcarpent's avatar jcarpent
Browse files

[LieGroup] Start implementing Lie group variant visitors

parent 6aa6a188
......@@ -204,6 +204,8 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/liegroup.hpp
multibody/liegroup/liegroup-variant.hpp
multibody/liegroup/liegroup-variant-visitors.hpp
multibody/liegroup/liegroup-variant-visitors.hxx
multibody/liegroup/operation-base.hpp
multibody/liegroup/operation-base.hxx
multibody/liegroup/vector-space.hpp
......
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_lie_group_variant_visitor_hpp__
#define __se3_lie_group_variant_visitor_hpp__
#include "pinocchio/multibody/liegroup/liegroup-variant.hpp"
namespace se3
{
/**
* @brief Visit a LieGroupVariant to get the dimension of
* the Lie group configuration space
*
* @param[in] lg The LieGroupVariant.
*
* @return The dimension of the Lie group configuration space
*/
inline int nq(const LieGroupVariant & lg);
/**
* @brief Visit a LieGroupVariant to get the dimension of
* the Lie group tangent space
*
* @param[in] lg The LieGroupVariant.
*
* @return The dimension of the Lie group tangent space
*/
inline int nv(const LieGroupVariant & lg);
}
/// Details
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx"
#endif // ifndef __se3_lie_group_variant_visitor_hpp__
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_lie_group_variant_visitor_hxx__
#define __se3_lie_group_variant_visitor_hxx__
#include "pinocchio/multibody/liegroup/operation-base.hpp"
namespace se3
{
/**
* @brief Lie Group visitor of the dimension of the configuration space nq
*/
class LieGroupNqVisitor: public boost::static_visitor<int>
{
public:
template<typename D>
int operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.nq(); }
static int run(const LieGroupVariant & lg)
{ return boost::apply_visitor( LieGroupNqVisitor(), lg ); }
};
inline int nq(const LieGroupVariant & lg) { return LieGroupNqVisitor::run(lg); }
/**
* @brief Lie Group visitor of the dimension of the tangent space nv
*/
class LieGroupNvVisitor: public boost::static_visitor<int>
{
public:
template<typename D>
int operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.nv(); }
static int run(const LieGroupVariant & lg)
{ return boost::apply_visitor( LieGroupNvVisitor(), lg ); }
};
inline int nv(const LieGroupVariant & lg) { return LieGroupNvVisitor::run(lg); }
}
#endif // ifndef __se3_lie_group_variant_visitor_hxx__
......@@ -15,6 +15,7 @@
// pinocchio. If not, see <http://www.gnu.org/licenses/>.
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
......@@ -414,4 +415,26 @@ BOOST_AUTO_TEST_CASE(test_dim_computation)
BOOST_CHECK(dim == Eigen::Dynamic);
}
struct TestLieGroupVariantVisitor
{
template<typename Derived>
void operator() (const LieGroupOperationBase<Derived> & lg) const
{
LieGroupVariant lg_variant(lg.derived());
test(lg,lg_variant);
}
template<typename Derived>
static void test(const LieGroupOperationBase<Derived> & lg, const LieGroupVariant & lg_variant)
{
BOOST_CHECK(lg.nq() == nq(lg_variant));
BOOST_CHECK(lg.nv() == nv(lg_variant));
}
};
BOOST_AUTO_TEST_CASE(test_liegroup_variant)
{
boost::mpl::for_each<LieGroupVariant::types>(TestLieGroupVariantVisitor());
}
BOOST_AUTO_TEST_SUITE_END ()
Supports Markdown
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