From cff32e431f16edbcf41015f24eb0bc28255e09b2 Mon Sep 17 00:00:00 2001
From: Justin Carpentier <jcarpent@laas.fr>
Date: Fri, 24 Aug 2018 23:15:42 +0200
Subject: [PATCH] [Joints/All] Remove default init with NAN
Because NAN is a float and may not be converted as it as a CppAD::AD<CppAD::cg::CG<T> > type
---
src/multibody/constraint.hpp | 6 ------
src/multibody/joint/joint-planar.hpp | 5 +++--
.../joint/joint-prismatic-unaligned.hpp | 17 +++++------------
src/multibody/joint/joint-prismatic.hpp | 7 +++----
.../joint/joint-revolute-unaligned.hpp | 18 ++++++------------
.../joint/joint-revolute-unbounded.hpp | 3 +--
src/multibody/joint/joint-revolute.hpp | 5 ++---
src/multibody/joint/joint-spherical-ZYX.hpp | 4 +---
src/multibody/joint/joint-spherical.hpp | 2 +-
src/multibody/joint/joint-translation.hpp | 7 ++-----
src/parsers/srdf.hxx | 2 +-
11 files changed, 25 insertions(+), 51 deletions(-)
diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp
index 5eab8a221..f4767ac1c 100644
--- a/src/multibody/constraint.hpp
+++ b/src/multibody/constraint.hpp
@@ -159,18 +159,12 @@ namespace se3
ConstraintTpl() : S()
{
EIGEN_STATIC_ASSERT(_Dim!=Eigen::Dynamic,YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR)
-#ifndef NDEBUG
- S.fill( NAN );
-#endif
}
// It is only valid for dynamics size
explicit ConstraintTpl(const int dim) : S(6,dim)
{
EIGEN_STATIC_ASSERT(_Dim==Eigen::Dynamic,YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR)
-#ifndef NDEBUG
- S.fill( NAN );
-#endif
}
template<typename VectorLike>
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 5212546be..44a12c26e 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -77,9 +77,10 @@ namespace se3
typedef CartesianAxis<2> AxisZ;
- MotionPlanarTpl () : m_x_dot(NAN), m_y_dot(NAN), m_theta_dot(NAN) {}
+ MotionPlanarTpl() {}
- MotionPlanarTpl (Scalar x_dot, Scalar y_dot, Scalar theta_dot)
+ MotionPlanarTpl(const Scalar & x_dot, const Scalar & y_dot,
+ const Scalar & theta_dot)
: m_x_dot(x_dot), m_y_dot(y_dot), m_theta_dot(theta_dot)
{}
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index 81b1213a7..c29385871 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -74,9 +74,7 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
- MotionPrismaticUnalignedTpl()
- : axis(Vector3::Constant(NAN)), rate(NAN)
- {}
+ MotionPrismaticUnalignedTpl() {}
template<typename Vector3Like, typename S2>
MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
@@ -210,7 +208,7 @@ namespace se3
typedef typename traits<ConstraintPrismaticUnaligned>::JointForce JointForce;
typedef typename traits<ConstraintPrismaticUnaligned>::DenseBase DenseBase;
- ConstraintPrismaticUnaligned() : axis(Vector3::Constant(NAN)) {}
+ ConstraintPrismaticUnaligned() {}
template<typename Vector3Like>
ConstraintPrismaticUnaligned(const Eigen::MatrixBase<Vector3Like> & axis)
@@ -421,18 +419,13 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataPrismaticUnalignedTpl()
- : M(Transformation_t::LinearType::Constant(NAN))
- , S(Constraint_t::Vector3::Constant(NAN))
- , v(Motion_t::Vector3::Constant(NAN),NAN)
- , U(), Dinv(), UDinv()
- {}
+ JointDataPrismaticUnalignedTpl() {}
template<typename Vector3Like>
JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
- : M(Transformation_t::LinearType::Constant(NAN))
+ : M()
, S(axis)
- , v(axis,NAN)
+ , v(axis,(Scalar)NAN)
, U(), Dinv(), UDinv()
{}
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 8733f809d..612abaf31 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -79,8 +79,8 @@ namespace se3
typedef SpatialAxis<_axis+LINEAR> Axis;
typedef typename Axis::CartesianAxis3 CartesianAxis3;
- MotionPrismaticTpl() : rate(NAN) {}
- MotionPrismaticTpl( const Scalar & v ) : rate(v) {}
+ MotionPrismaticTpl() {}
+ MotionPrismaticTpl(const Scalar & v) : rate(v) {}
// inline operator MotionPlain() const { return Axis() * rate; }
@@ -496,8 +496,7 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataPrismaticTpl() : M(NAN), U(), Dinv(), UDinv()
- {}
+ JointDataPrismaticTpl() {}
}; // struct JointDataPrismaticTpl
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 41265c6b4..6e5a44fc3 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -73,7 +73,7 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl);
- MotionRevoluteUnalignedTpl() : axis(Vector3::Constant(NAN)), w(NAN) {}
+ MotionRevoluteUnalignedTpl() {}
template<typename Vector3Like, typename OtherScalar>
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
@@ -223,7 +223,7 @@ namespace se3
typedef typename traits<ConstraintRevoluteUnalignedTpl>::JointForce JointForce;
typedef typename traits<ConstraintRevoluteUnalignedTpl>::DenseBase DenseBase;
- ConstraintRevoluteUnalignedTpl() : axis(Vector3::Constant(NAN)) {}
+ ConstraintRevoluteUnalignedTpl() {}
template<typename Vector3Like>
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
@@ -440,12 +440,7 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataRevoluteUnalignedTpl()
- : M(1)
- , S(Constraint_t::Vector3::Constant(NAN))
- , v(Motion_t::Vector3::Constant(NAN),NAN)
- , U(), Dinv(), UDinv()
- {}
+ JointDataRevoluteUnalignedTpl() {}
template<typename Vector3Like>
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
@@ -471,12 +466,11 @@ namespace se3
using Base::idx_v;
using Base::setIndexes;
- JointModelRevoluteUnalignedTpl() : axis(Eigen::Vector3d::Constant(NAN)) {}
+ JointModelRevoluteUnalignedTpl() {}
- template<typename OtherScalar>
- JointModelRevoluteUnalignedTpl(const OtherScalar x, const OtherScalar y, const OtherScalar z)
+ JointModelRevoluteUnalignedTpl(const Scalar & x, const Scalar & y, const Scalar & z)
+ : axis(x,y,z)
{
- axis << x, y, z ;
axis.normalize();
assert(axis.isUnitary() && "Rotation axis is not unitary");
}
diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index cb8072431..a01ea85ee 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -84,8 +84,7 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataRevoluteUnboundedTpl() : M(NAN,NAN), U(), Dinv(), UDinv()
- {}
+ JointDataRevoluteUnboundedTpl() {}
}; // struct JointDataRevoluteUnbounded
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index c66fd3954..0d5913393 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -208,7 +208,7 @@ namespace se3
typedef SpatialAxis<axis+ANGULAR> Axis;
typedef typename Axis::CartesianAxis3 CartesianAxis3;
- MotionRevoluteTpl() : w(NAN) {}
+ MotionRevoluteTpl() {}
template<typename OtherScalar>
MotionRevoluteTpl(const OtherScalar & w) : w(w) {}
@@ -565,8 +565,7 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataRevoluteTpl() : M(NAN,NAN), U(), Dinv(), UDinv()
- {}
+ JointDataRevoluteTpl() {}
}; // struct JointDataRevoluteTpl
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 347d3b2d3..303b4fdb6 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -76,9 +76,7 @@ namespace se3
typedef typename traits<ConstraintSphericalZYXTpl>::JointForce JointForce;
typedef typename traits<ConstraintSphericalZYXTpl>::DenseBase DenseBase;
- ConstraintSphericalZYXTpl()
- : S_minimal(Matrix3::Constant(NAN))
- {}
+ ConstraintSphericalZYXTpl() {}
template<typename Matrix3Like>
ConstraintSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 0c071b059..126457549 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -76,7 +76,7 @@ namespace se3
MOTION_TYPEDEF_TPL(MotionSphericalTpl);
- MotionSphericalTpl() : w(NAN, NAN, NAN) {}
+ MotionSphericalTpl() {}
template<typename Vector3Like>
MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 59e003698..6be4501fb 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -75,7 +75,7 @@ namespace se3
MOTION_TYPEDEF_TPL(MotionTranslationTpl);
- MotionTranslationTpl() : rate(NAN, NAN, NAN) {}
+ MotionTranslationTpl() {}
template<typename Vector3Like>
MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
@@ -480,10 +480,7 @@ namespace se3
D_t Dinv;
UD_t UDinv;
- JointDataTranslationTpl()
- : M(Transformation_t::LinearType::Constant(NAN))
- , U(), Dinv(), UDinv()
- {}
+ JointDataTranslationTpl() {}
}; // struct JointDataTranslationTpl
diff --git a/src/parsers/srdf.hxx b/src/parsers/srdf.hxx
index 4bb92db7c..614f1352f 100644
--- a/src/parsers/srdf.hxx
+++ b/src/parsers/srdf.hxx
@@ -288,7 +288,7 @@ namespace se3
} // BOOST_FOREACH
assert(false && "no half_sitting configuration found in the srdf file"); // Should we throw something here ?
- return ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType::Constant(model.nq,NAN); // warning : uninitialized vector is returned
+ return ModelTpl<Scalar,Options,JointCollectionTpl>::ConfigVectorType::Constant(model.nq,(Scalar)NAN); // warning : uninitialized vector is returned
}
}
} // namespace se3
--
GitLab