updated log

lbr-stack · Dec 7, 2023 · 5075b7d · 5075b7d
1 parent 24c89d1
commit 5075b7d
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 21 deletions.
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
@@ -20,11 +20,11 @@ struct CommandGuardParameters {
   using joint_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
   using joint_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
 
-  joint_name_array_t joint_names_; /**< Joint names.*/
-  joint_array_t min_position_;     /**< Minimum joint position [rad].*/
-  joint_array_t max_position_;     /**< Maximum joint position [rad].*/
-  joint_array_t max_velocity_;     /**< Maximum joint velocity [rad/s].*/
-  joint_array_t max_torque_;       /**< Maximum joint torque [Nm].*/
+  joint_name_array_t joint_names; /**< Joint names.*/
+  joint_array_t min_position;     /**< Minimum joint position [rad].*/
+  joint_array_t max_position;     /**< Maximum joint position [rad].*/
+  joint_array_t max_velocity;     /**< Maximum joint velocity [rad/s].*/
+  joint_array_t max_torque;       /**< Maximum joint torque [Nm].*/
 };
 
 class CommandGuard {

diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp
@@ -13,7 +13,7 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters,
   command_interface_.log_info();
   state_interface_.log_info();
   RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false");
-  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "AsyncClient configured.");
+  RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured.");
 }
 
 void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state,

diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp
@@ -36,22 +36,23 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command,
 
 void CommandGuard::log_info() const {
   RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
-  for (std::size_t i = 0; i < parameters_.joint_names_.size(); ++i) {
+  for (std::size_t i = 0; i < parameters_.joint_names.size(); ++i) {
     RCLCPP_INFO(
         rclcpp::get_logger(LOGGER_NAME),
         "*   Joint %s limits: Position: [%.1f, %.1f] deg, velocity: %.1f deg/s, torque: %.1f Nm",
-        parameters_.joint_names_[i].c_str(), parameters_.min_position_[i],
-        parameters_.max_position_[i], parameters_.max_velocity_[i], parameters_.max_torque_[i]);
+        parameters_.joint_names[i].c_str(), parameters_.min_position[i] * (180. / M_PI),
+        parameters_.max_position[i] * (180. / M_PI), parameters_.max_velocity[i] * (180. / M_PI),
+        parameters_.max_torque[i]);
   }
 }
 
 bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command,
                                                const_fri_state_t_ref /*lbr_state*/) const {
   for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) {
-    if (lbr_command.joint_position[i] < parameters_.min_position_[i] ||
-        lbr_command.joint_position[i] > parameters_.max_position_[i]) {
+    if (lbr_command.joint_position[i] < parameters_.min_position[i] ||
+        lbr_command.joint_position[i] > parameters_.max_position[i]) {
       RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.",
-                   parameters_.joint_names_[i].c_str());
+                   parameters_.joint_names[i].c_str());
       return false;
     }
   }
@@ -63,9 +64,9 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma
   const double &dt = lbr_state.getSampleTime();
   for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) {
     if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt >
-        parameters_.max_velocity_[i]) {
+        parameters_.max_velocity[i]) {
       RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.",
-                   parameters_.joint_names_[i].c_str());
+                   parameters_.joint_names[i].c_str());
       return false;
     }
   }
@@ -76,9 +77,9 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command
                                              const_fri_state_t_ref lbr_state) const {
   for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) {
     if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) >
-        parameters_.max_torque_[i]) {
+        parameters_.max_torque[i]) {
       RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint %s.",
-                   parameters_.joint_names_[i].c_str());
+                   parameters_.joint_names[i].c_str());
       return false;
     }
   }
@@ -89,13 +90,11 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l
                                                        const_fri_state_t_ref lbr_state) const {
   for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) {
     if (lbr_command.joint_position[i] <
-            parameters_.min_position_[i] +
-                parameters_.max_velocity_[i] * lbr_state.getSampleTime() ||
+            parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() ||
         lbr_command.joint_position[i] >
-            parameters_.max_position_[i] -
-                parameters_.max_velocity_[i] * lbr_state.getSampleTime()) {
+            parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) {
       RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.",
-                   parameters_.joint_names_[i].c_str());
+                   parameters_.joint_names[i].c_str());
       return false;
     }
   }