From e7698045642d347f551aa804b1d4993226bbac25 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 1 Mar 2023 16:02:32 +0100
Subject: [PATCH] wip

---
 CMakeLists.txt                               |  5 ++-
 demos/solo_imu_kine.cpp                      |  2 +-
 demos/solo_imu_kine_mocap.cpp                |  2 +-
 demos/solo_imu_mocap.cpp                     |  2 +-
 demos/solo_kine_mocap.cpp                    |  2 +-
 demos/solo_real_povcdl_estimation.cpp        |  2 +-
 include/bodydynamics/common/bodydynamics.h   | 34 ++++++++++++++++++++
 internal/config.h.in                         |  7 ++--
 test/gtest_factor_force_torque.cpp           |  2 +-
 test/gtest_factor_inertial_kinematics.cpp    |  2 +-
 test/gtest_processor_force_torque_preint.cpp |  2 +-
 test/gtest_processor_inertial_kinematics.cpp |  2 +-
 test/gtest_processor_point_feet_nomove.cpp   |  2 +-
 13 files changed, 51 insertions(+), 15 deletions(-)
 create mode 100644 include/bodydynamics/common/bodydynamics.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ffc262..fd7799a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -77,7 +77,7 @@ FIND_PACKAGE(wolfimu REQUIRED)
 FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
 
 # ============ CONFIG.H ============ 
-set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+set(_WOLF_CODE_DIR ${CMAKE_SOURCE_DIR})
 # variable used to compile the config.h.in file
 string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
 
@@ -110,6 +110,9 @@ include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h
 include/${PROJECT_NAME}/capture/capture_leg_odom.h
 include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h
   )
+SET(HDRS_COMMON
+include/${PROJECT_NAME}/common/bodydynamics.h
+  )
 SET(HDRS_FACTOR
 include/${PROJECT_NAME}/factor/factor_force_torque.h
 include/${PROJECT_NAME}/factor/factor_force_torque_preint.h
diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp
index d1fa022..944c9e4 100644
--- a/demos/solo_imu_kine.cpp
+++ b/demos/solo_imu_kine.cpp
@@ -271,7 +271,7 @@ int main (int argc, char **argv) {
     /////////////////////////
     // WOLF enters the scene
     // SETUP PROBLEM/SENSORS/PROCESSORS
-    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
     ProblemPtr problem = Problem::create("POV", 3);
 
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index 5322aaf..0470b43 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -247,7 +247,7 @@ int main (int argc, char **argv) {
     /////////////////////////
     // WOLF enters the scene
     // SETUP PROBLEM/SENSORS/PROCESSORS
-    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
     ProblemPtr problem = Problem::create("POV", 3);
 
diff --git a/demos/solo_imu_mocap.cpp b/demos/solo_imu_mocap.cpp
index 1f89d69..5de5db9 100644
--- a/demos/solo_imu_mocap.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -215,7 +215,7 @@ int main (int argc, char **argv) {
     /////////////////////////
     // WOLF enters the scene
     // SETUP PROBLEM/SENSORS/PROCESSORS
-    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
     ProblemPtr problem = Problem::create("POV", 3);
 
diff --git a/demos/solo_kine_mocap.cpp b/demos/solo_kine_mocap.cpp
index 2454cac..612cf12 100644
--- a/demos/solo_kine_mocap.cpp
+++ b/demos/solo_kine_mocap.cpp
@@ -217,7 +217,7 @@ int main (int argc, char **argv) {
     /////////////////////////
     // WOLF enters the scene
     // SETUP PROBLEM/SENSORS/PROCESSORS
-    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
     ProblemPtr problem = Problem::create("PO", 3);
     // ProblemPtr problem = Problem::create("POV", 3);
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 902e995..5243ee6 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -214,7 +214,7 @@ int main (int argc, char **argv) {
     /////////////////////////
     // WOLF enters the scene
     // SETUP PROBLEM/SENSORS/PROCESSORS
-    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
     ProblemPtr problem = Problem::create("POVCDL", 3);
 
diff --git a/include/bodydynamics/common/bodydynamics.h b/include/bodydynamics/common/bodydynamics.h
new file mode 100644
index 0000000..e0722b9
--- /dev/null
+++ b/include/bodydynamics/common/bodydynamics.h
@@ -0,0 +1,34 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+// Enable project-specific definitions and macros
+#include "bodydynamics/internal/config.h"
+#include <core/common/wolf.h>
+
+namespace wolf
+{
+
+// Folder schema Registry
+WOLF_REGISTER_FOLDER(_WOLF_BODYDYNAMICS_SCHEMA_DIR);
+
+}
diff --git a/internal/config.h.in b/internal/config.h.in
index 35f3468..727b1bc 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,13 +24,12 @@
 //            which will be added to the include path for compilation,
 //            and installed with the public wolf headers.
 
-#ifndef WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
-#define WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
+#pragma once
 
 #cmakedefine _WOLF_DEBUG
 
 #cmakedefine _WOLF_TRACE
 
-#define _WOLF_${PROJECT_NAME_UPPER}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_${PROJECT_NAME_UPPER}_CODE_DIR "${_WOLF_CODE_DIR}"
 
-#endif /* WOLF_INTERNAL_CONFIG_H_ */
+#define _WOLF_${PROJECT_NAME_UPPER}_SCHEMA_DIR "${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/schema"
\ No newline at end of file
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index d459963..43a82e1 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -208,7 +208,7 @@ class FactorInertialKinematics_2KF : public testing::Test
     void SetUp() override
     {
 
-        std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+        std::string bodydynamics_root = _WOLF_BODYDYNAMICS_CODE_DIR;
 
         mass_ = 10.0; // Small 10 kg robot
         //===================================================== SETTING PROBLEM
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 7d4ad65..98fbd3c 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -288,7 +288,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
 //        // using make_shared;
 //        // using static_pointer_cast;
 //
-//        std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+//        std::string bodydynamics_root = _WOLF_BODYDYNAMICS_CODE_DIR;
 //
 //        //===================================================== SETTING PROBLEM
 //        // WOLF PROBLEM
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 98ff733..7b4dfea 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -97,7 +97,7 @@ public:
 
     void SetUp() override
     {
-        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
         problem_ = Problem::create("POVCDL", 3);
 
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 3e044b7..1e3d31f 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -79,7 +79,7 @@ class FactorInertialKinematics_2KF : public testing::Test
 
     void SetUp() override
     {
-        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
         problem_ = Problem::create("POVCDL", 3);
 
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
index 2e36fe3..2a9c36f 100644
--- a/test/gtest_processor_point_feet_nomove.cpp
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -80,7 +80,7 @@ class PointFeetCaptures : public testing::Test
 
     void SetUp() override
     {
-        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+        std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_CODE_DIR;
 
         problem_ = Problem::create("PO", 3);
 
-- 
GitLab