From 4e65a14f4ee6783cbfe2cb92d0f5272faaeae306 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 20 Jul 2016 13:52:47 +0200
Subject: [PATCH] Add JointModelBase::normalized
---
src/algorithm/joint-configuration.hpp | 42 +++++++++++++++++++++++-
src/multibody/joint/joint-base.hpp | 17 ++++++++++
src/multibody/joint/joint-free-flyer.hpp | 7 ++++
src/multibody/joint/joint-spherical.hpp | 5 +++
unittest/joint-configurations.cpp | 16 +++++++++
5 files changed, 86 insertions(+), 1 deletion(-)
diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index 113e54c32..5c9faad60 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -104,7 +104,15 @@ namespace se3
*/
inline Eigen::VectorXd randomConfiguration(const Model & model);
-
+ /**
+ * @brief Normalize a configuration
+ *
+ * @param[in] model Model
+ * @param[in] q Configuration to normalize
+ * @return The normalized configuration
+ */
+ inline Eigen::VectorXd normalized(const Model & model,
+ const Eigen::VectorXd & q);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
@@ -299,6 +307,38 @@ namespace se3
return randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
}
+ struct NormalizedStep : public fusion::JointModelVisitor<NormalizedStep>
+ {
+ typedef boost::fusion::vector<const Eigen::VectorXd &,
+ Eigen::VectorXd &
+ > ArgsType;
+
+ JOINT_MODEL_VISITOR_INIT(NormalizedStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ const Eigen::VectorXd & q,
+ Eigen::VectorXd & result)
+ {
+ jmodel.jointConfigSelector(result) = jmodel.normalized(q);
+ }
+ };
+
+ inline Eigen::VectorXd
+ normalized(const Model & model,
+ const 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)
+ );
+ }
+ return norma;
+ }
+
+
} // namespace se3
#endif // ifndef __se3_joint_configuration_hpp__
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 923b5dff9..418adbb5e 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -336,6 +336,23 @@ namespace se3
ConfigVector_t neutralConfiguration() const
{ return derived().neutralConfiguration_impl(); }
+ /**
+ * @brief Normalize a configuration
+ *
+ * @param[in] q Configuration to normalize (size full model.nq)
+ *
+ * @return The normalized configuration
+ */
+ ConfigVector_t normalized(const Eigen::VectorXd & q) const
+ { return derived().normalized_impl(q); }
+
+ /**
+ * @brief Default implementation of normalized
+ */
+ ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+ {
+ return q.segment<NQ>(idx_q());
+ }
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-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 4fd2c0dfc..c7bf15faf 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -374,6 +374,13 @@ namespace se3
return q;
}
+ ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+ {
+ ConfigVector_t result (q.segment<NQ>(idx_q()));
+ result.tail<4>().normalize();
+ return result;
+ }
+
JointModelDense<NQ, NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index ca736be4e..15c7404e7 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -399,6 +399,11 @@ namespace se3
return q;
}
+ ConfigVector_t normalized_impl(const Eigen::VectorXd& q) const
+ {
+ return q.segment<NQ>(idx_q()).normalized();
+ }
+
JointModelDense<NQ, NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index bfa14c595..b135d8ef0 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -518,4 +518,20 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
}
+BOOST_AUTO_TEST_CASE ( normalized_test )
+{
+ using namespace se3;
+
+ // Creating the Model and Data
+ Model model = createModelWithAllJoints();
+ se3::Data data(model);
+
+ Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
+
+ 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_AUTO_TEST_SUITE_END ()
--
GitLab