From 5208cbc78d23a472f8c2ef282c15c6efc1dd9e04 Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Wed, 9 Dec 2015 19:45:53 +0100
Subject: [PATCH] [C++][Cmake] Set default value in debug mode for data
structure quantities
---
CMakeLists.txt | 7 +++++++
src/multibody/model.hpp | 12 ++++++++++--
2 files changed, 17 insertions(+), 2 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3b349ca4..efa38b3ca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,13 @@ ENDIF(WIN32)
# --- OPTIONS ----------------------------------------
OPTION (BUILD_BENCHMARK "Build the benchmarks" OFF)
+OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+
+IF (INITIALIZE_WITH_NAN)
+ MESSAGE (STATUS "Initialize with NaN all the Eigen entries")
+ ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
+ #LIST(APPEND CMAKE_CXX_FLAGS "-DEIGEN_INITIALIZE_MATRICES_BY_NAN")
+ENDIF (INITIALIZE_WITH_NAN)
# ----------------------------------------------------
# --- DEPENDANCIES -----------------------------------
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index da14e2178..a2ace9e4d 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -347,12 +347,12 @@ namespace se3
,lowerPositionLimit(ref.nq)
,upperPositionLimit(ref.nq)
{
+ /* Create data strcture associated to the joints */
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
- M.fill(NAN);
- for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
+ for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
computeLastChild(ref);
/* Init for Cholesky */
@@ -361,6 +361,14 @@ namespace se3
/* Init Jacobian */
J.fill(0);
+
+ /* Init universe states relatively to itself */
+
+ a[0].setZero();
+ v[0].setZero();
+ f[0].setZero();
+ oMi[0].setIdentity();
+ liMi[0].setIdentity();
}
inline void Data::computeLastChild(const Model& model)
--
GitLab