Commit fb14d535 authored by jcarpent's avatar jcarpent
Browse files

[Joints] Remove methods {integrate,differentiate,interpolate,neutral,random} from JointModels

One should now use the LieGroup methods instead in order to use the previous methods or implement his or het own methods.
parent 84f5ab1d
......@@ -229,108 +229,6 @@ namespace se3
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
*/
ConfigVector_t integrate(const Eigen::VectorXd & q,const Eigen::VectorXd & v) const
{ return derived().integrate_impl(q, v); }
/**
* @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); }
/**
* @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
*/
ConfigVector_t random() const
{ return derived().random_impl(); }
/**
* @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
*/
ConfigVector_t randomConfiguration(const ConfigVector_t & lower_pos_limit,
const ConfigVector_t & upper_pos_limit) const
{ return derived().randomConfiguration_impl(lower_pos_limit, upper_pos_limit); }
/**
* @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
*/
TangentVector_t difference(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{ return derived().difference_impl(q0, q1); }
/**
* @brief Distance between two configurations of the joint
*
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
double distance(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{ return derived().distance_impl(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] q1 Configuration 1 (size full model.nq)
* @param[in] q2 Configuration 2 (size full model.nq)
*/
bool isSameConfiguration(const Eigen::VectorXd & q1, const Eigen::VectorXd & q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{ return derived().isSameConfiguration_impl(q1,q2, prec); }
JointIndex i_id; // ID of the joint in the multibody list.
int i_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector.
......
......@@ -75,127 +75,6 @@ namespace se3
/// \returns The finite diffrence increment.
///
inline double finiteDifferenceIncrement(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointIntegrateVisitor to integrate joint's configuration
* for a tangent vector during one unit time
*
* @param[in] jmodel The JointModelVariant
* @param[in] q Initatial configuration (size full model.nq)
* @param[in] v Joints velocity (size full model.nv)
*
* @return The configuration integrated
*/
inline Eigen::VectorXd integrate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
/**
* @brief Visit a JointModelVariant through JointInterpolateVisitor to compute
* the interpolation between two joint's configurations
*
* @param[in] jmodel The JointModelVariant
* @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)
*/
inline Eigen::VectorXd interpolate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
const double u);
/**
* @brief Visit a JointModelVariant through JointRandomVisitor to
* generate a random configuration vector
*
* @param[in] jmodel The JointModelVariant
*
* @return The joint randomconfiguration
*/
inline Eigen::VectorXd random(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
*
* @param[in] jmodel The JointModelVariant
* @param[in] lower_pos_limit lower joint limit
* @param[in] upper_pos_limit upper joint limit
*
* @return The joint configuration
*/
inline Eigen::VectorXd randomConfiguration(const JointModelVariant & jmodel,
const Eigen::VectorXd & lower_pos_limit,
const Eigen::VectorXd & upper_pos_limit);
/**
* @brief Visit a JointModelVariant through JointDifferenceVisitor to compute
* the tangent vector that must be integrated during one unit time to go from q0 to q1
*
* @param[in] jmodel The JointModelVariant
* @param[in] q0 Initial configuration
* @param[in] q1 Wished configuration
*
* @return The corresponding velocity
*/
inline Eigen::VectorXd difference(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1);
/**
* @brief Visit a JointModelVariant through JointDifferenceVisitor to compute the distance
* between two configurations
*
* @param[in] jmodel The JointModelVariant
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
inline double distance(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1);
/**
* @brief Visit a JointModelVariant through JointNeutralfigurationVisitor to
* get the neutral configuration vector of the joint
*
* @param[in] jmodel The JointModelVariant
*
* @return The joint's neutral configuration
*/
inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointNormalizeVisitor to compute
* the normalized configuration.
*
* @param[in] jmodel The JointModelVariant
* @param[in,out] q configuration to normalize
*/
inline void normalize(const JointModelVariant & jmodel,
Eigen::VectorXd & q);
/**
* @brief Visit a JointModelVariant through JointIsSameConfigurationVisitor to determine
* if the two given configurations are equivalent or not.
*
* @param[in] jmodel The JointModelVariant
* @param[in] q1 Configuration 1
* @param[in] q2 Configuration 2
*
* @return Wheter the two configurations are equivalent
*/
inline bool isSameConfiguration(const JointModelVariant & jmodel,
const Eigen::VectorXd & q1,
const Eigen::VectorXd & q2);
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
......
......@@ -140,227 +140,6 @@ namespace se3
inline double finiteDifferenceIncrement(const JointModelVariant & jmodel)
{ return JointEpsVisitor::run(jmodel); }
/**
* @brief JointIntegrateVisitor
*/
class JointIntegrateVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
const Eigen::VectorXd & q;
const Eigen::VectorXd & v;
JointIntegrateVisitor(const Eigen::VectorXd & q,const Eigen::VectorXd & v) : q(q), v(v) {}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.integrate(q, v); }
static Eigen::VectorXd run( const JointModelVariant & jmodel, const Eigen::VectorXd & q,const Eigen::VectorXd & v)
{ return boost::apply_visitor( JointIntegrateVisitor(q,v), jmodel ); }
};
inline Eigen::VectorXd integrate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
return JointIntegrateVisitor::run(jmodel, q, v);
}
/**
* @brief JointInterpolateVisitor visitor
*/
class JointInterpolateVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
const Eigen::VectorXd & q0;
const Eigen::VectorXd & q1;
const double u;
JointInterpolateVisitor(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) : q0(q0), q1(q1), u(u) {}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.interpolate(q0, q1, u); }
static Eigen::VectorXd run( const JointModelVariant & jmodel, const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u)
{ return boost::apply_visitor( JointInterpolateVisitor(q0, q1, u), jmodel ); }
};
inline Eigen::VectorXd interpolate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
const double u)
{
return JointInterpolateVisitor::run(jmodel, q0, q1, u);
}
/**
* @brief JointRandomVisitor visitor
*/
class JointRandomVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.random(); }
static Eigen::VectorXd run(const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointRandomVisitor(), jmodel ); }
};
inline Eigen::VectorXd random(const JointModelVariant & jmodel)
{
return JointRandomVisitor::run(jmodel);
}
/**
* @brief JointRandomConfigurationVisitor visitor
*/
class JointRandomConfigurationVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
const Eigen::VectorXd & lower_pos_limit;
const Eigen::VectorXd & upper_pos_limit;
JointRandomConfigurationVisitor(const Eigen::VectorXd & lower_pos_limit, const Eigen::VectorXd & upper_pos_limit)
: lower_pos_limit(lower_pos_limit)
, upper_pos_limit(upper_pos_limit)
{}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.randomConfiguration(lower_pos_limit, upper_pos_limit); }
static Eigen::VectorXd run(const JointModelVariant & jmodel,
const Eigen::VectorXd & lower_pos_limit,
const Eigen::VectorXd & upper_pos_limit)
{ return boost::apply_visitor( JointRandomConfigurationVisitor(lower_pos_limit, upper_pos_limit), jmodel ); }
};
inline Eigen::VectorXd randomConfiguration(const JointModelVariant & jmodel,
const Eigen::VectorXd & lower_pos_limit,
const Eigen::VectorXd & upper_pos_limit)
{
return JointRandomConfigurationVisitor::run(jmodel, lower_pos_limit, upper_pos_limit);
}
/**
* @brief JointNeutralConfigurationVisitor visitor
*/
class JointNeutralConfigurationVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.neutralConfiguration(); }
static Eigen::VectorXd run(const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNeutralConfigurationVisitor(), jmodel ); }
};
inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel)
{
return JointNeutralConfigurationVisitor::run(jmodel);
}
/**
* @brief JointNormalizeVisitor visitor
*/
class JointNormalizeVisitor: public boost::static_visitor<>
{
public:
Eigen::VectorXd & q;
JointNormalizeVisitor(Eigen::VectorXd & q) : q(q) {}
template<typename D>
void operator()(const JointModelBase<D> & jmodel) const
{ jmodel.normalize(q); }
static void run(const JointModelVariant & jmodel, Eigen::VectorXd & q)
{ boost::apply_visitor( JointNormalizeVisitor(q), jmodel ); }
};
inline void normalize(const JointModelVariant & jmodel, Eigen::VectorXd & q)
{
JointNormalizeVisitor::run(jmodel, q);
}
/**
* @brief JointIsSameConfigurationVisitor visitor
*/
class JointIsSameConfigurationVisitor: public boost::static_visitor<bool>
{
public:
const Eigen::VectorXd & q1;
const Eigen::VectorXd & q2;
JointIsSameConfigurationVisitor(const Eigen::VectorXd & q1,const Eigen::VectorXd & q2) : q1(q1), q2(q2) {}
template<typename D>
bool operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.isSameConfiguration(q1, q2); }
static bool run(const JointModelVariant & jmodel, const Eigen::VectorXd & q1, const Eigen::VectorXd & q2)
{ return boost::apply_visitor( JointIsSameConfigurationVisitor(q1,q2), jmodel ); }
};
inline bool isSameConfiguration(const JointModelVariant & jmodel,
const Eigen::VectorXd & q1,
const Eigen::VectorXd & q2)
{
return JointIsSameConfigurationVisitor::run(jmodel, q1, q2);
}
/**
* @brief JointDifferenceVisitor visitor
*/
class JointDifferenceVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
const Eigen::VectorXd & q0;
const Eigen::VectorXd & q1;
JointDifferenceVisitor(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) : q0(q0), q1(q1) {}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.difference(q0, q1); }
static Eigen::VectorXd run( const JointModelVariant & jmodel, const Eigen::VectorXd & q0,const Eigen::VectorXd & q1)
{ return boost::apply_visitor( JointDifferenceVisitor(q0, q1), jmodel ); }
};
inline Eigen::VectorXd difference(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return JointDifferenceVisitor::run(jmodel, q0, q1);
}
/**
* @brief JointDistanceVisitor visitor
*/
class JointDistanceVisitor: public boost::static_visitor<double>
{
public:
const Eigen::VectorXd & q0;
const Eigen::VectorXd & q1;
JointDistanceVisitor(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) : q0(q0), q1(q1) {}
template<typename D>
double operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.distance(q0, q1); }
static double run( const JointModelVariant & jmodel, const Eigen::VectorXd & q0,const Eigen::VectorXd & q1)
{ return boost::apply_visitor( JointDistanceVisitor(q0, q1), jmodel ); }
};
inline double distance(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return JointDistanceVisitor::run(jmodel, q0, q1);
}
/**
* @brief JointNvVisitor visitor
*/
......
......@@ -306,110 +306,6 @@ namespace se3
return eps;
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
ConfigVector_t result(nq());
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) = ::se3::integrate(jmodel,qs,vs);
}
return result;
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
ConfigVector_t result(nq());
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) = ::se3::interpolate(jmodel,q0,q1,u);
}
return result;
}
ConfigVector_t random_impl() const
{
ConfigVector_t result(nq());
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) = ::se3::random(jmodel);
}
return result;
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lb,
const ConfigVector_t & ub) const throw (std::runtime_error)
{
ConfigVector_t result(nq());
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) =
::se3::randomConfiguration(jmodel,
lb.segment(idx_q,nq),
ub.segment(idx_q,nq));
}
return result;
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
TangentVector_t result(nv());
for(size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_v = m_idx_v[i];
const int nv = m_nvs[i];
result.segment(idx_v,nv) = ::se3::difference(jmodel,q0,q1);
}
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0,q1).norm();
}
void normalize_impl(Eigen::VectorXd & q) const
{
for (JointModelVector::const_iterator it = joints.begin(); it != joints.end(); ++it)
::se3::normalize(*it,q);
}
ConfigVector_t neutralConfiguration_impl() const
{
ConfigVector_t result(nq());
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) = ::se3::neutralConfiguration(jmodel);
}
return result;
}
bool isSameConfiguration_impl(const Eigen::VectorXd & q1, const Eigen::VectorXd & q2,
const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
if ( !::se3::isSameConfiguration(*i, q1, q2) )
return false;
}
return true;
}
int nv_impl() const { return m_nv; }
int nq_impl() const { return m_nq; }
......
......@@ -264,128 +264,6 @@ namespace se3
using std::sqrt;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const
{
typedef Eigen::Map<SE3::Quaternion_t> QuaternionMap_t;
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());