matlogger is now optional

Author: Hydran00

cartesian_impedance_controller/CMakeLists.txt

@@ -3,19 +3,38 @@ project(cartesian_impedance_controller)
 
 set(CMAKE_CXX_STANDARD 17)
 set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter)
+
+# Optional parameter to enable logging, only if MatLogger2 is found
+# Refer to https://github.com/ADVRHumanoids/MatLogger2 to install MatLogger2
+# output file is /tmp/cart_impedance_log<timestamp>.mat
+option(LOGGING "Enable logging" ON)
 add_compile_options(${ADDITIONAL_COMPILE_OPTIONS})
-# build in release
+
+# Build in release mode if not specified
 if(NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
+
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 add_definitions(-DEIGEN_MPL2_ONLY)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(effort_controller_base REQUIRED)
-# log and debug
 find_package(debug_msg REQUIRED)
-find_package(matlogger2 REQUIRED)
+
+# Try to find MatLogger2 if logging is enabled
+if (LOGGING)
+  find_package(matlogger2 QUIET)
+  if (matlogger2_FOUND)
+    message(STATUS "MatLogger2 found: Logging enabled")
+    set(LOGGING_ENABLED TRUE)
+  else()
+    message(WARNING "MatLogger2 not found: Logging disabled")
+    set(LOGGING_ENABLED FALSE)
+  endif()
+else()
+  set(LOGGING_ENABLED FALSE)
+endif()
 
 # Convenience variable for dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -25,6 +44,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
         debug_msg
 )
 
+if (LOGGING_ENABLED)
+  list(APPEND THIS_PACKAGE_INCLUDE_DEPENDS matlogger2)
+  add_compile_definitions(LOGGING)
+endif()
+
 ament_export_dependencies(
         ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
@@ -51,45 +75,31 @@ ament_target_dependencies(${PROJECT_NAME}
 
 # Prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
-target_link_libraries(${PROJECT_NAME}
-  matlogger2::matlogger2
-)
+
+if (LOGGING_ENABLED)
+  target_link_libraries(${PROJECT_NAME} matlogger2::matlogger2)
+endif()
+
 #--------------------------------------------------------------------------------
 # Install and export
 #--------------------------------------------------------------------------------
 
 pluginlib_export_plugin_description_file(controller_interface cartesian_impedance_controller_plugin.xml)
 
-# Note: The workflow as described here https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html#building-a-library
-# does not work for me.
-# I'm not sure what's the problem right now, but I need to out-comment the
-# lines below to achieve a correct symlink-install.
-
-#ament_export_targets(my_targets_from_this_package HAS_LIBRARY_TARGET)
-
 install(
   DIRECTORY include/
   DESTINATION include
 )
 
 install(
   TARGETS ${PROJECT_NAME}
-  #EXPORT my_targets_from_this_package
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
-  #INCLUDES DESTINATION include
 )
 
-# Note: For the target based workflow, they seem to be superfluous.
-# But since that doesn't work yet, I'll add them just in case.
-# I took the joint_trajectory_controller as inspiration.
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
-ament_export_include_directories(
-  include
-)
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
 
 ament_package()

cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -10,15 +10,12 @@
 #include "effort_controller_base/Utility.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
-<<<<<<< Updated upstream
-#include <matlogger2/matlogger2.h>
 
-#define DEBUG 0
-#define LOGGING 1
-=======
 
-#define DEBUG 1
->>>>>>> Stashed changes
+#define DEBUG 0
+#if LOGGING
+#include <matlogger2/matlogger2.h>
+#endif
 
 namespace cartesian_impedance_controller {
 
@@ -89,7 +86,9 @@ class CartesianImpedanceController
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
       m_target_frame_subscriber;
   rclcpp::Publisher<debug_msg::msg::Debug>::SharedPtr m_data_publisher;
+#if LOGGING
   XBot::MatLogger2::Ptr m_logger;
+#endif
   KDL::Frame m_target_frame;
   ctrl::Vector6D m_ft_sensor_wrench;
   std::string m_ft_sensor_ref_link;

cartesian_impedance_controller/src/cartesian_impedance_controller.cpp

@@ -137,9 +137,10 @@ CartesianImpedanceController::on_activate(
   m_q_starting_pose = Base::m_joint_positions.data;
 
   m_target_wrench = ctrl::Vector6D::Zero();
-
-  m_logger = XBot::MatLogger2::MakeLogger("/tmp/my_log");
+#if LOGGING
+  m_logger = XBot::MatLogger2::MakeLogger("/tmp/cart_impedance_log");
   m_logger->set_buffer_mode(XBot::VariableBuffer::Mode::circular_buffer);
+#endif
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
       CallbackReturn::SUCCESS;
 }
@@ -237,46 +238,6 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
   m_dyn_solver->JntToMass(Base::m_joint_positions, M);
   Eigen::MatrixXd Lambda = (jac * M.data.inverse() * jac.transpose()).inverse();
 
-  // RCLCPP_INFO_STREAM(get_node()->get_logger(), "M: " << M.data);
-
-  // lambda that computes condition number
-  auto compute_condition_number = [](const Eigen::MatrixXd &matrix) {
-    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
-    Eigen::VectorXd singular_values = svd.singularValues();
-    return singular_values(0) / singular_values(singular_values.size() - 1);
-  };
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Condition number of Jacobian: " << compute_condition_number(jac));
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Condition number of M: " << compute_condition_number(M.data));
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Condition number of Lambda: " << compute_condition_number(Lambda));
-
-  // // compue eigenvalues of M
-  // Eigen::EigenSolver<Eigen::MatrixXd> es(M.data);
-  // Eigen::VectorXd eigenvalues = es.eigenvalues().real();
-  // RCLCPP_INFO_STREAM(get_node()->get_logger(),
-  //                    "Eigenvalues of M: " << eigenvalues.transpose());
-  // RCLCPP_INFO_STREAM(get_node()->get_logger(),
-  //                    "Condition number of M: " << eigenvalues.maxCoeff() /
-  //                                                     eigenvalues.minCoeff());
-  // Eigen::EigenSolver<Eigen::MatrixXd> es3(jac);
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Eigenvalues of Lambda: " << es3.eigenvalues().real().transpose());
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Condition number of Lambda: " << es3.eigenvalues().real().maxCoeff() /
-  //                                           es3.eigenvalues().real().minCoeff());
-  // Eigen::JacobiSVD<Eigen::MatrixXd> svd(jac);
-  // RCLCPP_INFO_STREAM(
-  //     get_node()->get_logger(),
-  //     "Eigenvalues of Jacobian: " << svd.singularValues().transpose());
-  // RCLCPP_INFO_STREAM(get_node()->get_logger(), "----");
-
   // Compute the motion error
   ctrl::Vector6D motion_error = computeMotionError();
 
@@ -366,7 +327,14 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
 #endif
 #if LOGGING
   Eigen::VectorXd Force = K_d * motion_error - D_d * (jac * q_dot);
-
+  // lambda that computes condition number
+  auto compute_condition_number = [](const Eigen::MatrixXd &matrix) {
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
+    Eigen::VectorXd singular_values = svd.singularValues();
+    return singular_values(0) / singular_values(singular_values.size() - 1);
+  };
+  m_logger->add("condition_number mass", compute_condition_number(M.data));
+  m_logger->add("condition_number jac", compute_condition_number(jac));
   for (int i = 0; i < 7; i++) {
     m_logger->add("stiffness_" + std::to_string(i), stiffness_torque(i));
     m_logger->add("damping_" + std::to_string(i), damping_torque(i));