Commit 6b43b692 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #399 from florent-lamiraux/devel

Lie group operation struct are aware of the dimension of the element they manipulate
parents b9279764 3b6321e2
......@@ -433,8 +433,8 @@ namespace se3
const Eigen::VectorXd & lowerLimits,
const Eigen::VectorXd & upperLimits)
{
LieGroup_t::template operation<JointModel>::type
::randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
typename LieGroup_t::template operation<JointModel>::type a;
a.randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
jmodel.jointConfigSelector(upperLimits),
jmodel.jointConfigSelector(q));
}
......
......@@ -41,6 +41,20 @@ namespace se3
typedef CartesianProductOperation<LieGroup1, LieGroup2> LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
/// 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;
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
}
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,
......@@ -70,21 +84,26 @@ namespace se3
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
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>());
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,
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
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>());
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>
......
......@@ -127,7 +127,7 @@ namespace se3
* @return The joint configuration
*/
template <class Config_t>
static void random (const Eigen::MatrixBase<Config_t>& qout);
void random (const Eigen::MatrixBase<Config_t>& qout) const;
/**
* @brief Generate a configuration vector uniformly sampled among
......@@ -139,9 +139,10 @@ namespace se3
* @return The joint configuration
*/
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
void randomConfiguration
(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout);
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
/**
* @brief the tangent vector that must be integrated during one unit time to go from q0 to q1
......@@ -204,11 +205,12 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u);
static ConfigVector_t random();
ConfigVector_t random() const;
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 randomConfiguration
(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
template <class ConfigL_t, class ConfigR_t>
static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
......@@ -233,7 +235,23 @@ namespace se3
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec);
/// 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;
/// Get dimension of Lie Group tangent space
Index nv () const;
Derived& derived ()
{
return static_cast <Derived&> (*this);
}
const Derived& derived () const
{
return static_cast <const Derived&> (*this);
}
/// \}
protected:
......
......@@ -71,8 +71,8 @@ namespace se3 {
*/
template <class Derived>
template <class Config_t>
void LieGroupOperationBase<Derived>::random(
const Eigen::MatrixBase<Config_t>& qout)
void LieGroupOperationBase<Derived>::random
(const Eigen::MatrixBase<Config_t>& qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return Derived::random_impl (qout);
......@@ -83,12 +83,12 @@ namespace se3 {
void LieGroupOperationBase<Derived>::randomConfiguration(
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
derived ().randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
}
template <class Derived>
......@@ -163,7 +163,8 @@ namespace se3 {
}
template <class Derived>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::random()
typename LieGroupOperationBase<Derived>::ConfigVector_t
LieGroupOperationBase<Derived>::random() const
{
ConfigVector_t qout;
random(qout);
......@@ -172,9 +173,10 @@ namespace se3 {
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::randomConfiguration(
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
typename LieGroupOperationBase<Derived>::ConfigVector_t
LieGroupOperationBase<Derived>::randomConfiguration
(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const
{
ConfigVector_t qout;
randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
......@@ -226,6 +228,20 @@ namespace se3 {
{
return q0.isApprox(q1, prec);
}
template <class Derived>
typename LieGroupOperationBase <Derived>::Index
LieGroupOperationBase <Derived>::nq () const
{
return Derived::nq ();
}
template <class Derived>
typename LieGroupOperationBase <Derived>::Index
LieGroupOperationBase <Derived>::nv () const
{
return Derived::nv ();
}
} // namespace se3
#endif // __se3_lie_group_operation_base_hxx__
......@@ -61,6 +61,20 @@ namespace se3
typedef SpecialEuclideanOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
/// 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;
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
}
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,
......@@ -146,17 +160,18 @@ namespace se3
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
R2crossSO2_t::random(qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
R2crossSO2_t::randomConfiguration(lower, upper, qout);
R2crossSO2_t ().randomConfiguration(lower, upper, qout);
}
template <class ConfigL_t, class ConfigR_t>
......@@ -193,6 +208,20 @@ namespace se3
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
typedef SE3 Transformation_t;
/// 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;
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
}
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,
......@@ -243,17 +272,18 @@ namespace se3
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
R3crossSO3_t::random(qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
R3crossSO3_t::randomConfiguration(lower, upper, qout);
R3crossSO3_t ().randomConfiguration(lower, upper, qout);
}
template <class ConfigL_t, class ConfigR_t>
......
......@@ -55,6 +55,20 @@ namespace se3
typedef SpecialOrthogonalOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
/// 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;
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
}
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,
......@@ -121,7 +135,7 @@ namespace se3
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
Config_t& out = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived();
const Scalar angle = -PI + 2*PI * ((Scalar)rand())/RAND_MAX;
......@@ -129,9 +143,10 @@ namespace se3
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
random_impl(qout);
}
......@@ -147,6 +162,20 @@ namespace se3
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
/// 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;
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
}
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,
......@@ -198,7 +227,7 @@ namespace se3
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
QuaternionMap_t out (
(const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived().data()
......@@ -207,9 +236,10 @@ namespace se3
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
random_impl(qout);
}
......
......@@ -39,8 +39,27 @@ namespace se3
struct VectorSpaceOperation : public LieGroupOperationBase <VectorSpaceOperation<Size> >
{
typedef VectorSpaceOperation<Size> LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
/// Constructor
/// \param size size of the vector space: should be the equal to template
/// argument for static sized vector-spaces.
VectorSpaceOperation (int size = Size) : size_ (size)
{
assert (size_ >= 0);
assert (Size == Eigen::Dynamic || size_ == Size);
}
Index nq () const
{
return size_;
}
Index nv () const
{
return size_;
}
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,
......@@ -62,18 +81,19 @@ namespace se3
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
qout.setRandom();
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
void randomConfiguration_impl
(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
ConfigOut_t& res = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
for (int i = 0; i < NQ; ++i)
for (int i = 0; i < nq (); ++i)
{
if(lower_pos_limit[i] == -std::numeric_limits<Scalar>::infinity() ||
upper_pos_limit[i] == std::numeric_limits<Scalar>::infinity() )
......@@ -86,6 +106,8 @@ namespace se3
res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
}
}
private:
Index size_;
}; // struct VectorSpaceOperation
} // namespace se3
......
......@@ -54,8 +54,9 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(LieGroupType::integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(LieGroupType::interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
BOOST_CHECK_MESSAGE(jmodel.randomConfiguration( -1 * Ones, Ones).size()
== LieGroupType::randomConfiguration(-1 * Ones, Ones).size(),
BOOST_CHECK_MESSAGE
(jmodel.randomConfiguration( -1 * Ones, Ones).size() ==
LieGroupType().randomConfiguration(-1 * Ones, Ones).size(),
std::string(error_prefix + " - RandomConfiguration dimensions "));
BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(LieGroupType::difference(q1,q2)) ,std::string(error_prefix + " - difference "));
BOOST_CHECK_MESSAGE(fabs(jmodel.distance(q1,q2) - LieGroupType::distance(q1,q2)) < 1e-12 ,std::string(error_prefix + " - distance "));
......@@ -122,7 +123,7 @@ BOOST_AUTO_TEST_CASE ( test_vector_space )
bool error = false;
try {
VSO_t::randomConfiguration(lo, up, q);
VSO_t ().randomConfiguration(lo, up, q);
} catch (const std::runtime_error&) {
error = true;
}
......
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