How to use ros_control on your robot-ros_control_usage-ros_control-ros

References:

1. ROS Control | ROS Robotics

2. https://medium.com/@slaterobotics/how-to-implement-ros-control-on-a-custom-robot-748b52751f2e

3. The repository of this blogger: bandasaikrishna (github.com)

4. The example of using ros_control to control a DC motor by Arduino: https://github.com/bandasaikrishna/ros_control_example

1 What is ros_control

ROS_Control is a set of packages that includes controller interface, controller manager, transmissions, hardware interfaces and control toolbox.

All these packages together will allow you interact and control the joint actuators of the robot.

1.1 The workflow of ros_control

• ros_control’s workflow 1

ros_control will take the joint state data and an input set point (goal) as input from user/3rd party application as shown in the following picture and sends the appropriate commands to the actuators as an output.

In-order to achieve the provided set point(goal) , it uses a generic control loop feedback mechanism, typically a PID controller, to control the output. This output is passed to robot through the hardware interface.

• ros_control’s workflow 2

The third party block (Moveit/Navigation Stack) represents the one that is sending goal to ros_control packages. The controllers (base_controller/arm_controller/base controller) are responsible for computing the output command required to achieve the set goal.

In order to move the joints controllers have to talk to actuators. The hardware interface node (RobotHW) sits between the controllers & the real hardware and it will command the actuators to move and get the feedback from the joint sensors. This is how ros_control will work.

• ros_control’s input and output

Input: The current state and target state of the robot

Output: The command to be executed by the robot actuator

1.2 The type of controllers

ROS control package provides a set of controller plugins to interact in different ways with the joints of the robot .

The list is as follows:

1. joint_state_controller: This controller reads all joint positions and publish them on to the topic “/joint_states”. This controller does not send any command to the actuators. Used as “joint_state_controller/JointStateController”.

2. effort_controllers: Use when you want to send commands to an effort interface. This means that joint actuator you want to control accepts effort [current (or) voltage] command.

   1. JointPositionController : Used as “effort_controllers/JointPositionController”. This controller plugin accepts the position [radians (or) meters] value as input. Error (goal position - current position) is mapped to output effort command through a PID loop.​

   2. JointVelocityController : Used as “effort_controllers/JointVelocityController”. This controller plugin accepts the velocity [radians/sec (or) meters/sec] values as input. Error (goal velocity - current velocity) is mapped to output effort command through a PID loop.

   3. JointEffortController : Used as “effort_controllers/JointEffortController”. This controller plugin accepts the effort [force (or) torque] values as input. The input effort is simply forwarded as output effort command to the joint actuator. The values of P, I and D have no effect on output effort.

   4. JointGroupPositionController : Used as “effort_controllers/JointGroupPositionController”. This controller functions same as effort_controllers/JointPositionController for a group of joints.

   5. JointGroupEffortController : Used as “effort_controllers/JointGroupEffortController”. This controller functions same as effort_controllers/JointEffortController for a group of joints.

3. velocity_controllers: Use when you want to send commands to an velocity interface. This means that joint actuator you want to control accepts velocity command directly.

   1. JointPositionController : Used as “velocity_controllers/JointPositionController”. This controller plugin accepts the position [radians (or) meters] value as input. Error (goal position - current position) is mapped to output velocity command through a PID loop.

   2. JointVelocityController : Used as “velocity_controllers/JointVelocityController”. This controller plugin accepts the velocity [radians/sec (or) meters/sec] values as input. The input velocity value is simply forwarded as output command to the joint actuator. The values of P, I and D have no effect on output command.

   3. JointGroupVelocityController : Used as “velocity_controllers/JointGroupVelocityController”. This controller functions same as velocity_controllers/JointVelocityController for a group of joints.

4. position_controllers: Use when you want to send commands to an position interface. This means that joint actuator you want to control accepts position command directly.

   1. JointPositionController : Used as “position_controllers/JointPositionController”. This controller plugin accepts the position [radians (or) meters] values as input. The input position value is simply forwarded as output command to the joint actuator. The values of P, I and D have no effect on output command.

   2. JointGroupPositionController : Used as “position_controllers/JointGroupPositionController”. This controller functions same as position_controllers/JointPositionController for a group of joints.​

5. joint_trajectory_controllers: This controller is used for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached. Waypoints consist of positions, and optionally velocities and accelerations. For more information on this controller, click here.

   1. effort_controllers : Used as “effort_controllers/JointTrajectoryController”. This controller plugin is used for the joint actuators that accepts effort [current (or) voltage]. The position+velocity trajectory following error is mapped to the output effort commands through a PID loop.

   2. velocity_controllers : Used as “velocity_controllers/JointTrajectoryController”. This controller plugin is used for the joint actuators that accepts velocity command directly. The position+velocity trajectory following error is mapped to the output velocity commands through a PID loop.

   3. position_controllers : Used as “position_controllers/JointTrajectoryController”. This controller plugin is used for the joint actuators that accepts position command directly. The desired positions in the specified trajectory are simply forwarded to the joints. The values of P, I and D have no effect on output command.

Preference: The source code of ros_controllers in ros_control package. In addition, you can write your own controller plugin anytime and use it, if you are not satisfied with the existing controllers.

1.3 The type of hardware interfaces

hardware_interface implements all the building blocks for constructing a Robot hardware abstraction. It is the software representation of the robot.

We have a set of interfaces for different types of motors/actuators and sensors as listed below:

• Interfaces for Joint Actuators:

  1. EffortJointInterface : This interface is used for the joint actuators which accepts effort (voltage or current) command. As you might have guessed by now, this interface is used with the effort_controllers.

  2. VelocityJointInterface : This interface is used for the joint actuators which accepts velocity command directly. Used with the velocity_controllers.

  3. PositionJointInterface : This interface is used for the joint actuators which accepts position command directly. Used with the position_controllers.

• Interfaces for Joint Sensors:

  1. JointStateInterface : This interface is used when you have senors for getting the current position or/and velocity or/and effort (force/torque) of the joints. Used with joint_state_controller.

JointStateInterface is used in almost all robot because it gets the current joint positions of the robot which data in-turn is used by the tf/tf2 to calculate forward kinematics of the robot.

2. ImuSensorInterface : This interface is used when you have a IMU sensors which is used to get the orientation, angular velocity and linear acceleration of the joint/robot. This is used with imu_sensor_controller.

​ Based on the type of the motors and sensors you are using, you have to choose the interfaces accordingly.

---

2 How to use ros_control on your robot

Now that we have understood the types of interfaces available for joint actuators and sensors, lets learn how to write the hardware_interface node (the software representation of our robot).

• Note: I will write the following code in ubuntu20.04 with ros noetic. The different distributions are likely to have different API.

2.1 write .h and .cpp files

So lets assume a robot with three joints as example and will write hardware interface for that.

Lets start.

Our robot has three actuators JointA, JointB and JointC and position sensors at each joint. Lets say JointA & JointB accepts effort commands and JointC accepts position command.

Okay, we have enough information about our robot to write the hardware interface so lets begin to write the code.

Well, we need to create a header file and a cpp file in the robot’s catkin package. Let me name them as MyRobot_hardware_interface.h and MyRobot_hardware_interface.cpp.

The complete sample code is as follows:

• MyRobot_hardware_interface.h

#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>

class MyRobot : public hardware_interface::RobotHW 
{
    public:
        MyRobot(ros::NodeHandle& nh);
        ~MyRobot();
        void init();
        void update(const ros::TimerEvent& e);
        void read();
        void write(ros::Duration elapsed_time);
        
    protected:
        // Depend your tpyes of interfaces
        hardware_interface::JointStateInterface joint_state_interface_;
        hardware_interface::EffortJointInterface effort_joint_interface_;
        hardware_interface::PositionJointInterface position_joint_interface_;
        
        joint_limits_interface::JointLimits limits;
        joint_limits_interface::EffortJointSaturationInterface effortJointSaturationInterface;
        joint_limits_interface::PositionJointSaturationInterface positionJointSaturationInterface;
        
        double joint_position_[3];
        double joint_velocity_[3];
        double joint_effort_[3];
        double joint_effort_command_[2];
        double joint_position_command_;
        
        ros::NodeHandle nh_;
        ros::Timer my_control_loop_;
        ros::Duration elapsed_time_;
        double loop_hz_;
        boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
};

Lets break down the code of above header file and understand it.

1. Included all required header files.

#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>

2. Our robot hardware class must inherit from the hardware_interface::RobotHW

This is the class of robots hardware which consists of the methods for reading the joint sensors data, sending commands to the motor, control loop and of-course the joint interfaces data.

class MyRobot : public hardware_interface::RobotHW 
{ ...

3. Here are the constructor, destructor and other methods of MyRobot class.
   • init() method is where we define all joint handle, joint’s interfaces and joint limits interfaces.
   • update() method is the control loop().
   • read() method is for reading joint sensor data.
   • write() method for sending command to motors.

    public:
        MyRobot(ros::NodeHandle& nh);
        ~MyRobot();
        void init();
        void update(const ros::TimerEvent& e);
        void read();
        void write(ros::Duration elapsed_time);

4. We will see these definitions about hardware_interface in the header file.

Declare the type of joint interfaces and joint limit interfaces your robot actuators/motors are using.

Since our example robot has only effort and position accepting motors, I haves declared joint interfaces and limit interfaces for effort and position.

If you have some motors accepting velocity commands, then declare the velocity corresponding interfaces.

    protected:
        hardware_interface::JointStateInterface joint_state_interface_;
        hardware_interface::EffortJointInterface effort_joint_interface_;
        hardware_interface::PositionJointInterface position_joint_interface_;
        
        joint_limits_interface::JointLimits limits;
        joint_limits_interface::EffortJointSaturationInterface effortJointSaturationInterface;
        joint_limits_interface::PositionJointSaturationInterface positionJointSaturationInterface;

5. The command sent to the robot in this example

joint_position_[3], joint_velocity_[3], joint_effort_[3] are the array variables for reading position, velocity and effort of the joints of our robot. These variables are used by joint_state_interface.

The size of array is 3 since our example robot has 3 joints in total.
joint_effort_command_[2] array is for sending the commands to JointA and JointB.
joint_position_command_ is for sending command to JointC.

        double joint_position_[3];
        double joint_velocity_[3];
        double joint_effort_[3];
        double joint_effort_command_[2];
        double joint_position_command_;

6. We will see these definitions about ros_control in the header file.

Here are some other variables used in this our hardware_interface node. my_control_loop_ is a timer which calls control loop (update method) periodically at a set frequency (loop_hz_ ).

        ros::NodeHandle nh_;
        ros::Timer my_control_loop_;
        ros::Duration elapsed_time_;
        double loop_hz_;
        boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
};

---

So next lets have a look at MyRobot_hardware_interface.cpp

The following cpp file is self explanatory with the help of the comments.

• MyRobot_hardware_interface.cpp

#include <YOUR_PACKAGE_NAME/MYRobot_hardware_interface.h>

MyRobot::MyRobot(ros::NodeHandle& nh) : nh_(nh) {

// Declare all JointHandles, JointInterfaces and JointLimitInterfaces of the robot.
    init();
    
// Create the controller manager
    controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
    
//Set the frequency of the control loop.
    loop_hz_=10;
    ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
    
//Run the control loop
    my_control_loop_ = nh_.createTimer(update_freq, &MyRobot::update, this);
}

MyRobot::~MyRobot() {
}

void MyRobot::init() {
        
    // Create joint_state_interface for JointA
    hardware_interface::JointStateHandle jointStateHandleA("JointA", &joint_position_[0], &joint_velocity_[0], &joint_effort_[0]);
    joint_state_interface_.registerHandle(jointStateHandleA);
    // Create effort joint interface as JointA accepts effort command.
    hardware_interface::JointHandle jointEffortHandleA(jointStateHandleA, &joint_effort_command_[0]);
    effort_joint_interface_.registerHandle(jointEffortHandleA);     

    // Create joint_state_interface for JointB
    hardware_interface::JointStateHandle jointStateHandleB("JointB", &joint_position_[1], &joint_velocity_[1], &joint_effort_[1]);
    joint_state_interface_.registerHandle(jointStateHandleB);
    // Create effort joint interface as JointB accepts effort command..
    hardware_interface::JointHandle jointEffortHandleB(jointStateHandleB, &joint_effort_command_[1]);
    effort_joint_interface_.registerHandle(jointEffortHandleB);    
    
    // Create joint_state_interface for JointC
    hardware_interface::JointStateHandle jointStateHandleC("JointC", &joint_position_[2], &joint_velocity_[2], &joint_effort_[2]);
    joint_state_interface_.registerHandle(jointStateHandleC);
    // Create position joint interface as JointC accepts position command.
    hardware_interface::JointHandle jointPositionHandleC(jointStateHandleC, &joint_position_command_);
    position_joint_interface_.registerHandle(jointPositionHandleC);    

    // Register all joints interfaces    
    registerInterface(&joint_state_interface_);
    registerInterface(&effort_joint_interface_);
    registerInterface(&position_joint_interface_);    
}

    //This is the control loop
void MyRobot::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    read();
    controller_manager_->update(ros::Time::now(), elapsed_time_);
    write(elapsed_time_);
}

void MyRobot::read() {​

  // Write the protocol (I2C/CAN/ros_serial/ros_industrial)used to get the current joint position and/or velocity and/or effort       

  // from robot.
  // and fill JointStateHandle variables joint_position_[i], joint_velocity_[i] and joint_effort_[i]
}

void MyRobot::write(ros::Duration elapsed_time) {
  // Safety
  effortJointSaturationInterface.enforceLimits(elapsed_time);   // enforce limits for JointA and JointB
  positionJointSaturationInterface.enforceLimits(elapsed_time); // enforce limits for JointC

  // Write the protocol (I2C/CAN/ros_serial/ros_industrial)used to send the commands to the robot's actuators.
  // the output commands need to send are joint_effort_command_[0] for JointA, joint_effort_command_[1] for JointB and 

  //joint_position_command_ for JointC.

}

int main(int argc, char** argv)
{

    // Initialze the ROS node.
    ros::init(argc, argv, "MyRobot_hardware_inerface_node");
    ros::NodeHandle nh;
    
    // Separate Sinner thread for the Non-Real time callbacks such as service callbacks to load controllers
    ros::MultiThreadedspinner spinner(2); 
    
    // Create the object of the robot hardware_interface class and spin the thread. 
    MyRobot ROBOT(nh);
    spinner.spin();
    
    return 0;
}

I hope most part of the above cpp file is self explanatory with the help of the comments, I’ll explain only the important lines.

• This line will make the update() method (PID control loop) loop periodically at a specified frequency.

my_control_loop_ = nh_.createTimer(update_freq, &MyRobot::update, this);

• The control loop

//This is the control loop
void MyRobot::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    read();
    controller_manager_->update(ros::Time::now(), elapsed_time_);
    write(elapsed_time_);
}

update() method will simply call read() method to get the current joint states.
Then the current joint states are updated to the controller manager to compute the error (current - goal) and generate output commands for each joint using PID loop.
And finally calls write() method to send the output commands to the actuators/joints.

• Note: The above code can be improved in many ways, so you’re able to adapt it to your own situation.

2.2 controllers.yaml

Before going to write the configuration files, we have to first decide what actually we want to control (is it position, effort or velocity of the joints).

In addition, you have to use the space instead of the tab in .yaml files!

So lets go through controllers.yaml file.

• controllers.yaml

MyRobot:
    # Publish all joint states
    joints_update:
      type: joint_state_controller/JointStateController
      publish_rate: 50
    
    JointA_EffortController:                               # Name of the controller
      type: effort_controllers/JointPositionController     # Since JointA uses effort interface this controller type is  used      
      joint: JointA                                        # Name of the joint for which this controller belongs to.
      pid: {p: 100.0, i: 10.0, d: 1.0}                     # PID values
    
    JointB_EffortController:                                             
      type: effort_controllers/JointPositionController     # Since JointB uses effort interface this controller type is  used   
      joint: JointB                                                                 
      pid: {p: 100.0, i: 1.0, d: 0.0}

    JointC_PositionController:
      type: position_controllers/JointPositionController   # Since JointC uses position interface this controller type is  used 
      joint: JointC
      # No PID values defined since this controller simply passes the input position command to the actuators.

So based on the joint interface and what needs to be controlled, the type of the controller decided.

---

2.3 joint_limits.yaml

• joint_limits.yaml

joint_limits:

    JointA:
      has_position_limits: true
      min_position: -1.57
      max_position: 1.57
      has_velocity_limits: true
      max_velocity: 1.5
      has_acceleration_limits: false
      max_acceleration: 0.0
      has_jerk_limits: false
      max_jerk: 0
      has_effort_limits: true
      max_effort: 255

    JointB:
      has_position_limits: true
      min_position: 0
      max_position: 3.14
      has_velocity_limits: true
      max_velocity: 1.5
      has_acceleration_limits: false
      max_acceleration: 0.0
      has_jerk_limits: false
      max_jerk: 0
      has_effort_limits: true
      max_effort: 255
      
    JointC:
      has_position_limits: true
      min_position: 0
      max_position: 3.14
      has_velocity_limits: false
      max_velocity: 0
      has_acceleration_limits: false
      max_acceleration: 0.0
      has_jerk_limits: false
      max_jerk: 0
      has_effort_limits: false
      max_effort: 0

This is the format to write the joint limits of your robot. These joint limit values are used by the joint_limits_interface defined in the hardware_interface node, to keep the robot within the safety limits by enforcing the output commands.

---

2.4 MyRobot_control.launch

• MyRobot_control.launch

<launch>
    
  <rosparam file="$(find YOUR_PACKAGE_NAME)/config/controllers.yaml" command="load"/>
  <rosparam file="$(find YOUR_PACKAGE_NAME)/config/joint_limits.yaml" command="load"/>
​
  <node name="MyRobotHardwareInterface" pkg="YOUR_PACKAGE_NAME" type="MyRobot_hardware_inerface_node" output="screen"/>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/ >
    
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
        args="
            /MyRobot/joints_update
            /MyRobot/JointA_EffortController
            /MyRobot/JointB_EffortController
            /MyRobot/JointC_PositionController
        "/> 

</launch>

Again the above launch file is self explanatory but I would like to talk a bit about controller_manager.

In the above launch file controllers are loaded into the controller_manager through the controller “spawner” python script.

There is another way to load the controllers. Controllers are loaded into the the controller_manager through service requests.

These service requests can be sent manually at the command line. Click here for more info on how to use command line to load/start/stop the controllers.

---

Once the controllers are loaded and started, you can send the desired goal through the below topic interface.

ros_controllers will send the appropriate output commands to the actuators to achieve the goal.

Topic names for controlling our example robot.

• JointA: /MyRobot/JointA_EffortController/command
• JointB: /MyRobot/JointB_EffortController/command
• JointC: /MyRobot/JointC_PositionController/command

---

Summary:

1. You have to write the hardware_interface node for your robot.
2. Write the config file to select controller type for the joints based on the joint interface & application and PID values(if requrired).
3. Write a config file to define the joint limits of your robot.
4. Load and start controllers through controller_manager.
5. Use the topic interface to send the input goal to the controllers.