Chained Controller with Parallel Gripper Action Controller

[zacharyyamaoka]

Hi ros2 team,

Just making a note here in case someone runs into the same problem.

Out of the Box Parallel Gripper Action Controller doesn't seem to account for prefixes on chained controllers properly:

this works:

  eef_controller:
      ros__parameters:
        action_monitor_rate: 25.0
        joint: eef_joint_1 
        state_interfaces:
          - position
          - velocity

This doesn't work:

  chained_eef_controller:
      ros__parameters:
        action_monitor_rate: 25.0
        joint: moteus_eef_controller/eef_joint_1 
        state_interfaces:
          - position
          - velocity

My solution for now is to properly slice the joint names...

inline std::string get_full_prefix(const std::string & full_name)
{
  auto pos = full_name.rfind('/');
  if (pos == std::string::npos || pos == 0)
  {
    return "";  // No slash or nothing before it
  }
  return full_name.substr(0, pos);
}

inline std::string get_suffix(const std::string & full_name)
{
  auto pos = full_name.rfind('/');
  if (pos == std::string::npos || pos == full_name.length() - 1)
  {
    return full_name;  // No slash, or slash at end
  }
  return full_name.substr(pos + 1);
}

controller_interface::CallbackReturn GripperActionController::on_activate(
  const rclcpp_lifecycle::State &)
{

// Log all available command interfaces
  RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:");
  for (const auto & interface : command_interfaces_)
  {
    RCLCPP_INFO(
      get_node()->get_logger(),
      "- prefix: '%s', interface: '%s', full name: '%s', full prefix: '%s', suffix: '%s'",
      interface.get_prefix_name().c_str(),
      interface.get_interface_name().c_str(),
      interface.get_name().c_str(),
      get_full_prefix(interface.get_name()).c_str(),
      get_suffix(interface.get_name()).c_str());
  }

  auto command_interface_it = std::find_if(
    command_interfaces_.begin(), command_interfaces_.end(),
    [](const hardware_interface::LoanedCommandInterface & command_interface)
      { return get_suffix(command_interface.get_name()) == hardware_interface::HW_IF_POSITION; });
    // { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });


  if (command_interface_it == command_interfaces_.end())
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Expected 1 %s command interface",
      hardware_interface::HW_IF_POSITION);
    return controller_interface::CallbackReturn::ERROR;
  }
  if (get_full_prefix(command_interface_it->get_name()) != params_.joint)
  // if (command_interface_it->get_prefix_name() != params_.joint)
  {
    RCLCPP_ERROR(
      get_node()->get_logger(), "Command interface is different than joint name `%s` != `%s`",
      get_full_prefix(command_interface_it->get_name()).c_str(), params_.joint.c_str());
    return controller_interface::CallbackReturn::ERROR;
  }
  const auto position_state_interface_it = std::find_if(
    state_interfaces_.begin(), state_interfaces_.end(),
    [](const hardware_interface::LoanedStateInterface & state_interface)
      { return get_suffix(state_interface.get_name()) == hardware_interface::HW_IF_POSITION; });
    // { return state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; });


  if (position_state_interface_it == state_interfaces_.end())
  {
    RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position state interface");
    return controller_interface::CallbackReturn::ERROR;
  }
  if (get_full_prefix(position_state_interface_it->get_name()) != params_.joint)
  // if (position_state_interface_it->get_prefix_name() != params_.joint)
  {
    RCLCPP_ERROR(
      get_node()->get_logger(),
      "Position state interface is different than joint name `%s` != `%s`",
      get_full_prefix(position_state_interface_it->get_name()).c_str(), params_.joint.c_str());
    return controller_interface::CallbackReturn::ERROR;
  }

  const auto velocity_state_interface_it = std::find_if(
    state_interfaces_.begin(), state_interfaces_.end(),
    [](const hardware_interface::LoanedStateInterface & state_interface)
      { return get_suffix(state_interface.get_name()) == hardware_interface::HW_IF_VELOCITY; });
      // { return state_interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY; });

  if (velocity_state_interface_it == state_interfaces_.end())
  {
    RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 velocity state interface");
    return controller_interface::CallbackReturn::ERROR;
  }
  if (get_full_prefix(velocity_state_interface_it->get_name()) != params_.joint)
  // if (velocity_state_interface_it->get_prefix_name() != params_.joint)
  {
    RCLCPP_ERROR(
      get_node()->get_logger(),
      "Velocity command interface is different than joint name `%s` != `%s`",
      get_full_prefix(velocity_state_interface_it->get_name()).c_str(), params_.joint.c_str());
    return controller_interface::CallbackReturn::ERROR;
  }

  joint_command_interface_ = *command_interface_it;
  joint_position_state_interface_ = *position_state_interface_it;
  joint_velocity_state_interface_ = *velocity_state_interface_it;

  for (auto & interface : command_interfaces_)
  {
    if (interface.get_interface_name() == "set_gripper_max_effort")
    {
      effort_interface_ = interface;
    }
    else if (interface.get_interface_name() == "set_gripper_max_velocity")
    {
      speed_interface_ = interface;
    }
  }

JTC doesn't seem to have this issue as it doesn't some logic in here that I think takes care of it... note sure though!

    if (!controller_interface::get_ordered_interfaces(
          command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
    {

bool get_ordered_interfaces(
  std::vector<T> & unordered_interfaces, const std::vector<std::string> & ordered_names,
  const std::string & interface_type, std::vector<std::reference_wrapper<T>> & ordered_interfaces)
{
  ordered_interfaces.reserve(ordered_names.size());
  for (const auto & name : ordered_names)
  {
    for (auto & interface : unordered_interfaces)
    {
      if (!interface_type.empty())
      {
        // check case where:
        // (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
        if (
          ((name == interface.get_prefix_name()) &&
           (interface_type == interface.get_interface_name())) ||
          ((name + "/" + interface_type) == interface.get_name()))
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
      else
      {
        if (name == interface.get_name())
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
    }
  }

  return ordered_names.size() == ordered_interfaces.size();
}

another challenge I am looking into right now is using Moveit with a chained JTC, as move it sends trajectories with raw joint names joint_1, which fail the trajectory_validation in JTC as it expects names with chained_controller_1/joint_1

(PS. Sorry for not pushing a patch right now. I know I also have few other open issues I need to merge changes for... 😅, I am working solo, and I am very bad with this CI/CD stuff so makes it more difficult)