ROS2 Control - Admittance Controller Kinematics Chain Base Link 설정 (Bug)

[Environment]
- Ubuntu 22.04.5 LTS
- ROS2 Iron

1. 문제상황 분석

ROS2 Control의 admittance controller는 Force/Torque Sensor의 값을 토대로 로봇 end-effector의 이동 위치(혹은 속도)를 계산하기 위해 기구학(kinematics) 플러그인을 지정해 사용할 수 있다

# https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/src/admittance_controller_parameters.yaml
admittance_controller:
  kinematics:
    plugin_name: {
      type: string,
      description: "Specifies the name of the kinematics plugin to load."
    }
    plugin_package: {
      type: string,
      description: "Specifies the package name that contains the kinematics plugin."
    }
    base: {
      type: string,
      description: "Specifies the base link of the robot description used by the kinematics plugin."
    }
    tip: {
      type: string,
      description: "Specifies the end effector link of the robot description used by the kinematics plugin."
    }
    alpha: {
      type: double,
      default_value: 0.01,
      description: "Specifies the damping coefficient for the Jacobian pseudo inverse."
    }

 

이 때, 5개 파라미터는 각각 admittance controller node 내부에 다음과 같은 이름으로 파라미터가 지정되게 된다

• kinematics.plugin_name
• kinematics.plugin_package
• kinematics.base
• kinematics.tip
• kinematics.alpha

rqt 'Parameter Reconfigure'

파라미터 설명에 따르면 kinematics chain의 최초 링크(base link)를 바꾸고 싶으면 'base' 파라미터 문자열 값을 바꿔줘야 한다

 

admittance controller의 예제에 나와있는 kinematics_interface_kdl의 소스코드 중 인터페이스 초기화 메서드인 initialize를 보면 해당 항목이 어떻게 적용되는지 파악할 수 있다 (소스코드 링크)

/*
https://github.com/ros-controls/kinematics_interface/blob/master/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
*/

bool KinematicsInterfaceKDL::initialize(
  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
  const std::string & end_effector_name)
{
  // track initialization plugin
  initialized = true;

  // get robot description
  auto robot_param = rclcpp::Parameter();
  if (!parameters_interface->get_parameter("robot_description", robot_param))
  {
    RCLCPP_ERROR(LOGGER, "parameter robot_description not set");
    return false;
  }
  auto robot_description = robot_param.as_string();
  // get alpha damping term
  auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
  if (parameters_interface->has_parameter("alpha"))
  {
    parameters_interface->get_parameter("alpha", alpha_param);
  }
  alpha = alpha_param.as_double();
  // create kinematic chain
  KDL::Tree robot_tree;
  kdl_parser::treeFromString(robot_description, robot_tree);

  // get root name
  auto base_param = rclcpp::Parameter();
  if (parameters_interface->has_parameter("base"))
  {
    parameters_interface->get_parameter("base", base_param);
    root_name_ = base_param.as_string();
  }
  else
  {
    root_name_ = robot_tree.getRootSegment()->first;
  }

  if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
  {
    RCLCPP_ERROR(
      LOGGER, "failed to find chain from robot root %s to end effector %s", root_name_.c_str(),
      end_effector_name.c_str());
    return false;
  }
  // create map from link names to their index
  for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
  {
    link_name_map_[chain_.getSegment(i).getName()] = i + 1;
  }
  // allocate dynamics memory
  num_joints_ = chain_.getNrOfJoints();
  q_ = KDL::JntArray(num_joints_);
  I = Eigen::MatrixXd(num_joints_, num_joints_);
  I.setIdentity();
  // create KDL solvers
  fk_pos_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain_);
  jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain_);
  jacobian_ = std::make_shared<KDL::Jacobian>(num_joints_);

  return true;
}

ROS node의 파라미터 인터페이스 객체의 포인터인 parameters_interface로부터 "robot_description", "alpha", "base" 3개의 파라미터를 참조해 사용하는 것을 확인할 수 있다 (get_parameter 항목 참고)

 

그런데, admittance controller에서 kinematics 객체를 생성하고 초기화하는 구문을 보면 컨트롤러 노드의 파라미터가 별다른 변환없이 그대로 입력되게 구현되어 있는 것을 알 수 있다

/*
https://github.com/ros-controls/ros2_controllers/blob/iron/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
*/

controller_interface::return_type AdmittanceRule::configure(
  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
{
  num_joints_ = num_joints;

  // initialize memory and values to zero  (non-realtime function)
  reset(num_joints);

  // Load the differential IK plugin
  if (!parameters_.kinematics.plugin_name.empty())
  {
    try
    {
      // Make sure we destroy the interface first. Otherwise we might run into a segfault
      if (kinematics_loader_)
      {
        kinematics_.reset();
      }
      kinematics_loader_ =
        std::make_shared<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>(
          parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface");
      kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
        kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));

      if (!kinematics_->initialize(
            node->get_node_parameters_interface(), parameters_.kinematics.tip))
      {
        return controller_interface::return_type::ERROR;
      }
    }
    catch (pluginlib::PluginlibException & ex)
    {
      RCLCPP_ERROR(
        rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'",
        parameters_.kinematics.plugin_name.c_str(), ex.what());
      return controller_interface::return_type::ERROR;
    }
  }
  else
  {
    RCLCPP_ERROR(
      rclcpp::get_logger("AdmittanceRule"),
      "A differential IK plugin name was not specified in the config file.");
    return controller_interface::return_type::ERROR;
  }

  return controller_interface::return_type::OK;
}

※ kinematics_->initialize, node->get_node_parameters_interface() 구문 참고

end_effector 링크는 initialize 메서드의 두번째 인자로 문자열로 입력되기 때문에 parameters_.kinematics.tip으로 사용자가 설정한 대로 전달되지만, 첫번째 인자로 노드의 파라미터 인터페이스를 그대로 입력하게 되면 "alpha"와 "base" 파라미터는 절대로 적용될 수가 없다!

(각각 "kinematics.alpha", "kinematics.base"로 전달되기 때문)

 

// get root name
auto base_param = rclcpp::Parameter();
if (parameters_interface->has_parameter("base"))
{
  parameters_interface->get_parameter("base", base_param);
  root_name_ = base_param.as_string();
}
else
{
  root_name_ = robot_tree.getRootSegment()->first;
}

결국 사용자는 kinematics chain tree의 root link를 절대로 바꿀 수가 없다 (무조건 URDF상 최초 링크가 root로 설정됨)

2. 해결책 및 적용법

내가 찾은 솔루션은 ros2_controller 소스코드를 로컬에 클론한 뒤 아래와 같이 코드를 변경해 kinematics 초기화 시 전달되는 파라미터 인터페이스에 강제로 "alpha"와 "base" 파라미터를 추가해주는 방법이다

2.1. 소스코드 클론

※ 경로는 입맛에 맞게 변경하면 된다

$ mkdir ~/ros2_pkgs
$ mkdir ~/ros2_pkgs/src
$ cd ~/ros2_pkgs/src
$ git clone https://github.com/ros-controls/ros2_controllers.git -b ${ROS_DISTRO}

2.2 소스코드 수정

아래와 같이 node parameter를 강제로 추가해주는 구문을 추가해준다 

(파일명은 admittance_rule_impl.hpp)

controller_interface::return_type AdmittanceRule::configure(
  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
{
  num_joints_ = num_joints;

  // initialize memory and values to zero  (non-realtime function)
  reset(num_joints);

  // Load the differential IK plugin
  if (!parameters_.kinematics.plugin_name.empty())
  {
    try
    {
      // Make sure we destroy the interface first. Otherwise we might run into a segfault
      if (kinematics_loader_)
      {
        kinematics_.reset();
      }
      kinematics_loader_ =
        std::make_shared<pluginlib::ClassLoader<kinematics_interface::KinematicsInterface>>(
          parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface");
      kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
        kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));

      /* 추가 시작 */
      std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_interface = node->get_node_parameters_interface();
      
      if (node_parameters_interface->has_parameter("kinematics.alpha"))
      {
        auto alpha_param = rclcpp::Parameter();
        node_parameters_interface->get_parameter("kinematics.alpha", alpha_param);
        node_parameters_interface->declare_parameter("alpha", rclcpp::ParameterValue(alpha_param.as_double()));
        RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), "Add 'kinematics.alpha' parameter(%g) as 'alpha'", alpha_param.as_double());
      }
      
      if (node_parameters_interface->has_parameter("kinematics.base"))
      {
        auto base_param = rclcpp::Parameter();
        node_parameters_interface->get_parameter("kinematics.base", base_param);
        node_parameters_interface->declare_parameter("base", rclcpp::ParameterValue(base_param.as_string()));
        RCLCPP_INFO(rclcpp::get_logger("AdmittanceRule"), "Add 'kinematics.base' parameter(%s) as 'base'", base_param.as_string().c_str());
      }
      /* 추가 종료 */
      
      if (!kinematics_->initialize(
            node->get_node_parameters_interface(), parameters_.kinematics.tip))
      {
        return controller_interface::return_type::ERROR;
      }
    }
    catch (pluginlib::PluginlibException & ex)
    {
      RCLCPP_ERROR(
        rclcpp::get_logger("AdmittanceRule"), "Exception while loading the IK plugin '%s': '%s'",
        parameters_.kinematics.plugin_name.c_str(), ex.what());
      return controller_interface::return_type::ERROR;
    }
  }
  else
  {
    RCLCPP_ERROR(
      rclcpp::get_logger("AdmittanceRule"),
      "A differential IK plugin name was not specified in the config file.");
    return controller_interface::return_type::ERROR;
  }

  return controller_interface::return_type::OK;
}

2.3. 패키지 빌드

$ cd ~/ros2_pkgs
$ colcon build --symlink-install \
    --packages-select admittance_controller \
    --allow-overriding admittance_controller \
    --packages-ignore joint_trajectory_controller \
    --cmake-args -DBUILD_TESTING=OFF

기존에 바이너리로 설치되어 있던 admittance_controller를 대체하는 인자를 추가해준다

2.4. 사용

$ source ~/ros2_pkgs/install/local_setup.bash

bash 파일을 사용 전에 source하여 사용하면 된다

 

코드 수정 후 admittance controller 노드의 파라미터를 보면 "alpha"와 "base" 항목이 추가된 것을 확인할 수 있다

2.5. 기타

혹은 kinematics_interface의 initialize 구문을 아래와 같이 변경하는 방법도 있다 (해당 내용을 적용하는 방법에 대해서는 독자의 몫으로 남겨둔다)

/*
https://github.com/ros-controls/kinematics_interface/blob/master/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
*/

bool KinematicsInterfaceKDL::initialize(
  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
  const std::string & end_effector_name)
{
  // 생략
  // get root name
  auto base_param = rclcpp::Parameter();
  if (parameters_interface->has_parameter("base"))
  {
    parameters_interface->get_parameter("base", base_param);
    root_name_ = base_param.as_string();
  }
  /* 추가 구문 시작 */
  else if (parameters_interface->has_parameter("kinematics.base"))
  {
    parameters_interface->get_parameter("kinematics.base", base_param);
    root_name_ = base_param.as_string();
  }
  /* 추가 구문 끝 */
  else
  {
    root_name_ = robot_tree.getRootSegment()->first;
  }
  // 생략