From cd8f247d45421895efdb34732b946cbdf83c2cdd Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Fri, 1 Dec 2017 13:57:25 +0100
Subject: [PATCH] [Algo] Change normalize algo to directly work with LieGroups
---
src/algorithm/joint-configuration.hpp | 10 +++++++
src/algorithm/joint-configuration.hxx | 42 ++++++++++++++++++---------
unittest/joint-configurations.cpp | 10 +++----
3 files changed, 43 insertions(+), 19 deletions(-)
diff --git a/src/algorithm/joint-configuration.hpp b/src/algorithm/joint-configuration.hpp
index a306028ae..0750f02df 100644
--- a/src/algorithm/joint-configuration.hpp
+++ b/src/algorithm/joint-configuration.hpp
@@ -116,6 +116,16 @@ namespace se3
template<typename LieGroup_t>
inline Eigen::VectorXd randomConfiguration(const Model & model);
+ /**
+ * @brief Normalize a configuration
+ *
+ * @param[in] model Model
+ * @param[in,out] q Configuration to normalize
+ */
+ template<typename LieGroup_t>
+ inline void normalize(const Model & model,
+ Eigen::VectorXd & q);
+
/**
* @brief Normalize a configuration
*
diff --git a/src/algorithm/joint-configuration.hxx b/src/algorithm/joint-configuration.hxx
index c4cf419a2..d6e47e410 100644
--- a/src/algorithm/joint-configuration.hxx
+++ b/src/algorithm/joint-configuration.hxx
@@ -390,31 +390,45 @@ namespace se3
{
return randomConfiguration<LieGroupTpl>(model);
}
-
- struct NormalizeStep : public fusion::JointModelVisitor<NormalizeStep>
+
+ template<typename LieGroup_t, typename JointModel> struct NormalizeStepAlgo;
+
+ template<typename LieGroup_t>
+ struct NormalizeStep : public fusion::JointModelVisitor< NormalizeStep<LieGroup_t> >
{
typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
-
+
JOINT_MODEL_VISITOR_INIT(NormalizeStep);
-
- template<typename JointModel>
- static void algo(const se3::JointModelBase<JointModel> & jmodel,
- Eigen::VectorXd & q)
+
+ SE3_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, LieGroup_t)
+ };
+
+ template<typename LieGroup_t, typename JointModel>
+ struct NormalizeStepAlgo
+ {
+ static void run(const se3::JointModelBase<JointModel> & jmodel,
+ Eigen::VectorXd & qout)
{
- jmodel.normalize(q);
+ LieGroup_t::template operation<JointModel>::type::normalize(jmodel.jointConfigSelector(qout));
}
};
+
+ SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStep, NormalizeStepAlgo);
- inline void
- normalize(const Model & model,
- Eigen::VectorXd & q)
+ template<typename LieGroup_t>
+ inline void normalize(const Model & model, Eigen::VectorXd & qout)
{
- for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+ for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i)
{
- NormalizeStep::run(model.joints[i],
- NormalizeStep::ArgsType (q));
+ NormalizeStep<LieGroup_t>::run(model.joints[i],
+ typename NormalizeStep<LieGroup_t>::ArgsType(qout));
}
}
+
+ inline void normalize(const Model & model, Eigen::VectorXd & qout)
+ {
+ return normalize<LieGroupTpl>(model,qout);
+ }
template<typename LieGroup_t, typename JointModel> struct IsSameConfigurationStepAlgo;
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index 5018f2af8..dade8300f 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -60,13 +60,13 @@ void buildModel(Model & model)
{
addJointAndBody(model,JointModelFreeFlyer(),model.getJointId("universe"),SE3::Identity(),"freeflyer",Inertia::Random());
addJointAndBody(model,JointModelSpherical(),model.getJointId("freeflyer_joint"),SE3::Identity(),"spherical",Inertia::Random());
- addJointAndBody(model,JointModelRX(),model.getJointId("spherical_joint"),SE3::Identity(),"rx",Inertia::Random());
+ addJointAndBody(model,JointModelPlanar(),model.getJointId("spherical_joint"),SE3::Identity(),"planar",Inertia::Random());
+ addJointAndBody(model,JointModelRX(),model.getJointId("planar_joint"),SE3::Identity(),"rx",Inertia::Random());
addJointAndBody(model,JointModelPX(),model.getJointId("rx_joint"),SE3::Identity(),"px",Inertia::Random());
addJointAndBody(model,JointModelPrismaticUnaligned(SE3::Vector3(1,0,0)),model.getJointId("px_joint"),SE3::Identity(),"pu",Inertia::Random());
addJointAndBody(model,JointModelRevoluteUnaligned(SE3::Vector3(0,0,1)),model.getJointId("pu_joint"),SE3::Identity(),"ru",Inertia::Random());
addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("ru_joint"),SE3::Identity(),"sphericalZYX",Inertia::Random());
addJointAndBody(model,JointModelTranslation(),model.getJointId("sphericalZYX_joint"),SE3::Identity(),"translation",Inertia::Random());
- addJointAndBody(model,JointModelPlanar(),model.getJointId("translation_joint"),SE3::Identity(),"planar",Inertia::Random());
}
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
@@ -317,13 +317,13 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
Eigen::VectorXd expected(model.nq);
expected << 0,0,0,0,0,0,1,
0,0,0,1,
+ 0,0,1,0,
0,
0,
0,
0,
0,0,0,
- 0,0,0,
- 0,0,1,0;
+ 0,0,0;
BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results");
@@ -365,7 +365,7 @@ BOOST_AUTO_TEST_CASE ( normalize_test )
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;
+ const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
}
--
GitLab