From 54243c18db50c6e2cb9aba2198ac1f4c8207ca2b Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Thu, 4 Aug 2016 16:30:55 +0200
Subject: [PATCH] [C++] The method forwardKinematics now selects by itself the
section of the right DoFs over a full vector
---
src/multibody/joint/joint-free-flyer.hpp | 13 +++++--------
src/multibody/joint/joint-spherical.hpp | 11 ++++-------
2 files changed, 9 insertions(+), 15 deletions(-)
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 4d0472efa..2fc68d2ca 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -208,9 +208,9 @@ namespace se3
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
- EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
-
- ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
+ typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
+
+ ConstQuaternionMap_t quat(q.template tail<4>().data());
assert(std::fabs(quat.coeffs().norm() - 1.) <= 1e-14);
M.rotation(quat.matrix());
@@ -343,11 +343,8 @@ namespace se3
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
using std::acos;
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
-
- Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q_0);
- Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q_1);
+ Transformation_t M0(Transformation_t::Identity()); forwardKinematics(M0, q0);
+ Transformation_t M1(Transformation_t::Identity()); forwardKinematics(M1, q1);
return se3::log6(M0.inverse()*M1);
}
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 8b6afdccf..b258d0d21 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -271,9 +271,9 @@ namespace se3
template<typename V>
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<V> & q_joint) const
{
- EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,V);
+ typename Eigen::MatrixBase<V>::template ConstFixedSegmentReturnType<NQ>::Type & q = q_joint.template segment<NQ> (idx_q ());
- ConstQuaternionMap_t quat(q_joint.derived().data());
+ ConstQuaternionMap_t quat(q.data());
assert(std::fabs(quat.coeffs().norm() - 1.) <= 1e-14);
M.rotation(quat.matrix());
@@ -388,11 +388,8 @@ namespace se3
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
using std::acos;
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
-
- Transformation_t M0; forwardKinematics(M0, q_0);
- Transformation_t M1; forwardKinematics(M1, q_1);
+ Transformation_t M0; forwardKinematics(M0, q0);
+ Transformation_t M1; forwardKinematics(M1, q1);
return se3::log3((M0.rotation().transpose()*M1.rotation()).eval());
--
GitLab