feat: enhance Kuka Cartesian Impedance Controller with CLIK and update README

Author: Hydran00

README.md

@@ -1,10 +1,75 @@
+# Kuka Cartesian Impedance Controller
 ![build badge](https://github.com/lucabeber/effort_controller/actions/workflows/humble.yml/badge.svg)
 ![build badge](https://github.com/lucabeber/effort_controller/actions/workflows/jazzy.yml/badge.svg)
 ![build badge](https://github.com/lucabeber/effort_controller/actions/workflows/rolling.yml/badge.svg)
 
 
 This branch implements command the joint configuration to the [lbr_stack](https://github.com/idra-lab/lbr_fri_ros2_stack) KUKA hardware interface so it allows `Cartesian Impedance Control` as well as `Joint Position Control` depending on the command input chosen as `client_command_mode` in the hardware interface.
 
-Check out their use in the KUKA LBR example [here](https://github.com/idra-lab/kuka_impedance)!  
+This controller is designed to be used with the KUKA FRI in `POSITION` command mode. To switch FRI Command Mode you need to update the following line in the `LBRServer.java` application, chosing between the two controller.
+```
+% for cartesian impedance control
+control_mode_ = new CartesianImpedanceControlMode(0, 0, 0, 0, 0, 0, 0);
+```
+```
+% for cartesian impedance control
+control_mode_ = new PositionControlMode();
+```
 
+|Supported Controller Options| FRI Command Mode   | FRI Controller Mode            |
+|----------------------------|--------------------|--------------------------------|
+|Cartesian Impedance Control |`POSITION`          | `CartesianImpedanceControlMode`|
+|Kinematics control          |`POSITION`          | `PositionControlMode`          |
+
+Check out how to use this controller in our KUKA LBR example [here](https://github.com/idra-lab/kuka_impedance)!  
 The structure of the code and some libraries have been taken from the repo [Cartesian Controllers](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers).
+
+
+#### Parameters:
+Below is an example `controller_manager.yaml` for a controller specific configuration.
+
+```yaml
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    kuka_cartesian_impedance_controller:
+      type: kuka_cartesian_impedance_controller/KukaCartesianImpedanceController
+
+    # More controller instances here
+    # ...
+
+/**/kuka_cartesian_impedance_controller:
+  ros__parameters:
+    max_linear_velocity: 0.3
+    max_angular_velocity: 0.3
+    end_effector_link: "lbr_link_ee"
+    robot_base_link: "lbr_link_0"
+    compliance_ref_link: "lbr_link_ee"
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - lbr_A1
+      - lbr_A2
+      - lbr_A3
+      - lbr_A4
+      - lbr_A5
+      - lbr_A6
+      - lbr_A7
+    
+    # More controllers here
+```
+#### Nullspace target configuration
+The target nullspace configuration can be set in `kuka_cartesian_impedance_controller.cpp` by modifying the `m_q_ns` array. The values are in radians and correspond to the desired joint angles for the nullspace configuration.:
+```
+  m_q_ns(0) = deg2rad(0.0);
+  m_q_ns(1) = deg2rad(40.0);
+  m_q_ns(2) = deg2rad(0.0);
+  m_q_ns(3) = deg2rad(-72.6);
+  m_q_ns(4) = deg2rad(0.0);
+  m_q_ns(5) = deg2rad(80.05);
+  m_q_ns(6) = deg2rad(0.0);
+```

effort_controller_base/include/effort_controller_base/effort_controller_base.h

@@ -12,13 +12,8 @@
 #include <hardware_interface/loaned_command_interface.hpp>
 #include <hardware_interface/loaned_state_interface.hpp>
 #include <kdl/chain.hpp>
-#include <kdl/chaindynparam.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
-#include <kdl/chainfksolvervel_recursive.hpp>
-#include <kdl/chainiksolverpos_lma.hpp>
-#include <kdl/chainiksolverpos_nr_jl.hpp>
-#include <kdl/chainiksolvervel_pinv.hpp>
-#include <kdl/chainiksolvervel_pinv_nso.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
 #include <kdl/frames.hpp>
 #include <kdl/jacobian.hpp>
 #include <kdl/jntarray.hpp>
@@ -179,18 +174,12 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
   std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_to_jac_solver;
   std::shared_ptr<KDL::TreeFkSolverPos_recursive> m_forward_kinematics_solver;
   std::shared_ptr<KDL::ChainFkSolverPos_recursive> m_fk_solver;
-  std::shared_ptr<KDL::ChainIkSolverPos_NR_JL> m_ik_solver;
-
-  std::shared_ptr<KDL::ChainIkSolverVel_pinv_nso> m_ik_solver_vel_nso;
-
-  std::shared_ptr<KDL::ChainIkSolverVel_pinv> m_ik_solver_vel;
-  std::shared_ptr<KDL::ChainDynParam> m_dyn_solver;
+  KDL::JntArray m_q_ns; 
+  KDL::JntArray m_weights;
 
   /**
    * @brief Allow users to choose the IK solver type on startup
    */
-  // std::shared_ptr<pluginlib::ClassLoader<IKSolver> > m_solver_loader;
-  // std::shared_ptr<IKSolver> m_ik_solver;
 
   // Dynamic parameters
   std::string m_end_effector_link;
@@ -237,7 +226,6 @@ class EffortControllerBase : public controller_interface::ControllerInterface {
   KDL::JntArray m_joint_effort_limits;
   double m_delta_tau_max;
 
-  bool m_kuka_hw;
 };
 
 } // namespace effort_controller_base

effort_controller_base/src/effort_controller_base.cpp

@@ -188,6 +188,10 @@ EffortControllerBase::on_configure(
   // Initialize effort limits
   m_joint_effort_limits.resize(m_joint_number);
 
+  // initialize postural task
+  m_q_ns.resize(m_joint_number);
+  m_weights.resize(m_joint_number);
+
   // Parse joint limits
   m_upper_pos_limits.resize(m_joint_number);
   m_lower_pos_limits.resize(m_joint_number);
@@ -231,39 +235,23 @@ EffortControllerBase::on_configure(
   m_forward_kinematics_solver.reset(new KDL::TreeFkSolverPos_recursive(tmp));
   m_fk_solver.reset(new KDL::ChainFkSolverPos_recursive(m_robot_chain));
 
-  m_ik_solver_vel.reset(new KDL::ChainIkSolverVel_pinv(m_robot_chain));
-
-  m_ik_solver.reset(new KDL::ChainIkSolverPos_NR_JL(
-      m_robot_chain, m_lower_pos_limits, m_upper_pos_limits, *m_fk_solver,
-      *m_ik_solver_vel, 100, 1e-6));
-
-  // m_ik_solver.reset(new KDL::ChainIkSolverPos_LMA(m_robot_chain, 1e-4, 1000,
-  // 1e-6));
-
-  KDL::JntArray q_ns(m_joint_number), weights(m_joint_number);
-  q_ns(0) = 0.0;
-  q_ns(1) = 0.78;
-  q_ns(2) = 0.0;
-  q_ns(3) = -1.57;
-  q_ns(4) = 0.0;
-  q_ns(5) = -0.78;
-  q_ns(6) = 0.0;
-
-  weights(0) = 1.0;
-  weights(1) = 1.0;
-  weights(2) = 1.0;
-  weights(3) = 1.0;
-  weights(4) = 1.0;
-  weights(5) = 1.0;
-  weights(6) = 1.0;
-
-  m_ik_solver_vel_nso.reset(
-      new KDL::ChainIkSolverVel_pinv_nso(m_robot_chain, q_ns, weights));
-
-  m_jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(m_robot_chain));
-  m_dyn_solver.reset(new KDL::ChainDynParam(m_robot_chain, grav));
-  RCLCPP_INFO(get_node()->get_logger(),
-              "Finished initializing kinematics solvers");
+  auto deg2rad = [](double deg) { return deg * M_PI / 180.0; };
+
+  m_q_ns(0) = deg2rad(0.0);
+  m_q_ns(1) = deg2rad(40.0);
+  m_q_ns(2) = deg2rad(0.0);
+  m_q_ns(3) = deg2rad(-72.6);
+  m_q_ns(4) = deg2rad(0.0);
+  m_q_ns(5) = deg2rad(80.05);
+  m_q_ns(6) = deg2rad(0.0);
+  
+  m_weights(0) = 1.0;
+  m_weights(1) = 1.0;
+  m_weights(2) = 1.0;
+  m_weights(3) = 1.0; 
+  m_weights(4) = 1.0;
+  m_weights(5) = 1.0;
+  m_weights(6) = 1.0;
 
   RCLCPP_INFO_STREAM(get_node()->get_logger(), "Robot Chain: ");
   for (unsigned int i = 0; i < m_robot_chain.getNrOfSegments(); ++i) {
@@ -304,6 +292,7 @@ EffortControllerBase::on_configure(
   m_old_joint_velocities.data.setZero();
   m_simulated_joint_motion.resize(m_joint_number);
 
+
   RCLCPP_INFO(get_node()->get_logger(), "Finished Base on_configure");
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
       CallbackReturn::SUCCESS;
@@ -388,37 +377,16 @@ void EffortControllerBase::writeJointEffortCmds(
     if (target_joint_positions(i) + 2 * max_velocity > m_upper_pos_limits(i)) {
       // set equal to current
       target_joint_positions(i) = m_upper_pos_limits(i) - 2 * max_velocity - eps;
-      // RCLCPP_INFO(get_node()->get_logger(),
-      //             "Joint %s target is out of limits, setting to %f",
-      //             m_joint_names[i].c_str(), m_joint_positions(i));
     } else if (target_joint_positions(i) - 2 * max_velocity <
                m_lower_pos_limits(i)) {
       // set equal to current
       target_joint_positions(i) = m_lower_pos_limits(i) + 2 * max_velocity + eps;
-      // RCLCPP_INFO(get_node()->get_logger(),
-      //             "Joint %s target is out of limits, setting to %f",
-      //             m_joint_names[i].c_str(), m_joint_positions(i));
     }
 
     m_joint_cmd_pos_handles[i].get().set_value(target_joint_positions(i));
   }
 }
 
-void EffortControllerBase::computeIKSolution(
-    const KDL::Frame &desired_pose, ctrl::VectorND &simulated_joint_positions) {
-  // Invese kinematics
-  int ret = m_ik_solver->CartToJnt(m_joint_positions, desired_pose,
-                                   m_simulated_joint_motion);
-
-  // Check if solution was found
-  if (ret < 0) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Could not find IK solution");
-    return;
-  }
-
-  simulated_joint_positions = m_simulated_joint_motion.data;
-}
-
 ctrl::Vector6D
 EffortControllerBase::displayInBaseLink(const ctrl::Vector6D &vector,
                                         const std::string &from) {

kuka_cartesian_impedance_controller/README.md

@@ -4,9 +4,7 @@
          type="kuka_cartesian_impedance_controller::KukaCartesianImpedanceController"
          base_class_type="controller_interface::ControllerInterface">
     <description>
-      Steer your robot's end-effector with forces and torques.
-      It's ideal for teleoperation with contacts.
+      Cartesian impedance control for FRI implemented with closed-loop inverse kinematics (CLIK).
     </description>
   </class>
-
 </library>