New/fixing derivs no benchmarks

CMakeLists.txt

@@ -2,6 +2,12 @@ cmake_minimum_required(VERSION 3.13)
 
 project(grbda VERSION 2.1.0)
 
+if(CMAKE_BUILD_TYPE MATCHES Debug)
+    message(STATUS "Debug build")
+    add_compile_options(-O0 -g3)
+    # add_compile_definitions(EIGEN_INITIALIZE_MATRICES_BY_NAN)
+endif()
+
 message(STATUS "=============  !Generalized Rigid-Body Dynamics Algorithms!  =============")
 
 ################################################################################
@@ -18,13 +24,20 @@ option(ASAN "set compile options for Address Sanitizer" OFF)
 ################################################################################
 message(STATUS "======> Find Dependencies")
 
-find_package(Eigen3 REQUIRED)
-if(Eigen3_FOUND)
-  message(STATUS "Eigen3 found")
-  include_directories(${EIGEN3_INCLUDE_DIR})
-  include_directories(${EIGEN3_INCLUDE_DIR}/..)
+
+set(EIGEN_LOCAL_PATH "${PROJECT_SOURCE_DIR}/grbda-dependencies/eigen-3.4.0")
+if(EXISTS "${EIGEN_LOCAL_PATH}/Eigen/StdVector")
+  message(STATUS "Using local Eigen at ${EIGEN_LOCAL_PATH}")
+  include_directories(${EIGEN_LOCAL_PATH})
 else()
-  message(FATAL_ERROR "Eigen3 not found. Please install it or specify its location.")
+  find_package(Eigen3 REQUIRED)
+  if(Eigen3_FOUND)
+    message(STATUS "Eigen3 found via find_package")
+    include_directories(${EIGEN3_INCLUDE_DIR})
+    include_directories(${EIGEN3_INCLUDE_DIR}/..)
+  else()
+    message(FATAL_ERROR "Eigen3 not found. Please install it or specify its location.")
+  endif()
 endif()
 
 find_package(casadi REQUIRED)
@@ -72,7 +85,11 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${PROJECT_SOURCE_DIR}/cmake")
 set(CMAKE_PREFIX_PATH "${CMAKE_PREFIX_PATH};${PROJECT_SOURCE_DIR}/cmake")
 
-set(CMAKE_BUILD_TYPE Release)
+# Set default build type if not specified
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
 if(CMAKE_BUILD_TYPE STREQUAL "Debug")
   add_definitions(-DDEBUG_MODE)
 endif()
@@ -87,14 +104,23 @@ elseif(MARCH_NATIVE)
 endif()
 
 set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -ggdb -Wall \
+
+# Base warning flags for all builds
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ggdb -Wall \
   -Wextra -Wcast-align -Wdisabled-optimization -Wformat=2 \
   -Winit-self -Wmissing-include-dirs -Woverloaded-virtual \
   -Wshadow -Wsign-promo -Wno-sign-compare -Wno-unused-const-variable \
   -Wno-unused-parameter -Wno-unused-variable -Wno-uninitialized")
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3  -ggdb -std=gnu99 -I.")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb -std=gnu99 -I.")
+
+# Optimization flags per build type (don't override Debug's -O0!)
+if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
+endif()
 
 if(ASAN)
+  message(WARNING "ASAN option is deprecated. Use -DCMAKE_BUILD_TYPE=Debug instead.")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
 endif()
 
@@ -170,4 +196,4 @@ endif()
 if(BUILD_BENCHMARKS)
   message(STATUS "======>  Setup Benchmarks  ")
   add_subdirectory(Benchmarking)
-endif()
+endif()

UnitTests/CMakeLists.txt

@@ -1,6 +1,11 @@
 # Create single executable for all tests
 file(GLOB_RECURSE test_sources "*.cpp")
 
+# Tests that provide their own main() — link gtest only (no gmock_main)
+set(TESTS_WITH_CUSTOM_MAIN
+    testInverseDynamicsDerivativesComplexStep
+)
+
 # Create a test for each test file
 foreach(test_source ${test_sources})
     # Extract the file name without the extension
@@ -10,7 +15,11 @@ foreach(test_source ${test_sources})
     add_executable(${test_name} ${test_source})
 
     # Link the necessary libraries
-    target_link_libraries(${test_name} PRIVATE gtest gmock_main ${PROJECT_NAME})
+    if(${test_name} IN_LIST TESTS_WITH_CUSTOM_MAIN)
+        target_link_libraries(${test_name} PRIVATE gtest ${PROJECT_NAME})
+    else()
+        target_link_libraries(${test_name} PRIVATE gtest gmock_main ${PROJECT_NAME})
+    endif()
 
     # Add the test to CTest
     add_test(NAME ${test_name} COMMAND ${test_name})

UnitTests/googletest-src/googletest/CMakeLists.txt

@@ -8,6 +8,10 @@
 # ctest.  You can select which tests to run using 'ctest -R regex'.
 # For more options, run 'ctest --help'.
 
+if(CMAKE_BUILD_TYPE MATCHES Debug)
+  add_compile_options(-Wno-maybe-uninitialized)
+endif()
+
 # When other libraries are using a shared version of runtime libraries,
 # Google Test also has to use one.
 option(

UnitTests/testClusterJoints.cpp

@@ -24,9 +24,9 @@ createBiasVelocityCasadiFunction(std::shared_ptr<ClusterJoints::Base<casadi::SX>
     casadi::copy(cs_qd_sym, qd_sym);
 
     // Set state and update kinematics
-    JointState<SX> joint_state(JointCoordinate<SX>(q_sym, false),
+    JointState<SX> joint_state(JointCoordinate<SX>(q_sym, true),
                                JointCoordinate<SX>(qd_sym, false));
-    joint_state.position = TestHelpers::plus(joint->type(), q_sym, dq_sym);
+    joint_state.position = TestHelpers::plus(joint, q_sym, dq_sym);
     joint->updateKinematics(joint_state);
 
     // Differentiate the motion subspace matrix with repsect to q, ∂S/∂dq

UnitTests/testClusterTreeModel.cpp

@@ -1,6 +1,14 @@
 #include "gtest/gtest.h"
 
+#include <complex>
+#include <cstdlib>
 #include "config.h"
+
+namespace {
+struct TestSeedInitializer {
+    TestSeedInitializer() { std::srand(42); }
+} g_test_seed_initializer;
+}
 #include "grbda/Dynamics/ClusterTreeModel.h"
 #include "grbda/Robots/RobotTypes.h"
 
@@ -106,10 +114,6 @@ std::vector<URDFvsManualTestData> GetTestRobots()
                          std::make_shared<RevoluteChainWithRotor<3, double>>(false)});
     test_data.push_back({urdf_directory + "mini_cheetah.urdf",
                          std::make_shared<MiniCheetah<double>>()});
-    test_data.push_back({urdf_directory + "mit_humanoid_leg.urdf",
-                         std::make_shared<MIT_Humanoid_Leg<double>>()});
-    test_data.push_back({urdf_directory + "mit_humanoid.urdf",
-                         std::make_shared<MIT_Humanoid<double>>()});
     return test_data;
 }
 

UnitTests/testForwardKinematics.cpp

@@ -242,14 +242,31 @@ GTEST_TEST(ForwardKinematics, HumanoidModelComparison)
     for (int i = 0; i < 20; i++)
     {
         // Initialize Random States
-        ModelState<> model_state;
+        ModelState<> rotor_model_state;
+        ModelState<> no_rotor_model_state;
+
         for (const auto &cluster : rotor_model.clusters())
         {
             JointState<> joint_state = cluster->joint_->randomJointState();
-            model_state.push_back(joint_state);
+            rotor_model_state.push_back(joint_state);
+            if( joint_state.position.size() == 4  ) // Revolute Pair with Rotor Joint
+            {
+                const DMat<double> conv = cluster->joint_->spanningTreeToIndependentCoordsConversion().cast<double>();
+                const DVec<double> ind_pos = conv * joint_state.position;
+
+                // Revolute Pair State
+                JointState<> rp_state(JointCoordinate<double>(ind_pos, false),
+                                     JointCoordinate<double>(joint_state.velocity, false));
+
+                no_rotor_model_state.push_back(rp_state);
+            }
+            else
+            {
+                no_rotor_model_state.push_back(joint_state);
+            }
         }
-        rotor_model.setState(model_state);
-        no_rotor_model.setState(model_state);
+        rotor_model.setState(rotor_model_state);
+        no_rotor_model.setState(no_rotor_model_state);
 
         // Forward Kinematics
         rotor_model.forwardKinematicsIncludingContactPoints();