Integrating double diagonalization for damping estimation

cartesian_impedance_controller/CMakeLists.txt

@@ -4,18 +4,23 @@ project(cartesian_impedance_controller)
 set(CMAKE_CXX_STANDARD 17)
 set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter)
 add_compile_options(${ADDITIONAL_COMPILE_OPTIONS})
-
+# build in release
+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)
 
 # Convenience variable for dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
         rclcpp
         effort_controller_base
         Eigen3
+        debug_msg
 )
 
 ament_export_dependencies(

cartesian_impedance_controller/README.md

@@ -48,6 +48,7 @@ cartesian_impedance_controller:
     nullspace_stiffness: 0.0
     compensate_gravity: false
     compensate_coriolis: false
+    compensate_dJdq: false
 
 
 # More controller specifications here

cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -1,10 +1,15 @@
 #ifndef EFFORT_IMPEDANCE_CONTROLLER_H_INCLUDED
 #define EFFORT_IMPEDANCE_CONTROLLER_H_INCLUDED
 
+#include <effort_controller_base/effort_controller_base.h>
+
+#include <controller_interface/controller_interface.hpp>
+
+#include "controller_interface/controller_interface.hpp"
+#include "effort_controller_base/Utility.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
-#include <controller_interface/controller_interface.hpp>
-#include <effort_controller_base/effort_controller_base.h>
+#include "debug_msg/msg/debug.hpp"
 
 namespace cartesian_impedance_controller {
 
@@ -55,7 +60,7 @@ class CartesianImpedanceController
   using Base = effort_controller_base::EffortControllerBase;
 
   ctrl::Matrix6D m_cartesian_stiffness;
-  ctrl::Matrix6D m_cartesian_damping;
+//   ctrl::Matrix6D m_cartesian_damping;
   double m_null_space_stiffness;
   double m_null_space_damping;
   ctrl::Vector6D m_target_wrench;
@@ -73,6 +78,8 @@ class CartesianImpedanceController
       m_target_wrench_subscriber;
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
       m_target_frame_subscriber;
+  rclcpp::Publisher<debug_msg::msg::Debug>::SharedPtr
+      m_data_publisher;
   KDL::Frame m_target_frame;
   ctrl::Vector6D m_ft_sensor_wrench;
   std::string m_ft_sensor_ref_link;
@@ -83,11 +90,10 @@ 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;
   /**
    * Allow users to choose whether to specify their target wrenches in the
    * end-effector frame (= True) or the base frame (= False). The first one

cartesian_impedance_controller/src/cartesian_impedance_controller.cpp

@@ -1,8 +1,4 @@
 #include <cartesian_impedance_controller/cartesian_impedance_controller.h>
-#include <cartesian_impedance_controller/pseudo_inversion.h>
-
-#include "controller_interface/controller_interface.hpp"
-#include "effort_controller_base/Utility.h"
 
 namespace cartesian_impedance_controller {
 
@@ -20,6 +16,7 @@ CartesianImpedanceController::on_init() {
   auto_declare<std::string>("ft_sensor_ref_link", "");
   auto_declare<bool>("hand_frame_control", true);
   auto_declare<double>("nullspace_stiffness", 0.0);
+  auto_declare<bool>("compensate_dJdq", false);
 
   constexpr double default_lin_stiff = 500.0;
   constexpr double default_rot_stiff = 50.0;
@@ -32,7 +29,6 @@ CartesianImpedanceController::on_init() {
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
       CallbackReturn::SUCCESS;
-  ;
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
@@ -76,20 +72,22 @@ CartesianImpedanceController::on_configure(
   tmp[4] = 2 * sqrt(tmp[4]);
   tmp[5] = 2 * sqrt(tmp[5]);
 
-  m_cartesian_damping = tmp.asDiagonal();
+  // m_cartesian_damping = tmp.asDiagonal();
 
   // Set nullspace stiffness
   m_null_space_stiffness =
       get_node()->get_parameter("nullspace_stiffness").as_double();
   RCLCPP_INFO(get_node()->get_logger(), "Postural task stiffness: %f",
               m_null_space_stiffness);
 
+  m_compensate_dJdq = get_node()->get_parameter("compensate_dJdq").as_bool();
+  RCLCPP_INFO(get_node()->get_logger(), "Compensate dJdq: %d",
+              m_compensate_dJdq);
   // Set nullspace damping
   m_null_space_damping = 2 * sqrt(m_null_space_stiffness);
 
   // Set the identity matrix with dimension of the joint space
   m_identity = ctrl::MatrixND::Identity(m_joint_number, m_joint_number);
-
   // Make sure sensor wrenches are interpreted correctly
   // setFtSensorReferenceFrame(Base::m_end_effector_link);
 
@@ -111,6 +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<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::
@@ -135,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::
@@ -161,8 +154,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();
 
@@ -190,7 +184,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.
@@ -219,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);
@@ -229,45 +225,56 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
 
   pseudoInverse(jac.transpose(), &jac_tran_pseudo_inverse);
 
+  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_simulated_joint_motion.data;
+  ctrl::VectorND q_null_space(Base::m_joint_number);
 
   // Compute the motion error
   ctrl::Vector6D motion_error = computeMotionError();
 
   // Initialize the torque vectors
   ctrl::VectorND tau_task(Base::m_joint_number), tau_null(Base::m_joint_number),
-      tau_ext(Base::m_joint_number);
+      tau_ext(Base::m_joint_number), tau_task_old(Base::m_joint_number),
+      tau(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;
+  // init tau to zero
+  tau.setZero();
+  tau_ext.setZero();
+  tau_task_old.setZero();
+  tau_task.setZero();
+  tau_null.setZero();
 
   // Compute the stiffness and damping in the base link
   const auto base_link_stiffness =
       Base::displayInBaseLink(m_cartesian_stiffness, Base::m_end_effector_link);
-  const auto base_link_damping =
-      Base::displayInBaseLink(m_cartesian_damping, Base::m_end_effector_link);
 
-  // Compute the task torque
-  tau_task = jac.transpose() * (base_link_stiffness * motion_error -
-                                (base_link_damping * (jac * q_dot)));
+  Eigen::MatrixXd K_d = base_link_stiffness;
+  Eigen::VectorXd damping_correction = 5.0 * Eigen::VectorXd::Ones(6);
+  auto D_d = compute_correct_damping(Lambda, K_d, 1.0);
 
-  // Compute the null space torque
-  q_null_space = m_q_starting_pose;
-  tau_null = (m_identity - jac.transpose() * jac_tran_pseudo_inverse) *
-             (m_null_space_stiffness * (-q + q_null_space) -
-              m_null_space_damping * q_dot);
+  // 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++) {
+    D_d(i, i) = D_d(i, i) + damping_correction(3);
+  }
 
-  // Compute the torque to achieve the desired force
-  tau_ext = jac.transpose() * m_target_wrench;
+  // 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));
 
-  ctrl::VectorND tau = tau_task + tau_null + tau_ext;
+  // // 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;
 
   KDL::JntArray tau_coriolis(Base::m_joint_number),
       tau_gravity(Base::m_joint_number);
@@ -281,6 +288,56 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
                                       Base::m_joint_velocities, tau_coriolis);
     tau = tau + tau_coriolis.data;
   }
+  // Computes the Jacobian derivative * q_dot, negligible for most of the robot
+  if (m_compensate_dJdq) {
+    KDL::JntArrayVel q_in(Base::m_joint_positions, Base::m_joint_velocities);
+    KDL::Twist jac_dot_q_dot;
+    Base::m_jnt_to_jac_dot_solver->JntToJacDot(q_in, jac_dot_q_dot);
+    // convert KDL::Twist to Eigen::VectorXd
+    Eigen::VectorXd jac_dot_q_dot_eigen(6);
+    jac_dot_q_dot_eigen.head(3) << jac_dot_q_dot.vel.x(), jac_dot_q_dot.vel.y(),
+        jac_dot_q_dot.vel.z();
+    jac_dot_q_dot_eigen.tail(3) << jac_dot_q_dot.rot.x(), jac_dot_q_dot.rot.y(),
+        jac_dot_q_dot.rot.z();
+    Eigen::VectorXd j_tran_lambda_jdot_qdot =
+        jac.transpose() * Lambda * jac_dot_q_dot_eigen;
+    tau = tau + j_tran_lambda_jdot_qdot;
+  }
+  // datas.data.push_back(tau(0) + j_tran_lambda_jdot_qdot(0));
+  // datas.data.push_back(tau(0));
+
+  // add norm of j_tran_lambda_jdot_qdot
+  // datas.data.push_back(j_tran_lambda_jdot_qdot.norm());
+
+  // Compute the null space torque
+  q_null_space = m_q_starting_pose;
+  if (m_null_space_stiffness > 1e-6) {
+
+    // Compute dynamically consistent null space projector
+    tau_null =
+        (m_identity - jac.transpose() * Lambda * jac * M.data.inverse()) *
+        (m_null_space_stiffness * (-q + q_null_space) -
+         m_null_space_damping * q_dot);
+  } else {
+    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(debug_msg);
+
   return tau;
 }
 
@@ -320,7 +377,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>