added a N_JNTS alias to types

lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp

@@ -54,7 +54,7 @@ class PoseControlNode : public LBRBasePositionCommandNode {
     current_lbr_state_ = *lbr_state;
 
     double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS];
-    for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+    for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) {
       joint_position[i] = current_lbr_state_.measured_joint_position[i];
     }
     current_pose_ = compute_fk_(joint_position);
@@ -65,7 +65,7 @@ class PoseControlNode : public LBRBasePositionCommandNode {
     if (current_lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) {
       KDL::JntArray current_joint_positions(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
 
-      for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+      for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) {
         current_joint_positions(i) = current_lbr_state_.measured_joint_position[i];
       }
 
@@ -79,7 +79,7 @@ class PoseControlNode : public LBRBasePositionCommandNode {
     KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_);
     KDL::JntArray joint_positions = KDL::JntArray(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
 
-    for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) {
+    for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) {
       joint_positions(i) = position_array_ptr[i];
     }
 
@@ -131,7 +131,7 @@ class PoseControlNode : public LBRBasePositionCommandNode {
     {
       RCLCPP_ERROR(this->get_logger(), "Inverse kinematics failed.");
     } else {
-      for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) {
+      for (unsigned int i = 0; i < result_joint_positions.data.size(); ++i) {
         joint_position_command.joint_position[i] = result_joint_positions(i);
       }
     }

lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp

@@ -121,7 +121,7 @@ struct PIDParameters {
 class JointPIDArray {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray";
-  using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+  using pid_array_t = std::array<control_toolbox::Pid, N_JNTS>;
 
 public:
   JointPIDArray() = delete;

lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

@@ -39,8 +39,8 @@ class FTEstimator {
   std::unique_ptr<Kinematics> kinematics_ptr_;
 
   // force estimation
-  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, CARTESIAN_DOF> jacobian_inv_;
-  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1> tau_ext_;
+  Eigen::Matrix<double, N_JNTS, CARTESIAN_DOF> jacobian_inv_;
+  Eigen::Matrix<double, N_JNTS, 1> tau_ext_;
   Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_;
 };
 } // namespace lbr_fri_ros2

lbr_fri_ros2/include/lbr_fri_ros2/types.hpp

@@ -11,20 +11,23 @@
 #include "lbr_fri_idl/msg/lbr_state.hpp"
 
 namespace lbr_fri_ros2 {
+// joint DoF alias
+constexpr std::uint8_t N_JNTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS;
+
 // joint positions, velocities, accelerations, torques etc.
-using jnt_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+using jnt_array_t = std::array<double, N_JNTS>;
 using jnt_array_t_ref = jnt_array_t &;
 using const_jnt_array_t_ref = const jnt_array_t &;
 
 // joint names
-using jnt_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+using jnt_name_array_t = std::array<std::string, N_JNTS>;
 using jnt_name_array_t_ref = jnt_name_array_t &;
 using const_jnt_name_array_t_ref = const jnt_name_array_t &;
 
-// Cartesian dof
+// Cartesian DoF
 constexpr std::uint8_t CARTESIAN_DOF = 6;
 
-// Cartesian positions, velocities, accelerations, torques etc.
+// Cartesian positions, velocities, accelerations, wrenches etc.
 using cart_array_t = std::array<double, CARTESIAN_DOF>;
 using cart_array_t_ref = cart_array_t &;
 using const_cart_array_t_ref = const cart_array_t &;

lbr_fri_ros2/src/filters.cpp

@@ -30,11 +30,10 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency,
 bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; }
 
 void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) {
-  std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS,
-                [&, i = 0](const auto &current_i) mutable {
-                  previous[i] = exponential_filter_.compute(current_i, previous[i]);
-                  ++i;
-                });
+  std::for_each(current, current + N_JNTS, [&, i = 0](const auto &current_i) mutable {
+    previous[i] = exponential_filter_.compute(current_i, previous[i]);
+    ++i;
+  });
 }
 
 void JointExponentialFilterArray::initialize(const double &cutoff_frequency,

lbr_fri_ros2/src/ft_estimator.cpp

@@ -11,8 +11,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string
 void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position,
                           const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext,
                           const double &damping) {
-  tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
-      external_torque.data());
+  tau_ext_ = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(external_torque.data());
   auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position);
   jacobian_inv_ = pinv(jacobian.data, damping);
   f_ext_ = jacobian_inv_.transpose() * tau_ext_;

lbr_fri_ros2/src/kinematics.cpp

@@ -13,52 +13,48 @@ Kinematics::Kinematics(const std::string &robot_description, const std::string &
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
-  if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (chain_.getNrOfJoints() != N_JNTS) {
     std::string err = "Invalid number of joints in chain.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
   jacobian_solver_ = std::make_unique<KDL::ChainJntToJacSolver>(chain_);
   fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(chain_);
-  jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
-  q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+  jacobian_.resize(N_JNTS);
+  q_.resize(N_JNTS);
   q_.data.setZero();
 }
 
 const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) {
-  q_.data =
-      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  q_.data = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(q.data());
   jacobian_solver_->JntToJac(q_, jacobian_);
   return jacobian_;
 }
 
 const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector<double> &q) {
-  if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (q.size() != N_JNTS) {
     std::string err = "Invalid number of joint positions.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
-  q_.data =
-      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  q_.data = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(q.data());
   jacobian_solver_->JntToJac(q_, jacobian_);
   return jacobian_;
 }
 
 const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) {
-  q_.data =
-      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  q_.data = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(q.data());
   fk_solver_->JntToCart(q_, chain_tip_frame_);
   return chain_tip_frame_;
 }
 
 const KDL::Frame &Kinematics::compute_fk(const std::vector<double> &q) {
-  if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (q.size() != N_JNTS) {
     std::string err = "Invalid number of joint positions.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
-  q_.data =
-      Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(q.data());
+  q_.data = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(q.data());
   fk_solver_->JntToCart(q_, chain_tip_frame_);
   return chain_tip_frame_;
 }

lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp

@@ -46,8 +46,7 @@ class TwistImpl {
 
   lbr_fri_ros2::jnt_array_t q_;
   std::unique_ptr<lbr_fri_ros2::Kinematics> kinematics_ptr_;
-  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, lbr_fri_ros2::CARTESIAN_DOF>
-      jacobian_inv_;
+  Eigen::Matrix<double, lbr_fri_ros2::N_JNTS, lbr_fri_ros2::CARTESIAN_DOF> jacobian_inv_;
   Eigen::Matrix<double, lbr_fri_ros2::CARTESIAN_DOF, 1> twist_target_;
 };
 

lbr_ros2_control/src/controllers/admittance_controller.cpp

@@ -50,12 +50,12 @@ bool AdmittanceController::reference_command_interfaces_() {
       joint_position_command_interfaces_.emplace_back(std::ref(command_interface));
     }
   }
-  if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
         "Number of joint position command interfaces '%ld' does not match the number of joints "
         "in the robot '%d'.",
-        joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+        joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS);
     return false;
   }
 }
@@ -77,15 +77,15 @@ void AdmittanceController::clear_state_interfaces_() {
 }
 
 void AdmittanceController::configure_joint_names_() {
-  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     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);
+        joint_names_.size(), lbr_fri_ros2::N_JNTS);
     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++) {
+  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }

lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp

@@ -49,7 +49,7 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/,
   }
   std::for_each(command_interfaces_.begin(), command_interfaces_.end(),
                 [lbr_joint_position_command, idx = 0](auto &command_interface) mutable {
-                  command_interface.set_value((*lbr_joint_position_command)->joint_position[idx++]);
+                  command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]);
                 });
   return controller_interface::return_type::OK;
 }
@@ -70,15 +70,15 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact
 }
 
 void LBRJointPositionCommandController::configure_joint_names_() {
-  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     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);
+        joint_names_.size(), lbr_fri_ros2::N_JNTS);
     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++) {
+  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }

lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp

@@ -151,15 +151,15 @@ void LBRStateBroadcaster::init_state_msg_() {
 }
 
 void LBRStateBroadcaster::configure_joint_names_() {
-  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     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);
+        joint_names_.size(), lbr_fri_ros2::N_JNTS);
     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++) {
+  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }

lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp

@@ -46,7 +46,7 @@ LBRTorqueCommandController::update(const rclcpp::Time & /*time*/,
   if (!lbr_torque_command || !(*lbr_torque_command)) {
     return controller_interface::return_type::OK;
   }
-  for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) {
+  for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) {
     joint_position_command_interfaces_[idx].get().set_value(
         (*lbr_torque_command)->joint_position[idx]);
     torque_command_interfaces_[idx].get().set_value((*lbr_torque_command)->torque[idx]);
@@ -82,19 +82,19 @@ bool LBRTorqueCommandController::reference_command_interfaces_() {
       torque_command_interfaces_.emplace_back(std::ref(command_interface));
     }
   }
-  if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
         "Number of joint position command interfaces '%ld' does not match the number of joints "
         "in the robot '%d'.",
-        joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+        joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS);
     return false;
   }
-  if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (torque_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(this->get_node()->get_logger(),
                  "Number of torque command interfaces '%ld' does not match the number of joints "
                  "in the robot '%d'.",
-                 torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+                 torque_command_interfaces_.size(), lbr_fri_ros2::N_JNTS);
     return false;
   }
   return true;
@@ -106,15 +106,15 @@ void LBRTorqueCommandController::clear_command_interfaces_() {
 }
 
 void LBRTorqueCommandController::configure_joint_names_() {
-  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     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);
+        joint_names_.size(), lbr_fri_ros2::N_JNTS);
     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++) {
+  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }

lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp

@@ -51,7 +51,7 @@ LBRWrenchCommandController::update(const rclcpp::Time & /*time*/,
   if (!lbr_wrench_command || !(*lbr_wrench_command)) {
     return controller_interface::return_type::OK;
   }
-  for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) {
+  for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) {
     joint_position_command_interfaces_[idx].get().set_value(
         (*lbr_wrench_command)->joint_position[idx]);
   }
@@ -89,12 +89,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() {
       wrench_command_interfaces_.emplace_back(std::ref(command_interface));
     }
   }
-  if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) {
     RCLCPP_ERROR(
         this->get_node()->get_logger(),
         "Number of joint position command interfaces '%ld' does not match the number of joints "
         "in the robot '%d'.",
-        joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+        joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS);
     return false;
   }
   if (wrench_command_interfaces_.size() != CARTESIAN_DOF) {
@@ -112,15 +112,15 @@ void LBRWrenchCommandController::clear_command_interfaces_() {
 }
 
 void LBRWrenchCommandController::configure_joint_names_() {
-  if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+  if (joint_names_.size() != lbr_fri_ros2::N_JNTS) {
     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);
+        joint_names_.size(), lbr_fri_ros2::N_JNTS);
     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++) {
+  for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) {
     joint_names_[i] = robot_name + "_A" + std::to_string(i + 1);
   }
 }