Commit 262568b8 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add JointNormalizedVisitor and JointModel::normalized_impl

parent 4e65a14f
......@@ -349,7 +349,7 @@ namespace se3
/**
* @brief Default implementation of normalized
*/
ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
{
return q.segment<NQ>(idx_q());
}
......
......@@ -155,6 +155,18 @@ namespace se3
* @return The joint's neutral configuration
*/
inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointNormalizedVisitor to compute
* the normalized configuration.
*
* @param[in] jmodel The JointModelVariant
* @param[in] q configuration to normalize
*
* @return The normalized configuration
*/
inline Eigen::VectorXd normalized(const JointModelVariant & jmodel,
const Eigen::VectorXd & q);
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
......
......@@ -232,6 +232,28 @@ namespace se3
return JointNeutralConfigurationVisitor::run(jmodel);
}
/**
* @brief JointNormalizedVisitor visitor
*/
class JointNormalizedVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
const Eigen::VectorXd & q;
JointNormalizedVisitor(const Eigen::VectorXd & q) : q(q) {}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.normalized(q); }
static Eigen::VectorXd run(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
{ return boost::apply_visitor( JointNormalizedVisitor(q), jmodel ); }
};
inline Eigen::VectorXd normalized(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
{
return JointNormalizedVisitor::run(jmodel, q);
}
/**
* @brief JointDifferenceVisitor visitor
......
......@@ -197,6 +197,13 @@ namespace se3
return result;
}
ConfigVector_t normalized_impl(const Eigen::VectorXd &) const
{
ConfigVector_t result;
assert(false && "JointModelDense is read-only, should not perform any calc");
return result;
}
JointModelDense() {}
JointModelDense(JointIndex idx, int idx_q, int idx_v)
{
......
......@@ -146,6 +146,11 @@ namespace se3
return ::se3::distance(*this, q0, q1);
}
ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
{
return ::se3::normalized(*this, q);
}
ConfigVector_t neutralConfiguration_impl() const
{
return ::se3::neutralConfiguration(*this);
......
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