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

Homogenize Joint::configSpace and Device::configurationSpace

parent d2a24eb9
......@@ -252,31 +252,6 @@ namespace hpp {
/* --- CONFIG ----------------------------------------------------------- */
/* ---------------------------------------------------------------------- */
struct ConfigSpaceVisitor : public se3::fusion::JointModelVisitor<ConfigSpaceVisitor>
{
typedef boost::fusion::vector<LiegroupSpace&> ArgsType;
JOINT_MODEL_VISITOR_INIT(ConfigSpaceVisitor);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> &,
LiegroupSpace& space)
{
typedef typename DefaultLieGroupMap::operation<JointModel>::type LG_t;
space *= LiegroupSpace::create (LG_t());
}
};
template <>
void ConfigSpaceVisitor::algo<se3::JointModelComposite>(
const se3::JointModelBase<se3::JointModelComposite> & jmodel,
LiegroupSpace& space)
{
se3::details::Dispatch<ConfigSpaceVisitor>::run(jmodel,
ConfigSpaceVisitor::ArgsType(space));
}
/* Previous implementation of resizeState in hpp::model:: was setting the
* new part of the configuration to neutral configuration. This is not
* working but for empty extra-config. The former behavior is therefore not
......@@ -294,9 +269,8 @@ namespace hpp {
configSpace_ = LiegroupSpace::empty();
const Model& m (model());
ConfigSpaceVisitor::ArgsType args (*configSpace_);
for (JointIndex i = 1; i < m.joints.size(); ++i)
ConfigSpaceVisitor::run(m.joints[i], args);
*configSpace_ *= Joint(weakPtr_, i).configurationSpace();
if (extraConfigSpace_.dimension() > 0)
*configSpace_ *= LiegroupSpace::create (extraConfigSpace_.dimension());
}
......
......@@ -508,41 +508,37 @@ namespace hpp {
return os;
}
class ConfigSpaceVisitor : public boost::static_visitor <>
{
public:
// Initialize liegroupType_ since default constructor
// VectorSpaceOperation<-1, false> () make an assertion fail.
ConfigSpaceVisitor () : liegroupType_
(liegroup::VectorSpaceOperation
<Eigen::Dynamic, false> (0))
{
}
template <typename JointModel> void operator () (JointModel&)
{
typename LieGroupTpl::operation <JointModel>::type tmp;
liegroupType_ = LiegroupType (tmp);
}
struct ConfigSpaceVisitor : public se3::fusion::JointModelVisitor<ConfigSpaceVisitor>
{
typedef boost::fusion::vector<LiegroupSpace&> ArgsType;
void operator () (se3::JointModelComposite&)
{
assert (false && "Method Joint::configurationSpace is not implemented "
"for JointComposite");
}
JOINT_MODEL_VISITOR_INIT(ConfigSpaceVisitor);
const LiegroupType& result () const
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> &,
LiegroupSpace& space)
{
return liegroupType_;
typedef typename DefaultLieGroupMap::operation<JointModel>::type LG_t;
space *= LiegroupSpace::create (LG_t());
}
private:
LiegroupType liegroupType_;
}; // class configSpaceVisitor
};
template <>
void ConfigSpaceVisitor::algo<se3::JointModelComposite>(
const se3::JointModelBase<se3::JointModelComposite> & jmodel,
LiegroupSpace& space)
{
se3::details::Dispatch<ConfigSpaceVisitor>::run(jmodel,
ConfigSpaceVisitor::ArgsType(space));
}
LiegroupSpacePtr_t Joint::configurationSpace () const
{
ConfigSpaceVisitor v;
boost::apply_visitor (v, const_cast <JointModel&> (jointModel ()));
return LiegroupSpace::create (v.result ());
LiegroupSpacePtr_t res = LiegroupSpace::empty();
ConfigSpaceVisitor::ArgsType args(*res);
ConfigSpaceVisitor::run (jointModel(), args);
return res;
}
const JointModel& Joint::jointModel() const
......
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