Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
src/multibody/joint/joint-base.hpp
View file @ fb14d535
......@@ -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.
......
src/multibody/joint/joint-basic-visitors.hpp
View file @ fb14d535
......@@ -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
......
src/multibody/joint/joint-basic-visitors.hxx
View file @ fb14d535
......@@ -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
*/
......
src/multibody/joint/joint-composite.hpp
View file @ fb14d535
......@@ -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; }
......
src/multibody/joint/joint-free-flyer.hpp
View file @ fb14d535
......@@ -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 ());