removed cbf experiments and cleaned up code

Author: Hydran00

README.md

@@ -8,6 +8,5 @@ There is a base controller that communicate with the hardware interface of the r
 For now only a cartesian impedance controller is implemented but also other type of controllers like for example an admittance controller can be easily added. 
 
 Check out their use in the KUKA LBR example [here](https://github.com/idra-lab/kuka_impedance)!  
-This repo also contains a CoppeliaSim hardware interface and scene to test our controllers with simulated Franka robots.
 
 The structure of the code and some libraries have been taken from the repo [Cartesian Controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers).

cartesian_impedance_controller/include/cartesian_impedance_controller/cartesian_impedance_controller.h

@@ -6,10 +6,11 @@
 #include <controller_interface/controller_interface.hpp>
 
 #include "controller_interface/controller_interface.hpp"
+#include "debug_msg/msg/debug.hpp"
 #include "effort_controller_base/Utility.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
-#include "debug_msg/msg/debug.hpp"
+#define DEBUG 0
 
 namespace cartesian_impedance_controller {
 
@@ -38,7 +39,7 @@ namespace cartesian_impedance_controller {
  */
 class CartesianImpedanceController
     : public virtual effort_controller_base::EffortControllerBase {
-public:
+ public:
   CartesianImpedanceController();
 
   virtual LifecycleNodeInterface::CallbackReturn on_init() override;
@@ -52,34 +53,33 @@ class CartesianImpedanceController
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
-  controller_interface::return_type
-  update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+  controller_interface::return_type update(
+      const rclcpp::Time &time, const rclcpp::Duration &period) override;
 
   ctrl::VectorND computeTorque();
 
   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;
 
-private:
+ private:
   ctrl::Vector6D compensateGravity();
 
   void targetWrenchCallback(
       const geometry_msgs::msg::WrenchStamped::SharedPtr wrench);
-  void
-  targetFrameCallback(const geometry_msgs::msg::PoseStamped::SharedPtr target);
+  void targetFrameCallback(
+      const geometry_msgs::msg::PoseStamped::SharedPtr target);
   ctrl::Vector6D computeMotionError();
 
   rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
       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;
+  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;
@@ -103,6 +103,6 @@ class CartesianImpedanceController
   bool m_hand_frame_control;
 };
 
-} // namespace cartesian_impedance_controller
+}  // namespace cartesian_impedance_controller
 
 #endif

cartesian_impedance_controller/package.xml

@@ -6,17 +6,18 @@
   <description>The cartesian_impedance_controller package</description>
   <maintainer email="luca.beber@unitn.it">Luca Beber</maintainer>
   <license>BSD</license>
-  <url type="repository">https://github.com/lucabeber/effort_controllers</url> 
-  <author email="luca.beber@unitn.it">Luca Beber</author>  
+  <url type="repository">https://github.com/lucabeber/effort_controllers</url>
+  <author email="luca.beber@unitn.it">Luca Beber</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>effort_controller_base</depend>
   <depend>controller_interface</depend>
+  <depend>debug_msg</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
-    <controller_interface plugin="${prefix}/cartesian_impedance_controller_plugin.xml"/>
+    <controller_interface plugin="${prefix}/cartesian_impedance_controller_plugin.xml" />
   </export>
-</package>
+</package>

cartesian_impedance_controller/src/cartesian_impedance_controller.cpp

@@ -154,9 +154,8 @@ 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();
 
@@ -184,7 +183,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.
@@ -263,18 +262,11 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
   for (int i = 5; i < 6; i++) {
     D_d(i, i) = D_d(i, i) + 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;
+  // Compute the task torque
+  tau_task = stiffness_torque + damping_torque;
 
   KDL::JntArray tau_coriolis(Base::m_joint_number),
       tau_gravity(Base::m_joint_number);
@@ -303,16 +295,10 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
         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()) *
@@ -322,6 +308,8 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
     tau_null = ctrl::VectorND::Zero(Base::m_joint_number);
   }
 
+#if DEBUG
+  Eigen::VectorXd Force = K_d * motion_error - D_d * (jac * q_dot);
   for (int i = 0; i < 7; i++) {
     debug_msg.stiffness_torque[i] = stiffness_torque(i);
     debug_msg.damping_torque[i] = damping_torque(i);
@@ -331,12 +319,12 @@ ctrl::VectorND CartesianImpedanceController::computeTorque() {
       debug_msg.impedance_force[i] = Force(i);
     }
   }
-
+  m_data_publisher->publish(debug_msg);
+#endif
   // 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;
 }
@@ -377,7 +365,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>