Merge pull request #7 from lucabeber/logging

Logging is enabled if matlogger is found and LOGGIN flas is set to ON

Author: Hydran00

cartesian_impedance_controller/CMakeLists.txt

@@ -3,18 +3,39 @@ 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)
 find_package(debug_msg 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
         rclcpp
@@ -23,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}
 )
@@ -50,42 +76,30 @@ ament_target_dependencies(${PROJECT_NAME}
 # Prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
+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/README.md

@@ -21,6 +21,8 @@ cartesian_impedance_controller:
     robot_base_link: "base_link"
     ft_sensor_ref_link: "sensor_link"
     compliance_ref_link: "compliance_link"
+    max_impedance_force: 10.0 # (N) maximum task force
+    delta_tau_max: 1.0 # (Nm) maximum torque increment in one control cycle
     joints:
       - joint1
       - joint2

cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -10,7 +10,12 @@
 #include "effort_controller_base/Utility.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
+
+
 #define DEBUG 0
+#if LOGGING
+#include <matlogger2/matlogger2.h>
+#endif
 
 namespace cartesian_impedance_controller {
 
@@ -64,6 +69,7 @@ class CartesianImpedanceController
   //   ctrl::Matrix6D m_cartesian_damping;
   double m_null_space_stiffness;
   double m_null_space_damping;
+  double m_max_impendance_force;
   ctrl::Vector6D m_target_wrench;
 
  private:
@@ -80,6 +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

@@ -26,6 +26,7 @@ CartesianImpedanceController::on_init() {
   auto_declare<double>("stiffness.rot_x", default_rot_stiff);
   auto_declare<double>("stiffness.rot_y", default_rot_stiff);
   auto_declare<double>("stiffness.rot_z", default_rot_stiff);
+  auto_declare<double>("max_impedance_force", 70.0); //TODO
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
       CallbackReturn::SUCCESS;
@@ -72,8 +73,8 @@ CartesianImpedanceController::on_configure(
   tmp[4] = 2 * sqrt(tmp[4]);
   tmp[5] = 2 * sqrt(tmp[5]);
 
-  // m_cartesian_damping = tmp.asDiagonal();
-
+  m_max_impendance_force =
+      get_node()->get_parameter("max_impedance_force").as_double(); //TODO
   // Set nullspace stiffness
   m_null_space_stiffness =
       get_node()->get_parameter("nullspace_stiffness").as_double();
@@ -136,7 +137,10 @@ CartesianImpedanceController::on_activate(
   m_q_starting_pose = Base::m_joint_positions.data;
 
   m_target_wrench = ctrl::Vector6D::Zero();
-
+#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;
 }
@@ -154,8 +158,9 @@ CartesianImpedanceController::on_deactivate(
       CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type CartesianImpedanceController::update(
-    const rclcpp::Time &time, const rclcpp::Duration &period) {
+controller_interface::return_type
+CartesianImpedanceController::update(const rclcpp::Time &time,
+                                     const rclcpp::Duration &period) {
   // Update joint states
   Base::updateJointStates();
 
@@ -183,7 +188,7 @@ ctrl::Vector6D CartesianImpedanceController::computeMotionError() {
   // Use Rodrigues Vector for a compact representation of orientation errors
   // Only for angles within [0,Pi)
   KDL::Vector rot_axis = KDL::Vector::Zero();
-  double angle = error_kdl.M.GetRotAngle(rot_axis);  // rot_axis is normalized
+  double angle = error_kdl.M.GetRotAngle(rot_axis); // rot_axis is normalized
   double distance = error_kdl.p.Normalize();
 
   // Clamp maximal tolerated error.
@@ -209,6 +214,11 @@ ctrl::Vector6D CartesianImpedanceController::computeMotionError() {
 }
 
 ctrl::VectorND CartesianImpedanceController::computeTorque() {
+  // Redefine joints velocities in Eigen format
+  ctrl::VectorND q = Base::m_joint_positions.data;
+  ctrl::VectorND q_dot = Base::m_joint_velocities.data;
+  ctrl::VectorND q_null_space(Base::m_joint_number);
+
   // Compute the forward kinematics
   Base::m_fk_solver->JntToCart(Base::m_joint_positions, m_current_frame);
 
@@ -226,14 +236,8 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
 
   KDL::JntSpaceInertiaMatrix M(Base::m_joint_number);
   m_dyn_solver->JntToMass(Base::m_joint_positions, M);
-
   Eigen::MatrixXd Lambda = (jac * M.data.inverse() * jac.transpose()).inverse();
 
-  // Redefine joints velocities in Eigen format
-  ctrl::VectorND q = Base::m_joint_positions.data;
-  ctrl::VectorND q_dot = Base::m_joint_velocities.data;
-  ctrl::VectorND q_null_space(Base::m_joint_number);
-
   // Compute the motion error
   ctrl::Vector6D motion_error = computeMotionError();
 
@@ -254,12 +258,12 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
       Base::displayInBaseLink(m_cartesian_stiffness, Base::m_end_effector_link);
 
   Eigen::MatrixXd K_d = base_link_stiffness;
-  Eigen::VectorXd damping_correction = 5.0 * Eigen::VectorXd::Ones(6);
+  Eigen::VectorXd damping_correction = 3.0 * Eigen::VectorXd::Ones(6);
   auto D_d = compute_correct_damping(Lambda, K_d, 1.0);
 
   // add a small damping correction to the diagonal of D_d to account for model
   // inaccuracies, remove this loop if you face strange behavior
-  for (int i = 5; i < 6; i++) {
+  for (int i = 3; i < 6; i++) {
     D_d(i, i) = D_d(i, i) + damping_correction(3);
   }
   Eigen::VectorXd stiffness_torque = jac.transpose() * (K_d * motion_error);
@@ -320,6 +324,26 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
     }
   }
   m_data_publisher->publish(debug_msg);
+#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));
+    m_logger->add("coriolis_" + std::to_string(i), tau_coriolis(i));
+    m_logger->add("nullspace_" + std::to_string(i), tau_null(i));
+    if (i < 6) {
+      m_logger->add("impedance_force_" + std::to_string(i), Force(i));
+    }
+  }
 #endif
   // Compute the torque to achieve the desired force
   tau_ext = jac.transpose() * m_target_wrench;
@@ -365,7 +389,7 @@ void CartesianImpedanceController::targetFrameCallback(
                  KDL::Vector(target->pose.position.x, target->pose.position.y,
                              target->pose.position.z));
 }
-}  // namespace cartesian_impedance_controller
+} // namespace cartesian_impedance_controller
 
 // Pluginlib
 #include <pluginlib/class_list_macros.hpp>