Test functionality of controller's output in hardware_interface

[Doctor.No]

Dear Community,

I’m still confused about ros_control, specifically the hardware_interface. The tutorial of lesson 6 was great, and I tried to merge it with tutorial 3 to get some output.

my_robot_control.yaml looks as in the tutorial:

rrbot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

I just modified my_robot_control.launch slightly and integrated the launch commands from tutorial 6 to load the rrbot_hardware_interface:

  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/>


  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/rrbot/joint_states" />
  </node>

    <node name="rrbot_hardware_interface" pkg="rrbot_hardware_interface" type="rrbot_hardware_interface_node">
    <!-- Load standard controller joint names from YAML file to parameter server -->
    <rosparam command="load" file="$(find rrbot_bringup)/config/joint_names.yaml" />
  </node>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam command="load" file="$(find rrbot_bringup)/config/$(arg controllers_yaml).yaml" />

However I am confused about the write() function of rrbot_hardware_interface.cpp from tutorial 6:

bool RRBotHardwareInterface::write(const ros::Time time,
                                   const ros::Duration period) {
  // write command to hardware, in this example do mirror command to states
  for (size_t i = 0; i < hw_position_commands_.size(); ++i) {
    hw_position_states_[i] =
        hw_position_states_[i] +
        (hw_position_commands_[i] - hw_position_states_[i]) / 100.0;
  }

I thought that the write() function outputs the controller’s commands, meaning that if I assign a sine-wave to joint 1, some output should be written to the hw_position_commands_.

As seem in this screenshot I just receive zeros for the hardware_interface, while joint 1 does a sine-wave in gazebo.

Therefore, my question: Assuming that the controller receives some input (e.g. sine wave), how can I verify that the controller and hardware_interface work correctly and print the commands in hw_position_commands_?

Edit:
I understand that it is important to have a clear view of hardware and software setup, as explained in detail in this tutorial, however I’m still confused why the hw_position_commands_ does yield any output if I assign an input to the controller (e.g. diff_drive_controller, as in image of the tutorial)

[rzegers]

Hi @Doctor.No,

in the rqt screenshot that you posted, at the right side, I think this is the console output / ros logs, right?

My question is: do you know where in the code the line that publishes the message "Got state x.x for joint x!" is located ? I think that part in the code could provide some clues.

Cheers,

Roberto

[Doctor.No]

sure, this is right from the tutorial 6 of ros_control. It is the read() function of file rrbot_hardware_interface.cpp

bool RRBotHardwareInterface::read(const ros::Time time,
                                  const ros::Duration period) {
  // read robot states from hardware, in this example print only
  ROS_INFO_NAMED("RRBotHardwareInterface", "Reading...");

  // write command to hardware, in this example do mirror command to states
  for (size_t i = 0; i < hw_position_states_.size(); ++i) {
    ROS_INFO_NAMED("RRBotHardwareInterface", "Got state %.2f for joint %zu!",
                   hw_position_states_[i], i);
  }

  return true;
}

[f20200403]

Hello @Doctor.No
Understanding the read, update and write, which forms the control loop, should help you with your endeavour. I have found the following links useful; though they talk about ROS, they are useful to get the essence:
1.hardware_interface: hardware_interface::RobotHW Class Reference
2.hardware_interface: Main Page).
Try having a look, and let us continue the discussion then.

Best,

Arunabh

[rzegers]

Hi @Doctor.No,

thanks for pointing me to the code you mentioned.

@Doctor.No

I thought that the write() function outputs the controller’s commands, meaning that if I assign a sine-wave to joint 1, some output should be written to the hw_position_commands_.

As seem in this screenshot I just receive zeros for the hardware_interface, while joint 1 does a sine-wave in gazebo.

You are correct, the write() function is used to send the joint commands to the hardware. In this particular case, since we don’t have real hardware, we assign the command value to the state value using this assignment:

hw_position_states_[i] = hw_position_states_[i] + (hw_position_commands_[i] - hw_position_states_[i]) / 100.0;

It seems to me that the above equation is always zero. Could you add a print statement inside that write method and see what the value of hw_position_commands_[i] is ?
If that value is always zero then, the issue is coming from where or how hw_position_commands_[i] gets its value.

Cheers,

Roberto

[Doctor.No]

Thank you f20200403 and rzegers for your reply. Since there seems to have been some confusion here, I want to briefly clarify a few things.

My goal is that Tutorial 6: Hardware_Interface of RRBot, does not only return zeros as result. Therefore I took the tutorial 3, added the hardware_interface in the launch file and created a sin-wave at the joint.

My rrbot_hardware_interface.cpp looks almost as in the tutorial, I just printed the hardware commands via ROS_INFO_NAMED().

Here’s the complete code of the tutorial’s hardware interface (NOTE: If I am not allowed to post code from the official tutorial, please do tell me!). The lines I modified can be spotted by the “HERE” comment.

#include "rrbot_hardware_interface/rrbot_hardware_interface.hpp"

#include <limits>
#include <vector>

namespace rrbot_hardware_interface {

bool RRBotHardwareInterface::init(ros::NodeHandle & /*root_nh*/,
                                  ros::NodeHandle &robot_hw_nh) {
  if (!robot_hw_nh.getParam("joint_names", joint_names_)) {
    ROS_ERROR("Cannot find required parameter 'joint_names' on the parameter "
              "server.");
    throw std::runtime_error("Cannot find required parameter "
                             "'joint_names' on the parameter server.");
  }

  size_t num_joints = joint_names_.size();
  ROS_INFO_NAMED("RRBotHardwareInterface", "Found %zu joints.", num_joints);

  hw_position_states_.resize(num_joints,
                             std::numeric_limits<double>::quiet_NaN());
  hw_position_commands_.resize(num_joints,
                               std::numeric_limits<double>::quiet_NaN());
  hw_velocity_states_.resize(num_joints,
                             std::numeric_limits<double>::quiet_NaN());
  hw_effort_states_.resize(num_joints,
                           std::numeric_limits<double>::quiet_NaN());

  // Create ros_control interfaces
  for (size_t i = 0; i < num_joints; ++i) {
    // Create joint state interface for all joints
    joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
        joint_names_[i], &hw_position_states_[i], &hw_velocity_states_[i],
        &hw_effort_states_[i]));

    // Create joint position control interface
    // HERE we receive the commands from the controller's output (if I'm not completely mistaken)
    position_command_interface_.registerHandle(hardware_interface::JointHandle(
        joint_state_interface_.getHandle(joint_names_[i]),
        &hw_position_commands_[i]));
  }

  registerInterface(&joint_state_interface_);
  registerInterface(&position_command_interface_);

  // stat execution on hardware
  ROS_INFO_NAMED("RRBotHardwareInterface", "Starting...");

  // in this simple example reset state to initial positions
  for (size_t i = 0; i < num_joints; ++i) {
    hw_position_states_[i] = 0.0; // initial position is zero
    hw_position_commands_[i] = hw_position_states_[i];
  }

  return true;
}

bool RRBotHardwareInterface::read(const ros::Time time,
                                  const ros::Duration period) {
  // read robot states from hardware, in this example print only
  ROS_INFO_NAMED("RRBotHardwareInterface", "Reading...");

  // write command to hardware, in this example do mirror command to states
  for (size_t i = 0; i < hw_position_states_.size(); ++i) {
    ROS_INFO_NAMED("RRBotHardwareInterface", "Got state %.2f for joint %zu!",
                   hw_position_states_[i], i);
  }

  return true;
}

bool RRBotHardwareInterface::write(const ros::Time time,
                                   const ros::Duration period) {
  // HERE we print the controller's output (which is in my understanding the hw_position_commands_)
  for (size_t i = 0; i < hw_position_commands_.size(); ++i) {
    // hw_position_states_[i] =
    //     hw_position_states_[i] +
    //     (hw_position_commands_[i] - hw_position_states_[i]) / 100.0;
    ROS_INFO_NAMED("RRBotHardwareInterface",
                   "Got command %.2f for joint %zu!",
                   hw_position_commands_[i], i);
  }

  return true;
}

} // namespace rrbot_hardware_interface

My question is: How can I print the controller’s output (e.g. sin-wave)? I thought this was achievable via the hw_position_commands_ in the write function, but I just receive zeros.

[f20200403]

Hello @Doctor.No ,
Let me try to propose a general solution. Could you try running the ros2 topic list command to find the topic that is providing the joint state information and then use the ‘ros2 topic echo <topic_name>’ to print the joint state information to the terminal? That should give you the position, velocity, and effort values for each joint being controlled. Let me know if that works for you.

Best,

Arunabh