add cart imp mode to hwif and manager node

kroshu · Oct 15, 2024 · f26cf0c · f26cf0c
1 parent 419c591
commit f26cf0c
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp
@@ -213,6 +213,11 @@ CallbackReturn KukaFRIHardwareInterface::on_activate(const rclcpp_lifecycle::Sta
       fri_connection_->setPositionControlMode();
       fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE);
       break;
+    case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL:
+      fri_connection_->setCartesianImpedanceControlMode(
+        hw_cart_stiffness_commands_, hw_cart_damping_commands_);
+      fri_connection_->setClientCommandMode(ClientCommandModeID::CARTESIAN_POSE_COMMAND_MODE);
+      break;
     case kuka_drivers_core::ControlMode::WRENCH_CONTROL:
       fri_connection_->setCartesianImpedanceControlMode(
         hw_cart_stiffness_commands_, hw_cart_damping_commands_);
@@ -378,6 +383,8 @@ void KukaFRIHardwareInterface::updateCommand(const rclcpp::Time &)
       break;
     }
     case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL:
+      [[fallthrough]];
+    case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL:
     {
       const double * cartesianPoseQuaternion = hw_cart_pose_commands_.data();
       RCLCPP_DEBUG(

diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp
@@ -313,6 +313,8 @@ bool RobotManagerNode::onControlModeChangeRequest(int control_mode)
       break;
     case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL:
       break;
+    case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL:
+      break;
     case kuka_drivers_core::ControlMode::WRENCH_CONTROL:
       [[fallthrough]];
     case kuka_drivers_core::ControlMode::JOINT_TORQUE_CONTROL:
@@ -447,6 +449,8 @@ std::string RobotManagerNode::GetControllerName() const
       return joint_torque_controller_name_;
     case kuka_drivers_core::ControlMode::CARTESIAN_POSITION_CONTROL:
       return cart_pose_controller_name_;
+    case kuka_drivers_core::ControlMode::CARTESIAN_IMPEDANCE_CONTROL:
+      return cart_pose_controller_name_;
     case kuka_drivers_core::ControlMode::WRENCH_CONTROL:
       return wrench_controller_name_;
     default: