Commit d720c915 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Move space operation to new class LieGroup and inherited classes.

* The spaces taken into account are SO1, SO3, SE1, SE3, vector spaces
* and Cartesian product of two of them.
parent 358d9364
......@@ -189,6 +189,15 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-composite.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/liegroup.hpp
multibody/liegroup/operation-base.hpp
multibody/liegroup/vector-space.hpp
multibody/liegroup/cartesian-product.hpp
multibody/liegroup/special-orthogonal.hpp
multibody/liegroup/special-euclidean.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/fwd.hpp
multibody/constraint.hpp
......@@ -281,6 +290,7 @@ SET(HEADERS
${${PROJECT_NAME}_TOOLS_HEADERS}
${${PROJECT_NAME}_SPATIAL_HEADERS}
${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
${${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS}
${${PROJECT_NAME}_MULTIBODY_HEADERS}
${${PROJECT_NAME}_PARSERS_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS}
......@@ -295,6 +305,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/math")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/lua")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
......
//
// Copyright (c) 2016 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_cartesian_product_operation_hpp__
#define __se3_cartesian_product_operation_hpp__
#include <pinocchio/multibody/liegroup/operation-base.hpp>
namespace se3
{
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation;
template<typename LieGroup1, typename LieGroup2>
struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
typedef double Scalar;
enum {
NQ = LieGroup1::NQ + LieGroup2::NQ,
NV = LieGroup1::NV + LieGroup2::NV
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation : public LieGroupOperationBase <CartesianProductOperation<LieGroup1, LieGroup2> >
{
typedef CartesianProductOperation<LieGroup1, LieGroup2> LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
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,
const Eigen::MatrixBase<Tangent_t> & d)
{
Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& > (d).derived();
LieGroup1::difference(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NV>());
LieGroup2::difference(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NV>());
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::integrate(q.template head<LieGroup1::NQ>(), v.template head<LieGroup1::NV>(), out.template head<LieGroup1::NQ>());
LieGroup2::integrate(q.template tail<LieGroup2::NQ>(), v.template tail<LieGroup2::NV>(), out.template tail<LieGroup2::NQ>());
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>())
+ LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>());
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
LieGroup1::random(out.template head<LieGroup1::NQ>());
LieGroup2::random(out.template tail<LieGroup2::NQ>());
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::randomConfiguration(lower.template head<LieGroup1::NQ>(), upper.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NQ>());
LieGroup2::randomConfiguration(lower.template tail<LieGroup2::NQ>(), upper.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NQ>());
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
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);
}
}; // struct CartesianProductOperation
} // namespace se3
#endif // ifndef __se3_cartesian_product_operation_hpp__
//
// Copyright (c) 2016 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_hpp__
#define __se3_lie_group_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/liegroup/vector-space.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
#include "pinocchio/multibody/liegroup/special-euclidean.hpp"
// #include "pinocchio/multibody/joint/joint.hpp"
namespace se3 {
template<typename Joint>
struct LieGroup {
typedef VectorSpaceOperation<Joint::NQ> type;
};
template<> struct LieGroup <JointModelSpherical> {
typedef SpecialOrthogonalOperation type;
};
template<> struct LieGroup <JointModelFreeFlyer> {
typedef SpecialEuclideanOperation type;
};
template<> struct LieGroup <JointModelPlanar> {
typedef SpecialEuclidean1Operation type;
};
template<int Axis> struct LieGroup <JointModelRevoluteUnbounded<Axis> > {
typedef SpecialOrthogonal1Operation type;
};
// TODO REMOVE: For testing purposes only
// template<>
// struct LieGroup <JointModelTranslation> {
// typedef CartesianProductOperation<
// CartesianProductOperation<typename LieGroup<JointModelPX>::type, typename LieGroup<JointModelPY>::type>,
// typename LieGroup<JointModelPZ>::type
// > type;
// };
}
#endif // ifndef __se3_lie_group_hpp__
//
// Copyright (c) 2016 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_operation_base_hpp__
#define __se3_lie_group_operation_base_hpp__
#include "pinocchio/spatial/fwd.hpp" // struct traits
#include <Eigen/Core>
#include <limits>
namespace se3
{
#ifdef __clang__
#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix) \
typedef prefix traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
#define SE3_LIE_GROUP_TYPEDEF_NOARG() \
typedef int Index; \
typedef traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix) \
typedef int Index; \
typedef prefix traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_NOARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
#else
#define SE3_LIE_GROUP_TYPEDEF_ARG() \
typedef int Index; \
typedef typename traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef typename traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef typename traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG()
#endif
template<typename Derived>
struct LieGroupOperationBase
{
typedef Derived LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
///
/// \brief Return the resolution of the finite differerence increment according to the Scalar type
/// \remark Ideally, this function must depend on the value of q
///
/// \returns The finite difference increment.
///
// typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
// { return derived().finiteDifferenceIncrement(); }
/**
* @brief Integrate joint's configuration for a tangent vector during one unit time
*
* @param[in] q initatial configuration (size full model.nq)
* @param[in] v joint velocity (size full model.nv)
*
* @return The configuration integrated
*/
template <class Config_t, class Tangent_t>
static ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v)
{
ConfigVector_t qout;
integrate(q, v, qout);
return qout;
}
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
static void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
Derived::integrate_impl(q, v, qout);
}
/**
* @brief Interpolation between two joint's configurations
*
* @param[in] q0 Initial configuration to interpolate
* @param[in] q1 Final configuration to interpolate
* @param[in] u u in [0;1] position along the interpolation.
*
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
// ConfigVector_t interpolate(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, double u) const
// { return derived().interpolate_impl(q0, q1, u); }
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u)
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
return qout;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
// assert(is_config_vector_size(q0) && is_config_vector_size(q1));
Derived::interpolate_impl(q0, q1, u, qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
else integrate(q0, u * difference(q0, q1), qout);
}
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
*
* \warning Do not take into account the joint limits. To shoot a configuration uniformingly
* depending on joint limits, see uniformySample
*
* @return The joint configuration
*/
static ConfigVector_t random()
{
ConfigVector_t qout;
random(qout);
return qout;
}
template <class Config_t>
static void random (const Eigen::MatrixBase<Config_t>& qout)
{ return Derived::random_impl (qout); }
/**
* @brief Generate a configuration vector uniformly sampled among
* provided limits.
*
* @param[in] lower_pos_limit lower joint limit
* @param[in] upper_pos_limit upper joint limit
*
* @return The joint configuration
*/
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
{
ConfigVector_t qout;
randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
return qout;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{ Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout); }
/**
* @brief the tangent vector that must be integrated during one unit time to go from q0 to q1
*
* @param[in] q0 Initial configuration
* @param[in] q1 Wished configuration
*
* @return The corresponding velocity
*/
template <class ConfigL_t, class ConfigR_t>
static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t diff;
difference(q0, q1, diff);
return diff;
}
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d)
{
// assert(is_config_vector_size(q0) && is_config_vector_size(q1));
Derived::difference_impl(q0, q1, d);
}
/**
* @brief Squared distance between two configurations of the joint
*
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{ return Derived::squaredDistance_impl(q0, q1); }
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference(q0, q1, t);
return t.squaredNorm();
}
/**
* @brief Distance between two configurations of the joint
*
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static double distance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{ return sqrt(squaredDistance(q0, q1)); }
/**
* @brief Get neutral configuration of joint
*
* @return The joint's neutral configuration
*/
// ConfigVector_t neutralConfiguration() const
// { return derived().neutralConfiguration_impl(); }
/**
* @brief Normalize a configuration
*
* @param[in,out] q Configuration to normalize (size full model.nq)
*/
// void normalize(Eigen::VectorXd & q) const
// { return derived().normalize_impl(q); }
/**
* @brief Default implementation of normalize
*/
// void normalize_impl(Eigen::VectorXd &) const { }
/**
* @brief Check if two configurations are equivalent within the given precision
*
* @param[in] q0 Configuration 0
* @param[in] q1 Configuration 1
*/
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{ return Derived::isSameConfiguration_impl(q0,q1, prec); }
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{ return q0.isApprox(q1, prec); }
protected:
/// Default constructor.
///
/// Prevent the construction of derived class.
LieGroupOperationBase() {}
/// Copy constructor
///
/// Prevent the copy of derived class.
LieGroupOperationBase( const LieGroupOperationBase& clone) {}
}; // struct LieGroupOperationBase
} // namespace se3
#endif // ifndef __se3_lie_group_operation_base_hpp__
//
// Copyright (c) 2016 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_special_euclidean_operation_hpp__
#define __se3_special_euclidean_operation_hpp__
#include <limits>
#include <pinocchio/spatial/fwd.hpp>
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/multibody/liegroup/operation-base.hpp>
#include <pinocchio/multibody/liegroup/vector-space.hpp>
#include <pinocchio/multibody/liegroup/cartesian-product.hpp>
#include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
namespace se3
{
struct SpecialEuclideanOperation;
template <> struct traits<SpecialEuclideanOperation> {
typedef double Scalar;
enum {
NQ = 7,
NV = 6
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
struct SpecialEuclideanOperation : public LieGroupOperationBase <SpecialEuclideanOperation>
{
typedef CartesianProductOperation <VectorSpaceOperation<3>, SpecialOrthogonalOperation> R3crossSO3_t;
typedef SpecialEuclideanOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
typedef Eigen::Quaternion<Scalar> Quaternion_t;
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
typedef SE3 Transformation_t;
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,
const Eigen::MatrixBase<Tangent_t> & d)
{
ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
= log6( SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
* SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
QuaternionMap_t res_quat (out.template tail<4>().data());
SE3 M0 (quat.matrix(), q.derived().template head<3>());
SE3 M1 (M0 * exp6(Motion(v)));
out.template head<3>() = M1.translation();
res_quat = M1.rotation();