add limiting

Author: bmagyar

joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp

@@ -33,6 +33,25 @@ namespace joint_state_topic_hardware_interface
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
+struct VelocityLimits
+{
+  explicit VelocityLimits() = default;
+  explicit VelocityLimits(const hardware_interface::InterfaceInfo& info)
+  {
+    if (not info.min.empty())
+    {
+      min = std::stod(info.min);
+    }
+    if (not info.max.empty())
+    {
+      max = std::stod(info.max);
+    }
+  }
+
+  double min = -std::numeric_limits<double>::infinity();
+  double max = std::numeric_limits<double>::infinity();
+};
+
 class JointStateTopicSystem : public hardware_interface::SystemInterface
 {
 public:
@@ -70,6 +89,11 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface
   std::vector<std::vector<double>> joint_commands_;
   std::vector<std::vector<double>> joint_states_;
 
+  bool enable_command_limiting_ = false;
+  std::vector<double> nonlimited_velocity_commands_;
+  std::vector<double> limited_velocity_commands_;
+  std::vector<VelocityLimits> velocity_limits_;
+
   // If the difference between the current joint state and joint command is less than this value,
   // the joint command will not be published.
   double trigger_joint_command_threshold_ = 1e-5;

joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp

@@ -57,6 +57,9 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
   // Initialize storage for all joints' standard interfaces, regardless of their existence and set all values to nan
   joint_commands_.resize(standard_interfaces_.size());
   joint_states_.resize(standard_interfaces_.size());
+  velocity_limits_.resize(get_hardware_info().joints.size());
+  limited_velocity_commands_.resize(get_hardware_info().joints.size());
+  nonlimited_velocity_commands_.resize(get_hardware_info().joints.size());
   for (auto i = 0u; i < standard_interfaces_.size(); i++)
   {
     joint_commands_[i].resize(get_hardware_info().joints.size(), 0.0);
@@ -67,7 +70,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
   for (auto i = 0u; i < get_hardware_info().joints.size(); i++)
   {
     const auto& component = get_hardware_info().joints[i];
-    for (const auto& interface : component.state_interfaces)
+    for (const auto& interface : component.command_interfaces)
     {
       auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name);
       // If interface name is found in the interfaces list
@@ -80,6 +83,7 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
           joint_states_[index][i] = std::stod(interface.initial_value);
           joint_commands_[index][i] = std::stod(interface.initial_value);
         }
+        velocity_limits_[i] = VelocityLimits(interface);
       }
     }
   }
@@ -135,9 +139,14 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware
 
   // if the values on the `joint_states_topic` are wrapped between -2*pi and 2*pi (like they are in Isaac Sim)
   // sum the total joint rotation returned on the `joint_states_` interface
-  if (get_hardware_parameter("sum_wrapped_joint_states", "false") == "true")
+  sum_wrapped_joint_states_ =
+      hardware_interface::parse_bool(get_hardware_parameter("sum_wrapped_joint_states", "false"));
+  // Clamp the command values to the min/max defined in the <ros2_control> tag for each joint
+  enable_command_limiting_ = hardware_interface::parse_bool(get_hardware_parameter("enable_command_limiting", "false"));
+
+  if (enable_command_limiting_)
   {
-    sum_wrapped_joint_states_ = true;
+    RCLCPP_WARN(get_node()->get_logger(), "** Joint command limiting is enabled **");
   }
 
   return CallbackReturn::SUCCESS;
@@ -269,7 +278,17 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time&
       }
       else if (interface.name == hardware_interface::HW_IF_VELOCITY)
       {
-        joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+        nonlimited_velocity_commands_[i] = joint_commands_[VELOCITY_INTERFACE_INDEX][i];
+        if (enable_command_limiting_)
+        {
+          limited_velocity_commands_[i] =
+              std::clamp(nonlimited_velocity_commands_[i], velocity_limits_[i].min, velocity_limits_[i].max);
+          joint_state.velocity.push_back(limited_velocity_commands_[i]);
+        }
+        else
+        {
+          joint_state.velocity.push_back(nonlimited_velocity_commands_[i]);
+        }
       }
       else if (interface.name == hardware_interface::HW_IF_EFFORT)
       {