Skip to content
GitLab
Commit 3b6321e2 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Make random sampling methods non-static and const. 

  - This is necessary to correctly implement random sampling in dynamic-sized
    vector space.
parent 91efd554
Hide whitespace changes
Inline Side-by-side
src/algorithm/joint-configuration.hpp
View file @ 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));
}
......
src/multibody/liegroup/cartesian-product.hpp
View file @ 3b6321e2
......@@ -84,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,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
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>
......
src/multibody/liegroup/operation-base.hpp
View file @ 3b6321e2
......@@ -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,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout);
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;
/**
* @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,
......
src/multibody/liegroup/operation-base.hxx
View file @ 3b6321e2
......@@ -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);
......
src/multibody/liegroup/special-euclidean.hpp
View file @ 3b6321e2
......@@ -160,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,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
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>
......@@ -271,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,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
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>
......
src/multibody/liegroup/special-orthogonal.hpp
View file @ 3b6321e2
......@@ -135,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;
......@@ -143,9 +143,10 @@ namespace se3
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
random_impl(qout);
}
......@@ -226,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()
......@@ -235,9 +236,10 @@ namespace se3
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
{
random_impl(qout);
}
......
src/multibody/liegroup/vector-space.hpp
View file @ 3b6321e2
......@@ -81,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,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
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
{
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() )
......
unittest/liegroups.cpp
View file @ 3b6321e2
......@@ -54,9 +54,10 @@ 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(),
std::string(error_prefix + " - RandomConfiguration dimensions "));
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