diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index 5c9faad60084f4e92a1622e0810ee890749b540e..9bb6324b6e657cd2e1eb0b13789063d188b4e3b9 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -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;
}
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index cff9e9fbd311077f2d20c2a3296853ada149b5fd..8f79fb2165162c93e57921a81bcc9b487deceb55 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -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.
diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp
index 919b543d6d503bc595c1254aab795cde38064a06..5cfec59b01bf9c506fe0771e7c1eee6948064aa6 100644
--- a/src/multibody/joint/joint-basic-visitors.hpp
+++ b/src/multibody/joint/joint-basic-visitors.hpp
@@ -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
diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx
index 6a68be80fb0891be337a581e5ac0bc754841130d..c0faf57c8d2d649d229709052e55a86b35137a95 100644
--- a/src/multibody/joint/joint-basic-visitors.hxx
+++ b/src/multibody/joint/joint-basic-visitors.hxx
@@ -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);
}
diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp
index cb86cbf3531df5530aafe5fa97a1f84129861607..5d970c15bf300fd5814db392cbdc7e07941fd03d 100644
--- a/src/multibody/joint/joint-dense.hpp
+++ b/src/multibody/joint/joint-dense.hpp
@@ -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() {}
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index c7bf15faf1f05d421ea95ed9538e6a8fe229130b..f6786bd90e79dd0b14decb703ce50288bf05e3c3 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -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
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 15c7404e790d0643f8514f9b2d9a1a6b7541bd34..df7eaac16bf8a7c16952c484b3881a39749a3d7e 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -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
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 8011527b1c409cfbdfa417563b6bb943b2c147bf..8e406a79e1c0518f5144d3b4a6e298476b3a5869 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -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
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index b135d8ef09b6d45c6970cd7858b3203e580e1a60..d7941f51fe0461bba990a24319821cf1552ec871 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -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 ()