[Question]: I built a new robot, how to do it without using gripper

[parzivar]

Question

:~/interbotix_ws$ ros2 launch interbotix_xsarm_ros_control xsarm_ros_control.launch.py robot_model:=master_neck2
[INFO] [launch]: All log files can be found below /home/bluelab/.ros/log/2024-08-12-10-40-37-332014-bluelab-320330
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [320332]
[INFO] [spawner-2]: process started with pid [320334]
[INFO] [xs_sdk-3]: process started with pid [320336]
[INFO] [robot_state_publisher-4]: process started with pid [320338]
[ros2_control_node-1] [WARN] [1723430437.701903155] [master_neck2.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-1] [INFO] [1723430437.702355355] [resource_manager]: Loading hardware 'XSHardwareInterface' 
[ros2_control_node-1] [INFO] [1723430437.706138287] [resource_manager]: Initialize hardware 'XSHardwareInterface' 
[xs_sdk-3] [INFO] Using Interbotix X-Series Driver Version: 'v0.3.6'.
[xs_sdk-3] [INFO] Using logging level 'INFO'.
[xs_sdk-3] [INFO] Retrieving motor config file at '/home/bluelab/interbotix_ws/install/interbotix_xsarm_control/share/interbotix_xsarm_control/config/master_neck2.yaml'.
[xs_sdk-3] [INFO] Retrieving mode config file at '/home/bluelab/interbotix_ws/install/interbotix_xsarm_ros_control/share/interbotix_xsarm_ros_control/config/modes.yaml'.
[xs_sdk-3] [INFO] Loaded mode configs from '/home/bluelab/interbotix_ws/install/interbotix_xsarm_ros_control/share/interbotix_xsarm_ros_control/config/modes.yaml'.
[xs_sdk-3] [INFO] Loaded motor configs from '/home/bluelab/interbotix_ws/install/interbotix_xsarm_control/share/interbotix_xsarm_control/config/master_neck2.yaml'.
[xs_sdk-3] [INFO] Pinging all motors specified in the motor_config file. (Attempt 1/3)
[xs_sdk-3] [INFO] 	Found DYNAMIXEL ID:  4, Model: 'XM540-W270', Joint Name: 'wrist_angle'.
[xs_sdk-3] [INFO] 	Found DYNAMIXEL ID:  3, Model: 'XM540-W270', Joint Name: 'elbow'.
[xs_sdk-3] [INFO] 	Found DYNAMIXEL ID:  2, Model: 'XM540-W270', Joint Name: 'shoulder'.
[xs_sdk-3] [INFO] 	Found DYNAMIXEL ID:  1, Model: 'XM540-W270', Joint Name: 'waist'.
[xs_sdk-3] [WARN] Writing startup register values to EEPROM. This only needs to be done once on a robot if using a default motor config file, or after a motor config file has been modified. Can set `write_eeprom_on_startup` to false from now on.
[xs_sdk-3] [INFO] The operating mode for the 'arm' group was changed to 'position' with profile type 'velocity'.
[xs_sdk-3] [ERROR] The 'gripper' joint/group does not exist. Was it added to the motor config file?
[xs_sdk-3] [INFO] Interbotix X-Series Driver is up!
[xs_sdk-3] [ERROR] [1723430438.426531381] [interbotix_xs_sdk.xs_sdk]: Joint 'gripper' does not exist. Was it added to the motor config file?
[ros2_control_node-1] terminate called after throwing an instance of 'std::out_of_range'
[ros2_control_node-1]   what():  vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)
[xs_sdk-3] [INFO] [1723430438.523870106] [interbotix_xs_sdk.xs_sdk]: InterbotixRobotXS is up!
[ERROR] [ros2_control_node-1]: process has died [pid 320332, exit code -6, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args -r __ns:=/master_neck2 --params-file /tmp/launch_params_uiq6lto0 --params-file /tmp/launch_params_e5ue_3g0'].
[spawner-2] [INFO] [1723430440.000149682] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-2] [INFO] [1723430442.016349526] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-2] [INFO] [1723430444.029253673] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-2] [INFO] [1723430446.044529250] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-2] [ERROR] [1723430448.060732071] [master_neck2.arm_controller_spawner]: Controller manager not available

Robot Model

px100,I modified all the config based on px100

Operating System

Ubuntu 22.04

ROS Version

ROS 2 Humble

Additional Info

No response

[lukeschmitt-tr]

See if the steps described in #75 answer this question for you.

[parzivar]

thanks for your answer！！！now it can working such as run：

but when i run：ros2 launch interbotix_xsarm_moveit xsarm_moveit.launch.py robot_model:=master_neck2 hardware_type:=fake
it can work, but will report errors：

And when I changed fake into actual, it couldn't work at all.
I have changed the URDF 、interbotix_ros_control/config、interbotix_xsarm_moveit/config， but cant solve the error：

[xs_sdk-5] [INFO] Skipping Load Configs.
[xs_sdk-5] [INFO] The operating mode for the 'arm' group was changed to 'position' with profile type 'velocity'.
[xs_sdk-5] [INFO] Interbotix X-Series Driver is up!
[xs_sdk-5] [ERROR] [1723520550.017055511] [interbotix_xs_sdk.xs_sdk]: Joint 'gripper' does not exist. Was it added to the motor config file?
[ros2_control_node-3] terminate called after throwing an instance of 'std::out_of_range'
[ros2_control_node-3]   what():  vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)
[xs_sdk-5] [INFO] [1723520550.114269090] [interbotix_xs_sdk.xs_sdk]: InterbotixRobotXS is up!
[ERROR] [ros2_control_node-3]: process has died [pid 473802, exit code -6, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args -r __ns:=/master_neck2 --params-file /tmp/launch_params_59kcxr0p --params-file /tmp/launch_params_ws1qfk10'].
[spawner-4] [INFO] [1723520551.768699492] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-4] [INFO] [1723520553.784304135] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-4] [INFO] [1723520555.800012211] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-4] [INFO] [1723520557.813944758] [master_neck2.arm_controller_spawner]: Waiting for '/master_neck2/controller_manager' services to be available
[spawner-4] [ERROR] [1723520559.827656964] [master_neck2.arm_controller_spawner]: Controller manager not available
[ERROR] [spawner-4]: process has died [pid 473804, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner -c /master_neck2/controller_manager arm_controller --ros-args -r __node:=arm_controller_spawner -r __ns:=/master_neck2'].

[lukeschmitt-tr]

As of now, the xs_hardware_interface (our hardware interface for ros2_control) expects that a gripper is part of the arm. You will have to modify that interface to remove that assumption, among other changes to configuration files. I would guess that the line causing the interface to crash is this one.

[parzivar]

Thank you very much for your help. Now I can run actual customized robot!
Finally, I still have a problem. I use moveit for motion planning. When I check collision-aware IK, the robot cannot directly drag its end.

Below is my xs_hardware_interface.ccp file. I hope it can help others in need.

// Copyright 2022 Trossen Robotics
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the the copyright holder nor the names of its
//      contributors may be used to endorse or promote products derived from
//      this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <interbotix_xs_ros_control/xs_hardware_interface.hpp>

#include <limits>
#include <memory>
#include <string>
#include <vector>

namespace interbotix_xs_ros_control
{

CallbackReturn XSHardwareInterface::on_init(const hardware_interface::HardwareInfo & info)
{
  info_ = info;

  nh = std::make_shared<rclcpp::Node>("xs_hardware_interface");
  executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
  executor->add_node(nh);

  // get hardware parameters
  group_name = info_.hardware_parameters["group_name"];
//  gripper_name = info_.hardware_parameters["gripper_name"];
  std::string js_topic = info_.hardware_parameters["joint_states_topic"];

  // create pubs, subs, and services
  pub_group = nh->create_publisher<interbotix_xs_msgs::msg::JointGroupCommand>(
    "commands/joint_group",
    1);
//  pub_gripper = nh->create_publisher<interbotix_xs_msgs::msg::JointSingleCommand>(
//    "commands/joint_single",
//    1);
  sub_joint_states = nh->create_subscription<sensor_msgs::msg::JointState>(
    js_topic,
    1,
    std::bind(&XSHardwareInterface::joint_state_cb, this, std::placeholders::_1));
  srv_robot_info = nh->create_client<interbotix_xs_msgs::srv::RobotInfo>("get_robot_info");

  using namespace std::chrono_literals;

  // wait for a few seconds for the xs_sdk services to load
  if (!srv_robot_info->wait_for_service(5s)) {
    RCLCPP_FATAL(
      nh->get_logger(),
      "Could not get robot_info service within timeout.");
    throw std::runtime_error("Could not get robot_info service within timeout.");
  }

  // set up RobotInfo service requests, call, and await responses
  auto group_info_srv = std::make_shared<interbotix_xs_msgs::srv::RobotInfo::Request>();
//  auto gripper_info_srv = std::make_shared<interbotix_xs_msgs::srv::RobotInfo::Request>();
  group_info_srv->cmd_type = "group";
  group_info_srv->name = group_name;
//  gripper_info_srv->cmd_type = "single";
//  gripper_info_srv->name = gripper_name;
  auto group_future = srv_robot_info->async_send_request(group_info_srv);
//  auto gripper_future = srv_robot_info->async_send_request(gripper_info_srv);
  executor->spin_until_future_complete(group_future);
//  executor->spin_until_future_complete(gripper_future);
  auto group_res = group_future.get();
  num_joints = group_res->num_joints;
  joint_state_indices = group_res->joint_state_indices;
//  auto grip_res = gripper_future.get();
//  joint_state_indices.push_back(grip_res->joint_state_indices.at(0));

  // Get robot joint names from service response, configure vectors
  std::vector<std::string> joint_names = group_res->joint_names;
//  joint_names.push_back(grip_res->joint_names.at(0));
  joint_positions.resize(num_joints);
  joint_velocities.resize(num_joints);
  joint_efforts.resize(num_joints);
  joint_position_commands.resize(num_joints);
  joint_commands_prev.resize(num_joints);

  // create thread that spins the executor for the pubs, subs, and services
  update_thread = std::thread(&XSHardwareInterface::executor_cb, this);

  while (joint_states.position.size() == 0 && rclcpp::ok()) {
    RCLCPP_INFO_ONCE(nh->get_logger(), "Waiting for joint states...");
  }

  RCLCPP_INFO(nh->get_logger(), "Joint states received.");

  // Initialize the joint_position_commands vector to the current joint states
  for (size_t i{0}; i < num_joints; i++) {
    joint_position_commands.at(i) = joint_states.position.at(joint_state_indices.at(i));
    joint_commands_prev.at(i) = joint_position_commands.at(i);
  }
  joint_commands_prev.resize(num_joints);
  gripper_cmd_prev = joint_states.position.at(joint_state_indices.back()) * 2;

  // Command robot to its sleep position to push it within its limits
  //  Until joint_limits_interface is merged into ros2_control repo
  //  https://github.com/ros-controls/ros2_control/pull/462
  interbotix_xs_msgs::msg::JointGroupCommand group_cmd;
  group_cmd.name = group_name;
  group_cmd.cmd = group_res->joint_sleep_positions;
  pub_group->publish(group_cmd);

  joint_position_commands.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  joint_positions.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  for (const hardware_interface::ComponentInfo & joint : info_.joints) {
    if (joint.command_interfaces.size() != 1) {
      RCLCPP_ERROR(
        nh->get_logger(),
        "Joint '%s' has %d command interfaces found. 1 expected.",
        joint.name.c_str(), static_cast<int>(joint.command_interfaces.size()));
      return CallbackReturn::ERROR;
    }

    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
      RCLCPP_ERROR(
        nh->get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.",
        joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
        hardware_interface::HW_IF_POSITION);
      return CallbackReturn::ERROR;
    }
  }
  return CallbackReturn::SUCCESS;
}

void XSHardwareInterface::executor_cb()
{
  executor->spin();
}

CallbackReturn XSHardwareInterface::start()
{
  return CallbackReturn::SUCCESS;
}

CallbackReturn XSHardwareInterface::stop()
{
  update_thread.join();  // make sure to join update thread before stopping
  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> XSHardwareInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    state_interfaces.emplace_back(
      hardware_interface::StateInterface(
        info_.joints[i].name,
        hardware_interface::HW_IF_POSITION,
        &joint_positions[i]));
    state_interfaces.emplace_back(
      hardware_interface::StateInterface(
        info_.joints[i].name,
        hardware_interface::HW_IF_VELOCITY,
        &joint_velocities[i]));
  }
  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> XSHardwareInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    command_interfaces.emplace_back(
      hardware_interface::CommandInterface(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION,
        &joint_position_commands[i]));
  }
  return command_interfaces;
}

return_type XSHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &)
{
  std::lock_guard<std::mutex> lck(joint_state_mtx_);
  for (size_t i = 0; i < num_joints; i++) {
    joint_positions.at(i) = joint_states.position.at(joint_state_indices.at(i));
  }
  return return_type::OK;
}

return_type XSHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
{
  interbotix_xs_msgs::msg::JointGroupCommand group_msg;
//  interbotix_xs_msgs::msg::JointSingleCommand gripper_msg;
  group_msg.name = group_name;
//  gripper_msg.name = gripper_name;
//  gripper_msg.cmd = joint_position_commands.back() * 2;

  for (size_t i{0}; i < num_joints; i++) {
    group_msg.cmd.push_back(joint_position_commands.at(i));
  }

  // Only publish commands if different than the previous update's commands
  if (joint_commands_prev != group_msg.cmd) {
    pub_group->publish(group_msg);
    joint_commands_prev = group_msg.cmd;
  }
//  if (gripper_cmd_prev != gripper_msg.cmd) {
//    pub_gripper->publish(gripper_msg);
//    gripper_cmd_prev = gripper_msg.cmd;
//  }
  return return_type::OK;
}

void XSHardwareInterface::joint_state_cb(const sensor_msgs::msg::JointState & msg)
{
  std::lock_guard<std::mutex> lck(joint_state_mtx_);
  joint_states = msg;
}

}  // namespace interbotix_xs_ros_control

#include "pluginlib/class_list_macros.hpp"  // NOLINT
PLUGINLIB_EXPORT_CLASS(
  interbotix_xs_ros_control::XSHardwareInterface,
  hardware_interface::SystemInterface)