Commit 6795c5ce authored by jcarpent's avatar jcarpent
Browse files

[LieGroup] Continue implementing Variant for LieGroup

parent 13318719
......@@ -113,7 +113,6 @@ namespace se3
const Eigen::VectorXd & v,
Eigen::VectorXd & result)
{
std::cout << jmodel.shortname() << std::endl;
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.integrate(jmodel.jointConfigSelector (q),
jmodel.jointVelocitySelector(v),
......
......@@ -27,7 +27,7 @@ namespace se3
* @brief Visit a LieGroupVariant to get the dimension of
* the Lie group configuration space
*
* @param[in] lg The LieGroupVariant.
* @param[in] lg the LieGroupVariant.
*
* @return The dimension of the Lie group configuration space
*/
......@@ -37,12 +37,57 @@ namespace se3
* @brief Visit a LieGroupVariant to get the dimension of
* the Lie group tangent space
*
* @param[in] lg The LieGroupVariant.
* @param[in] lg the LieGroupVariant.
*
* @return The dimension of the Lie group tangent space
*/
inline int nv(const LieGroupVariant & lg);
/**
* @brief Visit a LieGroupVariant to get the name of it
*
* @param[in] lg the LieGroupVariant.
*
* @return The Lie group name
*/
inline std::string name(const LieGroupVariant & lg);
/**
* @brief Visit a LieGroupVariant to get the neutral element of it
*
* @param[in] lg the LieGroupVariant.
*
* @return The Lie group neutral element
*/
inline Eigen::VectorXd neutral(const LieGroupVariant & lg);
/**
* @brief Visit a LieGroupVariant to call its integrate method
*
* @param[in] lg the LieGroupVariant.
* @param[in] q the starting configuration.
* @param[in] v the tangent velocity.
*
*/
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
inline void integrate(const LieGroupVariant & lg,
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout);
/**
* @brief Visit a LieGroupVariant to call its integrate method
*
* @param[in] lg The LieGroupVariant.
*
*/
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
inline void integrate(const LieGroupVariant & lg,
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout);
}
/// Details
......
......@@ -19,15 +19,45 @@
#define __se3_lie_group_variant_visitor_hxx__
#include "pinocchio/multibody/liegroup/operation-base.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include <string>
#define LIE_GROUP_VISITOR(VISITOR) \
VISITOR(ArgsType & args) : args(args) {} \
ArgsType & args
namespace se3
{
namespace visitor
{
namespace bf = boost::fusion;
template<typename Visitor>
struct LieGroupVisitorBase : public boost::static_visitor<>
{
template<typename D>
void operator() (const LieGroupOperationBase<D> & lg) const
{
bf::invoke(&Visitor::template algo<D>,
bf::append(boost::ref(lg),
static_cast<const Visitor*>(this)->args));
}
template<typename ArgsTmp>
static void run(const LieGroupVariant & lg,
ArgsTmp args)
{
return boost::apply_visitor(Visitor(args),lg);
}
};
}
/**
* @brief Lie Group visitor of the dimension of the configuration space nq
*/
class LieGroupNqVisitor: public boost::static_visitor<int>
struct LieGroupNqVisitor: public boost::static_visitor<int>
{
public:
template<typename D>
int operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.nq(); }
......@@ -40,9 +70,8 @@ namespace se3
/**
* @brief Lie Group visitor of the dimension of the tangent space nv
*/
class LieGroupNvVisitor: public boost::static_visitor<int>
struct LieGroupNvVisitor: public boost::static_visitor<int>
{
public:
template<typename D>
int operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.nv(); }
......@@ -52,6 +81,77 @@ namespace se3
};
inline int nv(const LieGroupVariant & lg) { return LieGroupNvVisitor::run(lg); }
/**
* @brief Visitor of the Lie Group name
*/
struct LieGroupNameVisitor: public boost::static_visitor<std::string>
{
template<typename D>
std::string operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.name(); }
static std::string run(const LieGroupVariant & lg)
{ return boost::apply_visitor( LieGroupNameVisitor(), lg ); }
};
inline std::string name(const LieGroupVariant & lg) { return LieGroupNameVisitor::run(lg); }
/**
* @brief Visitor of the Lie Group neutral element
*/
template<typename Vector>
struct LieGroupNeutralVisitor: public boost::static_visitor<Vector>
{
template<typename D>
Vector operator()(const LieGroupOperationBase<D> & lg) const
{ return lg.neutral(); }
static Vector run(const LieGroupVariant & lg)
{ return boost::apply_visitor( LieGroupNeutralVisitor(), lg ); }
};
inline Eigen::VectorXd neutral(const LieGroupVariant & lg)
{ return LieGroupNeutralVisitor<Eigen::VectorXd>::run(lg); }
/**
* @brief Visitor of the Lie Group integrate method
*/
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
struct LieGroupIntegrateVisitor : visitor::LieGroupVisitorBase< LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> >
{
typedef boost::fusion::vector<const Eigen::MatrixBase<ConfigIn_t> &,
const Eigen::MatrixBase<Tangent_t> &,
const Eigen::MatrixBase<ConfigOut_t> &> ArgsType;
LIE_GROUP_VISITOR(LieGroupIntegrateVisitor);
template<typename D>
static void algo(const LieGroupOperationBase<D> & lg,
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
lg.integrate(Eigen::Ref<const typename D::ConfigVector_t>(q),
Eigen::Ref<const typename D::TangentVector_t>(v),
Eigen::Ref<typename D::ConfigVector_t>(qout_));
}
};
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
inline void integrate(const LieGroupVariant & lg,
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
typedef LieGroupIntegrateVisitor<ConfigIn_t,Tangent_t,ConfigOut_t> Operation;
assert(q.size() == nq(lg));
assert(v.size() == nv(lg));
assert(qout.size() == nq(lg));
ConfigOut_t & qout_ = const_cast< ConfigOut_t& >(qout.derived());
Operation::run(lg,typename Operation::ArgsType(q,v,qout_));
}
}
#endif // ifndef __se3_lie_group_variant_visitor_hxx__
......
......@@ -427,8 +427,23 @@ struct TestLieGroupVariantVisitor
template<typename Derived>
static void test(const LieGroupOperationBase<Derived> & lg, const LieGroupVariant & lg_variant)
{
typedef typename Derived::ConfigVector_t ConfigVector_t;
typedef typename Derived::TangentVector_t TangentVector_t;
BOOST_CHECK(lg.nq() == nq(lg_variant));
BOOST_CHECK(lg.nv() == nv(lg_variant));
BOOST_CHECK(lg.name() == name(lg_variant));
BOOST_CHECK(lg.neutral() == neutral(lg_variant));
ConfigVector_t q0 = lg.random();
TangentVector_t v = TangentVector_t::Random(lg.nv());
ConfigVector_t qout(lg.nq()), qout_ref(lg.nq());
lg.integrate(q0, v, qout_ref);
integrate(lg_variant, q0, v, qout);
BOOST_CHECK(qout.isApprox(qout_ref));
}
};
......
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