controller modifications for name prefix (#189)

lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml

@@ -1,7 +1,7 @@
 /**/admittance_control:
   ros__parameters:
-    base_link: "link_0"
-    end_effector_link: "link_ee"
+    base_link: "lbr_link_0"
+    end_effector_link: "lbr_link_ee"
     f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5]
     dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0]
     dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp

@@ -32,8 +32,8 @@ class PoseControlNode : public LBRBasePositionCommandNode {
       return;
     }
 
-    this->declare_parameter<std::string>("base_link", "link_0");
-    this->declare_parameter<std::string>("end_effector_link", "link_ee");
+    this->declare_parameter<std::string>("base_link", "lbr_link_0");
+    this->declare_parameter<std::string>("end_effector_link", "lbr_link_ee");
 
     std::string root_link = this->get_parameter("base_link").as_string();
     std::string tip_link = this->get_parameter("end_effector_link").as_string();

lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml

@@ -1,7 +1,7 @@
 /**/admittance_control:
   ros__parameters:
-    base_link: "link_0"
-    end_effector_link: "link_ee"
+    base_link: "lbr_link_0"
+    end_effector_link: "lbr_link_ee"
     f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5]
     dq_gains: [1., 1., 1., 1., 1., 1., 1.]
     dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml

@@ -1,7 +1,7 @@
 /**/admittance_rcm_control:
   ros__parameters:
-    base_link: "link_0"
-    end_effector_link: "link_ee"
+    base_link: "lbr_link_0"
+    end_effector_link: "lbr_link_ee"
     f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]
     dq_gains: [1., 1., 1., 1., 1., 1., 1.]
     dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py

@@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_control") -> None:
         super().__init__(node_name=node_name)
 
         # parameters
-        self.declare_parameter("base_link", "link_0")
-        self.declare_parameter("end_effector_link", "link_ee")
+        self.declare_parameter("base_link", "lbr_link_0")
+        self.declare_parameter("end_effector_link", "lbr_link_ee")
         self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5])
         self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
         self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1])

lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py

@@ -8,8 +8,8 @@ class AdmittanceController(object):
     def __init__(
         self,
         robot_description: str,
-        base_link: str = "link_0",
-        end_effector_link: str = "link_ee",
+        base_link: str = "lbr_link_0",
+        end_effector_link: str = "lbr_link_ee",
         f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]),
         dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]),
         dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]),

lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py

@@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None:
         super().__init__(node_name)
 
         # declare and get parameters
-        self.declare_parameter("base_link", "link_0")
-        self.declare_parameter("end_effector_link", "link_ee")
+        self.declare_parameter("base_link", "lbr_link_0")
+        self.declare_parameter("end_effector_link", "lbr_link_ee")
         self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0])
         self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
         self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1])

lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py

@@ -6,8 +6,8 @@ class AdmittanceRCMController:
     def __init__(
         self,
         robot_description: str,
-        base_link: str = "link_0",
-        end_effector_link: str = "link_ee",
+        base_link: str = "lbr_link_0",
+        end_effector_link: str = "lbr_link_ee",
     ):
         self._robot = optas.RobotModel(
             urdf_string=robot_description, time_derivs=[0, 1]

lbr_description/ros2_control/lbr_system_interface.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="lbr_system_interface"
-        params="mode joint_limits system_parameters_path">
+        params="robot_name mode joint_limits system_parameters_path">
         <!-- load system parameters via yaml -->
         <xacro:property name="system_parameters"
             value="${xacro.load_yaml(system_parameters_path)}" />

lbr_description/ros2_control/lbr_system_parameters.yaml

@@ -19,8 +19,8 @@ hardware:
   open_loop: true # KUKA works the best in open_loop control mode
 
 estimated_ft_sensor: # estimates the external force-torque from the external joint torque values
-  chain_root: link_0
-  chain_tip: link_ee
+  chain_root: lbr_link_0
+  chain_tip: lbr_link_ee
   damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
   force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered
   force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered

lbr_description/urdf/iiwa14/iiwa14_description.xacro

@@ -22,6 +22,7 @@
         <!-- macros for gazebo / mock / hardware -->
         <xacro:lbr_gazebo robot_name="${robot_name}" />
         <xacro:lbr_system_interface
+            robot_name="${robot_name}"
             mode="${mode}"
             joint_limits="${joint_limits}"
             system_parameters_path="${system_parameters_path}" />

lbr_description/urdf/iiwa7/iiwa7_description.xacro

@@ -22,6 +22,7 @@
         <!-- macros for gazebo / mock / hardware -->
         <xacro:lbr_gazebo robot_name="${robot_name}" />
         <xacro:lbr_system_interface
+            robot_name="${robot_name}"
             mode="${mode}"
             joint_limits="${joint_limits}"
             system_parameters_path="${system_parameters_path}" />

lbr_description/urdf/med14/med14_description.xacro

@@ -22,6 +22,7 @@
         <!-- macros for gazebo / mock / hardware -->
         <xacro:lbr_gazebo robot_name="${robot_name}" />
         <xacro:lbr_system_interface
+            robot_name="${robot_name}"
             mode="${mode}"
             joint_limits="${joint_limits}"
             system_parameters_path="${system_parameters_path}" />

lbr_description/urdf/med7/med7_description.xacro

@@ -22,6 +22,7 @@
         <!-- macros for gazebo / mock / hardware -->
         <xacro:lbr_gazebo robot_name="${robot_name}" />
         <xacro:lbr_system_interface
+            robot_name="${robot_name}"
             mode="${mode}"
             joint_limits="${joint_limits}"
             system_parameters_path="${system_parameters_path}" />

lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

@@ -35,8 +35,8 @@ class FTEstimator {
   using cart_array_t_ref = cart_array_t &;
   using const_cart_array_t_ref = const cart_array_t &;
 
-  FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0",
-              const std::string &chain_tip = "link_ee",
+  FTEstimator(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
+              const std::string &chain_tip = "lbr_link_ee",
               const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5});
   void compute(const_jnt_pos_array_t_ref measured_joint_position,
                const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext,

lbr_fri_ros2/test/test_position_command.cpp

@@ -24,7 +24,8 @@ int main() {
 
   pid_params.p = 1.0;
 
-  cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4",
+                                  "lbr_A5", "lbr_A6", "lbr_A7"};
   cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180.,
                                     120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180.,
                                     175. * M_PI / 180.};

lbr_fri_ros2/test/test_torque_command.cpp

@@ -24,7 +24,8 @@ int main() {
 
   pid_params.p = 10.0;
 
-  cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4",
+                                  "lbr_A5", "lbr_A6", "lbr_A7"};
   cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180.,
                                     120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180.,
                                     175. * M_PI / 180.};

lbr_fri_ros2/test/test_wrench_command.cpp

@@ -24,7 +24,8 @@ int main() {
 
   pid_params.p = 10.0;
 
-  cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4",
+                                  "lbr_A5", "lbr_A6", "lbr_A7"};
   cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180.,
                                     120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180.,
                                     175. * M_PI / 180.};

lbr_ros2_control/config/lbr_controllers.yaml

@@ -32,15 +32,21 @@
     lbr_wrench_command_controller:
       type: lbr_ros2_control/LBRWrenchCommandController
 
-    # Admittance controller
     admittance_controller:
       type: lbr_ros2_control/AdmittanceController
 
+# ROS 2 control broadcasters
 /**/force_torque_broadcaster:
   ros__parameters:
     frame_id: lbr_link_ee # namespace: https://github.com/ros2/rviz/issues/1103
     sensor_name: estimated_ft_sensor
 
+# LBR ROS 2 control broadcasters
+/**/lbr_state_broadcaster:
+  ros__parameters:
+    robot_name: lbr
+
+# ROS 2 control controllers
 /**/joint_trajectory_controller:
   ros__parameters:
     joints:
@@ -70,3 +76,16 @@
       - lbr_A6
       - lbr_A7
     interface_name: position
+
+# LBR ROS 2 control controllers
+/**/lbr_joint_position_command_controller:
+  ros__parameters:
+    robot_name: lbr
+
+/**/lbr_torque_command_controller:
+  ros__parameters:
+    robot_name: lbr
+
+/**/lbr_wrench_command_controller:
+  ros__parameters:
+    robot_name: lbr

lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp

@@ -50,9 +50,9 @@ class AdmittanceController : public controller_interface::ControllerInterface {
   bool reference_state_interfaces_();
   void clear_command_interfaces_();
   void clear_state_interfaces_();
+  void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_;

lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp

@@ -40,8 +40,9 @@ class LBRJointPositionCommandController : public controller_interface::Controlle
   on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
 protected:
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  void configure_joint_names_();
+
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
 
   realtime_tools::RealtimeBuffer<lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr>
       rt_lbr_joint_position_command_ptr_;

lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp

@@ -47,9 +47,10 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface {
 protected:
   void init_state_interface_map_();
   void init_state_msg_();
+  void configure_joint_names_();
 
   std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+      "lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"};
   std::unordered_map<std::string, std::unordered_map<std::string, double>> state_interface_map_;
 
   rclcpp::Publisher<lbr_fri_idl::msg::LBRState>::SharedPtr state_publisher_ptr_;

lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp

@@ -44,9 +44,9 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf
 protected:
   bool reference_command_interfaces_();
   void clear_command_interfaces_();
+  void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_, torque_command_interfaces_;

lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp

@@ -47,9 +47,9 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf
 protected:
   bool reference_command_interfaces_();
   void clear_command_interfaces_();
+  void configure_joint_names_();
 
-  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
-      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
 
   std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
       joint_position_command_interfaces_, wrench_command_interfaces_;

lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp

@@ -55,8 +55,8 @@ struct SystemInterfaceParameters {
 };
 
 struct EstimatedFTSensorParameters {
-  std::string chain_root{"link_0"};
-  std::string chain_tip{"link_ee"};
+  std::string chain_root{"lbr_link_0"};
+  std::string chain_tip{"lbr_link_ee"};
   double damping{0.2};
   double force_x_th{2.0};
   double force_y_th{2.0};

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -74,4 +74,18 @@ void AdmittanceController::clear_command_interfaces_() {
 void AdmittanceController::clear_state_interfaces_() {
   estimated_ft_sensor_state_interface_.clear();
 }
+
+void AdmittanceController::configure_joint_names_() {
+  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    RCLCPP_ERROR(
+        this->get_node()->get_logger(),
+        "Number of joint names (%ld) does not match the number of joints in the robot (%d).",
+        joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+    throw std::runtime_error("Failed to configure joint names.");
+  }
+  std::string robot_name = this->get_node()->get_parameter("robot_name").as_string();
+  for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+    joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
+  }
+}
 } // namespace lbr_ros2_control

lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp

@@ -29,6 +29,8 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init(
             [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) {
               rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg);
             });
+    this->get_node()->declare_parameter("robot_name", "lbr");
+    configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
                  "Failed to initialize LBR position command controller with: %s.", e.what());
@@ -66,6 +68,20 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact
     const rclcpp_lifecycle::State & /*previous_state*/) {
   return controller_interface::CallbackReturn::SUCCESS;
 }
+
+void LBRJointPositionCommandController::configure_joint_names_() {
+  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    RCLCPP_ERROR(
+        this->get_node()->get_logger(),
+        "Number of joint names (%ld) does not match the number of joints in the robot (%d).",
+        joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+    throw std::runtime_error("Failed to configure joint names.");
+  }
+  std::string robot_name = this->get_node()->get_parameter("robot_name").as_string();
+  for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+    joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
+  }
+}
 } // namespace lbr_ros2_control
 
 #include "pluginlib/class_list_macros.hpp"

lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp

@@ -21,13 +21,8 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() {
     rt_state_publisher_ptr_ =
         std::make_shared<realtime_tools::RealtimePublisher<lbr_fri_idl::msg::LBRState>>(
             state_publisher_ptr_);
-    if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
-      RCLCPP_ERROR(
-          this->get_node()->get_logger(),
-          "Number of joint names (%ld) does not match the number of joints in the robot (%d).",
-          joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
-      return controller_interface::CallbackReturn::ERROR;
-    }
+    this->get_node()->declare_parameter("robot_name", "lbr");
+    configure_joint_names_();
   } catch (const std::exception &e) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
                  "Failed to initialize LBR state broadcaster with: %s.", e.what());
@@ -154,6 +149,20 @@ void LBRStateBroadcaster::init_state_msg_() {
   rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits<uint32_t>::quiet_NaN();
   rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits<double>::quiet_NaN();
 }
+
+void LBRStateBroadcaster::configure_joint_names_() {
+  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    RCLCPP_ERROR(
+        this->get_node()->get_logger(),
+        "Number of joint names (%ld) does not match the number of joints in the robot (%d).",
+        joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+    throw std::runtime_error("Failed to configure joint names.");
+  }
+  std::string robot_name = this->get_node()->get_parameter("robot_name").as_string();
+  for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+    joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
+  }
+}
 } // namespace lbr_ros2_control
 
 #include "pluginlib/class_list_macros.hpp"