From 254b0be86591ff6a406a7c12cb14497e5d190b50 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 19 Jul 2016 15:08:41 +0200
Subject: [PATCH] [C++][unittest] Fixed distance computations in joints
---
src/multibody/joint/joint-planar.hpp | 5 +---
.../joint/joint-prismatic-unaligned.hpp | 7 ++----
src/multibody/joint/joint-prismatic.hpp | 5 +---
.../joint/joint-revolute-unaligned.hpp | 7 ++----
src/multibody/joint/joint-revolute.hpp | 7 ++----
src/multibody/joint/joint-spherical-ZYX.hpp | 5 +---
src/multibody/joint/joint-translation.hpp | 5 +---
unittest/joint-configurations.cpp | 24 +++++++------------
8 files changed, 18 insertions(+), 47 deletions(-)
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index b0508ba39..4a8e7f35b 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -422,10 +422,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
-
- return (q_1-q_0).norm();
+ return difference_impl(q0, q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index 89ddc5311..c314b546b 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -421,17 +421,14 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
- ConfigVector_t result;
+ TangentVector_t result;
result << (q_1 - q_0);
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- const Scalar & q_0 = q0[idx_q()];
- const Scalar & q_1 = q1[idx_q()];
-
- return (q_1-q_0);
+ return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 6d58aac4a..18217b60b 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -483,10 +483,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- const Scalar & q_0 = q0[idx_q()];
- const Scalar & q_1 = q1[idx_q()];
-
- return (q_1-q_0);
+ return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 3416bd2da..323b2c68d 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -425,17 +425,14 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
- ConfigVector_t result;
+ TangentVector_t result;
result << (q_1 - q_0);
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- const Scalar & q_0 = q0[idx_q()];
- const Scalar & q_1 = q1[idx_q()];
-
- return (q_1-q_0);
+ return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 3a463d8f5..864de2a5d 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -510,17 +510,14 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
- ConfigVector_t result;
+ TangentVector_t result;
result << (q_1 - q_0);
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- const Scalar & q_0 = q0[idx_q()];
- const Scalar & q_1 = q1[idx_q()];
-
- return (q_1-q_0);
+ return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 0f88052c5..07eed8ee3 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -449,10 +449,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
-
- return (q_1 - q_0).norm();
+ return difference_impl(q0, q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 41e796008..7bfc7e3af 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -361,10 +361,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
- Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
-
- return (q_1 - q_0).norm();
+ return difference_impl(q0, q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index bf2ff08d2..da3bee61b 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -15,18 +15,6 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-/*
- * Compare the value obtained with the RNEA with the values obtained from
- * RBDL. The test is not complete. It only validates the RNEA for the revolute
- * joints. The free-flyer is not tested. It should be extended to account for
- * the free flyer and for the other algorithms.
- *
- * Additionnal notes: the RNEA is an algorithm that can be used to validate
- * many others (in particular, the mass matrix (CRBA) can be numerically
- * validated from the RNEA, then the center-of-mass jacobian can be validated
- * from the mass matrix, etc.
- *
- */
#include <iostream>
#include <iomanip>
@@ -40,6 +28,10 @@
#include <vector>
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE JointConfigurationsTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
bool configurations_are_equals(const Eigen::VectorXd & conf1, const Eigen::VectorXd & conf2)
{
@@ -131,10 +123,6 @@ se3::Model createBoundedModelWithAllJoints()
return model;
}
-#define BOOST_TEST_DYN_LINK
-#define BOOST_TEST_MODULE JointConfigurationsTest
-#include <boost/test/unit_test.hpp>
-#include <boost/utility/binary.hpp>
BOOST_AUTO_TEST_SUITE ( JointConfigurationsTest )
@@ -469,6 +457,10 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test )
result = distance(model,q1,q1);
BOOST_CHECK_MESSAGE(result.isApprox(expected, 1e-12), "Distance between two same configs of full model - wrong results");
+ //
+ // Test Case 3 : distance between q1 and q2 == distance between q2 and q1
+ //
+ BOOST_CHECK_MESSAGE(distance(model, q1, q2) == distance(model, q2, q1), "Distance q1 -> q2 != Distance q2 -> q1");
}
BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
--
GitLab