removed useless vars and added debug_msg package

Author: Hydran00

cartesian_impedance_controller/CMakeLists.txt

@@ -13,12 +13,14 @@ 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)
 
 # Convenience variable for dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
         rclcpp
         effort_controller_base
         Eigen3
+        debug_msg
 )
 
 ament_export_dependencies(

cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -9,7 +9,7 @@
 #include "effort_controller_base/Utility.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
+#include "debug_msg/msg/debug.hpp"
 
 namespace cartesian_impedance_controller {
 
@@ -78,7 +78,7 @@ class CartesianImpedanceController
       m_target_wrench_subscriber;
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
       m_target_frame_subscriber;
-  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
+  rclcpp::Publisher<debug_msg::msg::Debug>::SharedPtr
       m_data_publisher;
   KDL::Frame m_target_frame;
   ctrl::Vector6D m_ft_sensor_wrench;
@@ -90,11 +90,7 @@ class CartesianImpedanceController
 
   ctrl::MatrixND m_identity;
   ctrl::VectorND m_q_starting_pose;
-  ctrl::VectorND m_tau_old;
 
-  ctrl::Vector3D m_old_rot_error;
-  ctrl::VectorND m_old_vel_error;
-  double const m_alpha = 0.3;
   double m_vel_old = 0.0;
   double current_acc_j0 = 0.0;
   bool m_compensate_dJdq = false;

cartesian_impedance_controller/src/cartesian_impedance_controller.cpp

@@ -29,7 +29,6 @@ CartesianImpedanceController::on_init() {
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
       CallbackReturn::SUCCESS;
-  ;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
@@ -110,9 +109,8 @@ CartesianImpedanceController::on_configure(
           get_node()->get_name() + std::string("/target_frame"), 3,
           std::bind(&CartesianImpedanceController::targetFrameCallback, this,
                     std::placeholders::_1));
-  m_data_publisher =
-      get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(
-          get_node()->get_name() + std::string("/data"), 1);
+  m_data_publisher = get_node()->create_publisher<debug_msg::msg::Debug>(
+      get_node()->get_name() + std::string("/data"), 1);
 
   RCLCPP_INFO(get_node()->get_logger(), "Finished Impedance on_configure");
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
@@ -137,13 +135,6 @@ CartesianImpedanceController::on_activate(
 
   m_q_starting_pose = Base::m_joint_positions.data;
 
-  // Initialize the old torque to zero
-  m_tau_old = ctrl::VectorND::Zero(Base::m_joint_number);
-
-  m_old_rot_error = ctrl::Vector3D::Zero();
-
-  m_old_vel_error = ctrl::VectorND::Zero(Base::m_joint_number);
-
   m_target_wrench = ctrl::Vector6D::Zero();
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
@@ -222,6 +213,8 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
   // Compute the forward kinematics
   Base::m_fk_solver->JntToCart(Base::m_joint_positions, m_current_frame);
 
+  debug_msg::msg::Debug debug_msg;
+
   // Compute the jacobian
   Base::m_jnt_to_jac_solver->JntToJac(Base::m_joint_positions,
                                       Base::m_jacobian);
@@ -256,19 +249,14 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
   tau_task_old.setZero();
   tau_task.setZero();
   tau_null.setZero();
-  
-
-  // Filter the velocity error
-  q_dot = m_alpha * q_dot + (1 - m_alpha) * m_old_vel_error;
-  for (int i = 0; i < q_dot.size(); i++) {
-    q_dot(i) = std::round(q_dot(i) * 1000) / 1000;
-  }
-  m_old_vel_error = q_dot;
 
   // Compute the stiffness and damping in the base link
   const auto base_link_stiffness =
       Base::displayInBaseLink(m_cartesian_stiffness, Base::m_end_effector_link);
 
+  RCLCPP_INFO_STREAM(get_node()->get_logger(), "Stiffness: \n"
+                                                   << base_link_stiffness);
+
   // const auto base_link_damping =
   //     Base::displayInBaseLink(m_cartesian_damping,
   //     Base::m_end_effector_link);
@@ -277,22 +265,22 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
   Eigen::VectorXd damping_correction = 5.0 * Eigen::VectorXd::Ones(6);
   auto D_d = compute_correct_damping(Lambda, K_d, 1.0);
 
-  for(int i = 0; i < 6; i++){
-    D_d(i,i) = D_d(i,i) + damping_correction(i);
-  }
-
+  // for(int i = 5; i < 6; i++){
+  D_d(3, 3) = D_d(3, 3) + damping_correction(3);
+  // }
 
   // Compute the task torque
   Eigen::VectorXd Force = (K_d * motion_error - (D_d * (jac * q_dot)));
   tau_task = jac.transpose() * Force;
 
+  Eigen::VectorXd stiffness_torque = jac.transpose() * (K_d * motion_error);
+  Eigen::VectorXd damping_torque = jac.transpose() * -(D_d * (jac * q_dot));
+
   // // add damping force for orientation
   // Eigen::VectorXd damping_force = (4 * K_d.cwiseSqrt() * (jac * q_dot));
   // damping_force.head(3) << 0, 0, 0;
   // Force = Force - damping_force;
 
-  std_msgs::msg::Float64MultiArray datas;
-
   KDL::JntArray tau_coriolis(Base::m_joint_number),
       tau_gravity(Base::m_joint_number);
 
@@ -339,11 +327,21 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
     tau_null = ctrl::VectorND::Zero(Base::m_joint_number);
   }
 
+  for (int i = 0; i < 7; i++) {
+    debug_msg.stiffness_torque[i] = stiffness_torque(i);
+    debug_msg.damping_torque[i] = damping_torque(i);
+    debug_msg.coriolis_torque[i] = tau_coriolis(i);
+    debug_msg.nullspace_torque[i] = tau_null(i);
+    if (i < 6) {
+      debug_msg.impedance_force[i] = Force(i);
+    }
+  }
+
   // Compute the torque to achieve the desired force
   tau_ext = jac.transpose() * m_target_wrench;
 
   tau += tau_task + tau_null + tau_ext;
-  m_data_publisher->publish(datas);
+  m_data_publisher->publish(debug_msg);
 
   return tau;
 }

cbf_cartesian_impedance_controller/include/cbf_cartesian_impedance_controller/cbf_cartesian_impedance_controller.hpp

@@ -74,11 +74,6 @@ class CBFCartesianImpedanceController
 
   ctrl::MatrixND m_identity;
   ctrl::VectorND m_q_starting_pose;
-  ctrl::VectorND m_tau_old;
-
-  ctrl::Vector3D m_old_rot_error;
-  ctrl::VectorND m_old_vel_error;
-  double const m_alpha = 0.3;
   rclcpp::Time m_last_time;
 
   bool m_received_initial_frame = false;

cbf_cartesian_impedance_controller/src/cbf_cartesian_impedance_controller.cpp

@@ -139,13 +139,6 @@ CBFCartesianImpedanceController::on_activate(
 
   m_q_starting_pose = Base::m_joint_positions.data;
 
-  // Initialize the old torque to zero
-  m_tau_old = ctrl::VectorND::Zero(Base::m_joint_number);
-
-  m_old_rot_error = ctrl::Vector3D::Zero();
-
-  m_old_vel_error = ctrl::VectorND::Zero(Base::m_joint_number);
-
   m_target_wrench = ctrl::Vector6D::Zero();
 
   m_last_time = get_node()->get_clock()->now();
@@ -166,8 +159,9 @@ CBFCartesianImpedanceController::on_deactivate(
       CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type CBFCartesianImpedanceController::update(
-    const rclcpp::Time &time, const rclcpp::Duration &period) {
+controller_interface::return_type
+CBFCartesianImpedanceController::update(const rclcpp::Time &time,
+                                        const rclcpp::Duration &period) {
   // Update joint states
   Base::updateJointStates();
 
@@ -195,7 +189,7 @@ ctrl::Vector6D CBFCartesianImpedanceController::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.
@@ -296,13 +290,6 @@ ctrl::VectorND CBFCartesianImpedanceController::computeTorque() {
   ctrl::VectorND tau_task(Base::m_joint_number), tau_null(Base::m_joint_number),
       tau_ext(Base::m_joint_number);
 
-  // Filter the velocity errorm_old_vel_error
-  // q_dot = m_alpha * q_dot + (1 - m_alpha) * m_old_vel_error;
-  // for (int i = 0; i < q_dot.size(); i++) {
-  //   q_dot(i) = std::round(q_dot(i) * 1000) / 1000;
-  // }
-  // m_old_vel_error = q_dot;
-
   // Compute the stiffness and damping in the base link
   const auto base_link_stiffness =
       Base::displayInBaseLink(m_cartesian_stiffness, Base::m_end_effector_link);
@@ -380,7 +367,7 @@ void CBFCartesianImpedanceController::targetFrameCallback(
     m_received_initial_frame = true;
   }
 }
-}  // namespace cbf_cartesian_impedance_controller
+} // namespace cbf_cartesian_impedance_controller
 
 // Pluginlib
 #include <pluginlib/class_list_macros.hpp>

debug_msg/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.22)
+project(debug_msg)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(debug_msg
+  msg/Debug.msg
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+if (BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(FRIClient REQUIRED)
+
+  # link against messages from same package
+  rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
+  add_library(${PROJECT_NAME}_library INTERFACE)
+  target_include_directories(${PROJECT_NAME}_library INTERFACE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+  )
+  target_link_libraries(${PROJECT_NAME}_library INTERFACE
+    ${cpp_typesupport_target}
+  )
+
+endif()
+
+ament_package()

debug_msg/msg/Debug.msg

@@ -0,0 +1,6 @@
+float64[7] commanded_torque
+float64[7] stiffness_torque
+float64[7] damping_torque
+float64[7] coriolis_torque
+float64[7] nullspace_torque
+float64[6] impedance_force

debug_msg/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>debug_msg</name>
+  <version>1.5.0</version>
+  <description>ROS 2 message for debugging effort controller.</description>
+  <maintainer email="davide.nardi-1@unitn.it">Davide</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>builtin_interfaces</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

effort_controller_base/CMakeLists.txt

@@ -23,7 +23,6 @@ find_package(kdl_parser REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(urdf REQUIRED)
-find_package(lbr_fri_idl)
 
 # Convenience variable for dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -33,7 +32,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
         pluginlib
         urdf
         Eigen3
-        lbr_fri_idl
 )
 
 ament_export_dependencies(

effort_controller_base/include/effort_controller_base/effort_controller_base.h

@@ -36,7 +36,6 @@
 #include "controller_interface/helpers.hpp"
 #include "double_diagonalization.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "lbr_fri_idl/msg/lbr_state.hpp"
 #include "pseudo_inversion.h"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
@@ -201,19 +200,14 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
       m_joint_state_pos_handles;
   std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
       m_joint_state_vel_handles;
-  rclcpp::Subscription<lbr_fri_idl::msg::LBRState>::SharedPtr
-      m_lbr_state_subscriber;
-  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
-      m_commanded_torque_publisher;
-  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
-      m_measured_torque_publisher;
-
   size_t m_joint_number;
 
   KDL::JntArray m_joint_positions;
   KDL::JntArray m_joint_velocities;
+  KDL::JntArray m_old_joint_velocities;
   KDL::JntArray m_simulated_joint_motion;
 
+
 private:
   std::vector<std::string> m_cmd_interface_types;
   std::vector<std::string> m_state_interface_types;
@@ -232,7 +226,8 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
   bool m_initialized = {false};
   bool m_configured = {false};
   bool m_active = {false};
-
+  // joint velocity filter
+  const double m_dotq_alpha = 0.3;
   // Dynamic parameters
   std::string m_robot_description;