Commit 425fa905 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Rename normalized into normalize

parent 262568b8
......@@ -105,14 +105,13 @@ namespace se3
inline Eigen::VectorXd randomConfiguration(const Model & model);
/**
* @brief Normalize a configuration
* @brief Normalize a configuration
*
* @param[in] model Model
* @param[in] q Configuration to normalize
* @return The normalized configuration
* @param[in] model Model
* @param[in,out] q Configuration to normalize
*/
inline Eigen::VectorXd normalized(const Model & model,
const Eigen::VectorXd & q);
inline void normalize(const Model & model,
Eigen::VectorXd & q);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
......@@ -307,35 +306,29 @@ namespace se3
return randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
}
struct NormalizedStep : public fusion::JointModelVisitor<NormalizedStep>
struct NormalizeStep : public fusion::JointModelVisitor<NormalizeStep>
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
JOINT_MODEL_VISITOR_INIT(NormalizedStep);
JOINT_MODEL_VISITOR_INIT(NormalizeStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q,
Eigen::VectorXd & result)
Eigen::VectorXd & q)
{
jmodel.jointConfigSelector(result) = jmodel.normalized(q);
jmodel.normalize(q);
}
};
inline Eigen::VectorXd
normalized(const Model & model,
const Eigen::VectorXd & q)
inline void
normalize(const Model & model,
Eigen::VectorXd & q)
{
Eigen::VectorXd norma(model.nq);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
{
NormalizedStep::run(model.joints[i],
NormalizedStep::ArgsType (q, norma)
);
NormalizeStep::run(model.joints[i],
NormalizeStep::ArgsType (q));
}
return norma;
}
......
......@@ -339,20 +339,15 @@ namespace se3
/**
* @brief Normalize a configuration
*
* @param[in] q Configuration to normalize (size full model.nq)
*
* @return The normalized configuration
* @param[in,out] q Configuration to normalize (size full model.nq)
*/
ConfigVector_t normalized(const Eigen::VectorXd & q) const
{ return derived().normalized_impl(q); }
void normalize(Eigen::VectorXd & q) const
{ return derived().normalize_impl(q); }
/**
* @brief Default implementation of normalized
* @brief Default implementation of normalize
*/
ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
{
return q.segment<NQ>(idx_q());
}
void normalize_impl(Eigen::VectorXd &) const { }
JointIndex i_id; // ID of the joint in the multibody list.
int i_q; // Index of the joint configuration in the joint configuration vector.
......
......@@ -157,16 +157,14 @@ namespace se3
inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointNormalizedVisitor to compute
* @brief Visit a JointModelVariant through JointNormalizeVisitor to compute
* the normalized configuration.
*
* @param[in] jmodel The JointModelVariant
* @param[in] q configuration to normalize
*
* @return The normalized configuration
* @param[in] jmodel The JointModelVariant
* @param[in,out] q configuration to normalize
*/
inline Eigen::VectorXd normalized(const JointModelVariant & jmodel,
const Eigen::VectorXd & q);
inline void normalize(const JointModelVariant & jmodel,
Eigen::VectorXd & q);
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
......
......@@ -233,25 +233,25 @@ namespace se3
}
/**
* @brief JointNormalizedVisitor visitor
* @brief JointNormalizeVisitor visitor
*/
class JointNormalizedVisitor: public boost::static_visitor<Eigen::VectorXd>
class JointNormalizeVisitor: public boost::static_visitor<>
{
public:
const Eigen::VectorXd & q;
Eigen::VectorXd & q;
JointNormalizedVisitor(const Eigen::VectorXd & q) : q(q) {}
JointNormalizeVisitor(Eigen::VectorXd & q) : q(q) {}
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.normalized(q); }
void operator()(const JointModelBase<D> & jmodel) const
{ jmodel.normalize(q); }
static Eigen::VectorXd run(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
{ return boost::apply_visitor( JointNormalizedVisitor(q), jmodel ); }
static void run(const JointModelVariant & jmodel, Eigen::VectorXd & q)
{ boost::apply_visitor( JointNormalizeVisitor(q), jmodel ); }
};
inline Eigen::VectorXd normalized(const JointModelVariant & jmodel, const Eigen::VectorXd & q)
inline void normalize(const JointModelVariant & jmodel, Eigen::VectorXd & q)
{
return JointNormalizedVisitor::run(jmodel, q);
JointNormalizeVisitor::run(jmodel, q);
}
......
......@@ -197,11 +197,9 @@ namespace se3
return result;
}
ConfigVector_t normalized_impl(const Eigen::VectorXd &) const
void normalize_impl(Eigen::VectorXd &) const
{
ConfigVector_t result;
assert(false && "JointModelDense is read-only, should not perform any calc");
return result;
}
JointModelDense() {}
......
......@@ -374,11 +374,9 @@ namespace se3
return q;
}
ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
void normalize_impl(Eigen::VectorXd& q) const
{
ConfigVector_t result (q.segment<NQ>(idx_q()));
result.tail<4>().normalize();
return result;
q.segment<4>(idx_q()+3).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
......
......@@ -399,9 +399,9 @@ namespace se3
return q;
}
ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
void normalize_impl(Eigen::VectorXd& q) const
{
return q.segment<NQ>(idx_q()).normalized();
q.segment<NQ>(idx_q()).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
......
......@@ -146,9 +146,9 @@ namespace se3
return ::se3::distance(*this, q0, q1);
}
ConfigVector_t normalized_impl(const Eigen::VectorXd & q) const
{
return ::se3::normalized(*this, q);
void normalize_impl(Eigen::VectorXd & q) const
{
return ::se3::normalize(*this, q);
}
ConfigVector_t neutralConfiguration_impl() const
......
......@@ -518,7 +518,7 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
}
BOOST_AUTO_TEST_CASE ( normalized_test )
BOOST_AUTO_TEST_CASE ( normalize_test )
{
using namespace se3;
......@@ -526,12 +526,14 @@ BOOST_AUTO_TEST_CASE ( normalized_test )
Model model = createModelWithAllJoints();
se3::Data data(model);
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
Eigen::VectorXd q (Eigen::VectorXd::Ones(model.nq));
se3::normalize(model, q);
Eigen::VectorXd qn = se3::normalized(model, q);
BOOST_CHECK(fabs(qn.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
BOOST_CHECK(fabs(qn.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
BOOST_CHECK(fabs(q.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
BOOST_CHECK(fabs(q.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
const int n = model.nq - 7 - 4;
BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
}
BOOST_AUTO_TEST_SUITE_END ()
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