Need a hybrid position-force controller

kuka_lwr/lwr_controllers/CMakeLists.txt

@@ -82,6 +82,7 @@ set(INC_FILES ${INCLUDE_DIR}/controllers/gravity_compensation.h
               ${INCLUDE_DIR}/controllers/cartesian_position.h
               ${INCLUDE_DIR}/controllers/change_ctrl_mode.h
               ${INCLUDE_DIR}/controllers/passive_ds.h
+              ${INCLUDE_DIR}/controllers/cartesian_hybrid.h
               )
 
 set(SRC_FILES ${SRC_DIR}/controllers/gravity_compensation.cpp
@@ -91,6 +92,7 @@ set(SRC_FILES ${SRC_DIR}/controllers/gravity_compensation.cpp
               ${SRC_DIR}/controllers/ff_fb_cartesian.cpp
               ${SRC_DIR}/controllers/change_ctrl_mode.cpp
               ${SRC_DIR}/controllers/passive_ds.cpp
+              ${SRC_DIR}/controllers/cartesian_hybrid.cpp
 )
 
 set(INC_FILES_UTIL

kuka_lwr/lwr_controllers/include/controllers/base_controllers.h

@@ -10,10 +10,7 @@ class Base_controllers{
 
 public:
 
-    Base_controllers(lwr_controllers::CTRL_MODE  ctrl_mode):ctrl_mode(ctrl_mode)
-    {
-
-    }
+    Base_controllers(lwr_controllers::CTRL_MODE  ctrl_mode):ctrl_mode(ctrl_mode) {}
 
     virtual void stop() = 0;
 

kuka_lwr/lwr_controllers/include/controllers/cartesian_hybrid.h

@@ -0,0 +1,43 @@
+#ifndef  CONTROLLERS_CARTESIAN_POSITION_H_
+#define  CONTROLLERS_CARTESIAN_POSITION_H_
+
+#include "utils/definitions.h"
+#include "controllers/change_ctrl_mode.h"
+#include "controllers/base_controllers.h"
+#include <ros/ros.h>
+
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Wrench.h>
+
+#include <kdl/jntarray.hpp>
+#include <kdl/jntarrayacc.hpp>
+
+namespace controllers{
+
+class Cartesian_hybrid : public Base_controllers {
+
+public:
+
+    Cartesian_hybrid(ros::NodeHandle &nh,controllers::Change_ctrl_mode& change_ctrl_mode);
+
+    void stop();
+
+private:
+
+    void command_cart_pos(const geometry_msgs::PoseConstPtr &msg);
+
+private:
+
+    Change_ctrl_mode&   change_ctrl_mode;
+
+    ros::Subscriber     sub_command_pose_;
+    KDL::Frame          x_des_;	//desired pose
+
+    bool                cmd_flag_, bFirst;
+
+
+};
+
+}
+
+#endif

kuka_lwr/lwr_controllers/include/utils/definitions.h

@@ -9,8 +9,9 @@ enum class CTRL_MODE{
     CART_VELOCITIY,         /// velocity
     CART_POSITION,          /// position
     JOINT_POSITION,         /// standard joint position controller (for goto joint pose)
-    GRAV_COMP,               /// sets the controller into gravity compensation
-    FF_FB_CARTESIAN             /// feedforward + feedback trajectory for the end effector
+    GRAV_COMP,              /// sets the controller into gravity compensation
+    FF_FB_CARTESIAN,        /// feedforward + feedback trajectory for the end effector
+    CART_HYBRID             /// hybrid cartesian impedance - force controller
 };
 
 enum class ROBOT_CTRL_MODE
@@ -32,6 +33,8 @@ inline std::string ctrl_mod2str(CTRL_MODE mode)
         return "GRAV_COMP";
     case CTRL_MODE::FF_FB_CARTESIAN:
         return "FF_FB_CARTESIAN";
+    case CTRL_MODE::CART_HYBRID:
+        return "CART_HYBRID";
     }
 
 }

kuka_lwr/lwr_controllers/src/controllers/cartesian_hybrid.cpp

@@ -0,0 +1,38 @@
+#include "controllers/cartesian_hybrid.h"
+
+namespace controllers{
+
+
+Cartesian_hybrid::Cartesian_hybrid(ros::NodeHandle &nh,controllers::Change_ctrl_mode& change_ctrl_mode):
+    Base_controllers(lwr_controllers::CTRL_MODE::CART_POSITION),change_ctrl_mode(change_ctrl_mode)
+{
+
+    sub_command_pose_      = nh.subscribe("cart_hybrid/cmd_pos",      1, &Cartesian_hybrid::command_cart_pos,     this);
+
+
+}
+
+void Cartesian_hybrid::stop(){
+
+}
+
+void Cartesian_hybrid::command_cart_pos(const geometry_msgs::PoseConstPtr &msg)
+{
+
+   // ROS_INFO("================================================> command_cart_pos callback!");
+    KDL::Frame frame_des_(
+                KDL::Rotation::Quaternion(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w),
+                KDL::Vector(msg->position.x,msg->position.y,msg->position.z));
+
+    x_des_      = frame_des_;
+    cmd_flag_   = true;
+
+    if(!bFirst){
+        change_ctrl_mode.switch_mode(lwr_controllers::CTRL_MODE::CART_POSITION);
+    }
+    bFirst            = true;
+
+}
+
+
+}