Slate Robotics

Oct 9, 2020

9 min read

How to implement ros_control on a custom robot

ROS has a ton of built-in packages to make it so that you don’t have to do math. When I was first studying robotics, I was blown away by how much math was required to just control a two-wheel differential drive robot. Kinematics, PID control, and everything in-between meant that I spent a lot of time solving problems that were already solved. This is the beauty of ROS.

ros_control is another one of those packages that allows you to focus your time on problems specific to your robot — the problems that haven’t solved. Well, sort of. Unfortunately, I found that the tutorials on ros.org for ros_control were not terribly decipherable. Only after I had spent a week going through source code could I really look back on these and understand what they meant.

This is a blog post to help those of you who might be struggling like I was. The example code used throughout the tutorial is boilerplate code that won’t build. It’s to give you a better idea for how to setup your own robot-specific package. I’ll also assume a basic understanding of ROS: creating your own packages, rostopics, rosservice, roscpp, etc.

Overview

  • Standardization of APIs for controllers and hardware interfaces, making it easier to integrate with other packages, such as MoveIt
  • Built-in control loop feedback mechanisms like PID controllers
  • Easily enforce joint limits at a low level in the stack

The most helpful image I’ve found of understanding how it works is this:

Other packages send certain high-level desired goals to the controllers, and ros_control utilizes its controllers to work with the hardware to move joints based on those specifications.

Let’s say you want to move a joint from 0 radians to 1.57 radians. The robot’s state before you do anything is 0 radians. You (or a high level package) sends 1.57 to that joint’s pre-specified Joint Position Controller as the desired goal. The Joint Position Controller works with an even lower level package to actually send current to the actuators. The Joint Position Controller has its own PID that continuously reads the joint’s state and adjusts the current sent to the motors based on the error between the current state and the goal state.

ros_control handles two things from that process:

  • receiving the goals (effort, position, velocity, trajectory, etc.)
  • running the PID controllers

ros_control doesn’t know or handle:

  • implementing hardware control (sending current to motors)
  • reading hardware state

You have to control the hardware (actuators, servos, motors, etc.) using your own code by listening to what ros_control says it should do. You also must provide ros_control the current state of the robot and its joints if you want to control position, velocity, trajectory, etc.

Installation

sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers

Setting up your package

So, in order to get ros_control on our robot, we’ll need to create our own package called something like ROBOT_hardware_intereface. At Slate Robotics, we built tr1_hardware_interface to control the TR1 with ros_control, which is the basis for most of this tutorial. I recommend you check out that repository if you’re confused at any point.

The directory for our ROBOT_hardware_interface package is simple and follows the standard guidelines. We’ll need a config/ directory where you have .yaml files to define controllers, hardware (the joints available to the controllers), and joint limits. We'll make some launch files in launch/ to make it easy to fire-up certain controllers. There are only two cpp files we'll need to create in our src/ directory, and two header files in our include/ROBOT_hardware_interface/ directory. Here's what that looks like:

launch/
ROBOT_position_controllers.launch
config/
controllers.yaml
hardware.yaml
joint_limits.yaml
include/
ROBOT_hardware_interface/
ROBOT_hardware.h
ROBOT_hardware_interface.h
src/
ROBOT_hardware_interface.cpp
ROBOT_hardware_interface_node.cpp
CMakeLists.txt
package.xml

launch/ROBOT_position_controllers.launch

<launch>
<rosparam file="$(find ROBOT_hardware_interface)/config/hardware.yaml" command="load"/>
<rosparam file="$(find ROBOT_hardware_interface)/config/controllers.yaml" command="load"/>
<rosparam file="$(find ROBOT_hardware_interface)/config/joint_limits.yaml" command="load"/>
<node name="ROBOT_hardware_interface" pkg="ROBOT_hardware_interface" type="ROBOT_hardware_interface_node" output="screen"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/"
args="
/ROBOT/controller/state
/ROBOT/controller/position/YOUR_ROBOT_JOINT
/ROBOT/controller/position/YOUR_OTHER_ROBOT_JOINT
"/>
</launch>

All we’re doing here is:

  • loading rosparams hardware, controllers, and joint_limits
  • firing up our ROBOT_hardware_interface node
  • firing up controller_spawner node, which is part of ros_control stack

Take note of “args” in the controller_spawner node. We’re specifying which controllers from controllers.yaml that we want to use. Speaking of which, controllers.yaml follows this structure:

config/controllers.yaml

ROBOT:
controller:
state:
type: joint_state_controller/JointStateController
publish_rate: 50
position:
YOUR_ROBOT_JOINT:
type: effort_controllers/JointPositionController
joint: YOUR_ROBOT_JOINT
pid: {p: 10.0, i: 0.0, d: 1.0}
YOUR_OTHER_ROBOT_JOINT:
type: effort_controllers/JointPositionController
joint: YOUR_ROBOT_JOINT
pid: {p: 5.0, i: 2.0, d: 1.0}

The type element can be anything from the ros_controllers repository. Something confusing about this is that a controller is seemingly both an effort and position controller at the same time. For instance, there's both an effort_controllers/JointPositionController and position_controllers/JointPositionController option.

The difference is what commands get passed to your hardware. effort_controllers/ means that the controller is using an effort command (the amount of current to the motors, in most cases) to control position, and position_controllers/ means that the controller is using position itself to control position, which might make sense for controlling servos, for instance.

Further, each controller will require its own parameters. You can view the comments of the header files in the ros_controllers repository to see what’s required. JointPositionController, for example, requires you define a joint and pid parameter. As you can see, the joint is something in rosparam, which came from our config/hardware.yaml file.

config/hardware.yaml

ROBOT:
hardware_interface:
loop_hz: 10 # hz
joints:
- YOUR_ROBOT_JOINT
- YOUR_OTHER_ROBOT_JOINT

We have defined loop_hz, which is a parameter we've set here for convenience. We'll use it in our src/ROBOT_hardware_interface.cpp file. And of course, the joints are defined here as well.

You can list as many joints as your robot has to offer here. You’ll have access to the names of these joints as we loop through to control them in the src/ files, so you can use that to your advantage when actuating the motors.

config/joint_limits.yaml

joint_limits:
YOUR_ROBOT_JOINT:
has_position_limits: true
min_position: -1.0
max_position: 1.5708
has_velocity_limits: true
max_velocity: 2.0
has_acceleration_limits: true
max_acceleration: 5.0
has_jerk_limits: true
max_jerk: 100.0
has_effort_limits: true
max_effort: 1.0
YOUR_OTHER_ROBOT_JOINT:
has_position_limits: true
min_position: -1.5708
max_position: 3.1415
has_velocity_limits: true
max_velocity: 2.0
has_acceleration_limits: true
max_acceleration: 5.0
has_jerk_limits: true
max_jerk: 100.0
has_effort_limits: true
max_effort: 1.0

include/ROBOT_hardware_interface/ROBOT_hardware.h

#ifndef ROS_CONTROL__ROBOT_HARDWARE_H
#define ROS_CONTROL__ROBOT_HARDWARE_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 <joint_limits_interface/joint_limits_rosparam.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>

namespace ROBOT_hardware_interface
{
/// \brief Hardware interface for a robot
class ROBOTHardware : public hardware_interface::RobotHW
{
protected:
// Interfaces
hardware_interface::JointStateInterface joint_state_interface_;
hardware_interface::PositionJointInterface position_joint_interface_;
hardware_interface::VelocityJointInterface velocity_joint_interface_;
hardware_interface::EffortJointInterface effort_joint_interface_;

joint_limits_interface::EffortJointSaturationInterface effort_joint_saturation_interface_;
joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limits_interface_;
joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface_;
joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limits_interface_;
joint_limits_interface::VelocityJointSaturationInterface velocity_joint_saturation_interface_;
joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limits_interface_;

// Custom or available transmissions
// transmission_interface::RRBOTTransmission rrbot_trans_;
// std::vector<transmission_interface::SimpleTransmission> simple_trans_;

// Shared memory
int num_joints_;
int joint_mode_; // position, velocity, or effort
std::vector<std::string> joint_names_;
std::vector<int> joint_types_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<double> joint_effort_command_;
std::vector<double> joint_lower_limits_;
std::vector<double> joint_upper_limits_;
std::vector<double> joint_effort_limits_;

}; // class

} // namespace

#endif

include/ROBOT_hardware_interface/ROBOT_hardware_interface.h

#ifndef ROS_CONTROL__ROBOT_HARDWARE_INTERFACE_H
#define ROS_CONTROL__ROBOT_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_interface.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#include <ROBOTcpp/ROBOT.h>
#include <ROBOT_hardware_interface/ROBOT_hardware.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;


namespace ROBOT_hardware_interface
{
static const double POSITION_STEP_FACTOR = 10;
static const double VELOCITY_STEP_FACTOR = 10;

class ROBOTHardwareInterface: public ROBOT_hardware_interface::ROBOTHardware
{
public:
ROBOTHardwareInterface(ros::NodeHandle& nh);
~ROBOTHardwareInterface();
void init();
void update(const ros::TimerEvent& e);
void read();
void write(ros::Duration elapsed_time);

protected:
ROBOTcpp::ROBOT ROBOT;
ros::NodeHandle nh_;
ros::Timer non_realtime_loop_;
ros::Duration control_period_;
ros::Duration elapsed_time_;
PositionJointInterface positionJointInterface;
PositionJointSoftLimitsInterface positionJointSoftLimitsInterface;
double loop_hz_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
double p_error_, v_error_, e_error_;
};

}

#endif

src/ROBOT_hardware_interface.cpp

  1. init(): Register joint-specific handles to each type of controller interfaces (joint state, position, effort, etc.)
  2. read(): Read joint positions from the robot’s hardware and set the joint_position_ array with that data.
  3. write(): Actuate the robot’s joints using the joint_effort_command_ variable. Based on our position controllers defined in config/controllers.yaml, this will be set by ros_control by using the desired joint position (joint_position_command_), the error between joint_position_ and joint_position_command_, and the PID parameter for that controller in config/controllers.yaml--all of which gets calculated when we call update().
  4. update(): simply read joint state, call update() on our controller manager, and write/actuate the joints based on what's calculated.
#include <sstream>
#include <ROBOT_hardware_interface/ROBOT_hardware_interface.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <ROBOTcpp/ROBOT.h>

using namespace hardware_interface;
using joint_limits_interface::JointLimits;
using joint_limits_interface::SoftJointLimits;
using joint_limits_interface::PositionJointSoftLimitsHandle;
using joint_limits_interface::PositionJointSoftLimitsInterface;

namespace ROBOT_hardware_interface
{
ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
init();
controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
nh_.param("/ROBOT/hardware_interface/loop_hz", loop_hz_, 0.1);
ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
non_realtime_loop_ = nh_.createTimer(update_freq, &TR1HardwareInterface::update, this);
}

ROBOTHardwareInterface::~ROBOTHardwareInterface() {

}

void ROBOTHardwareInterface::init() {
// Get joint names
nh_.getParam("/ROBOT/hardware_interface/joints", joint_names_);
num_joints_ = joint_names_.size();

// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
joint_effort_command_.resize(num_joints_);

// Initialize Controller
for (int i = 0; i < num_joints_; ++i) {
ROBOTcpp::Joint joint = ROBOT.getJoint(joint_names_[i]);

// Create joint state interface
JointStateHandle jointStateHandle(joint.name, &joint_position_[i], &joint_velocity_[i], &joint_effort_[i]);
joint_state_interface_.registerHandle(jointStateHandle);

// Create position joint interface
JointHandle jointPositionHandle(jointStateHandle, &joint_position_command_[i]);
JointLimits limits;
SoftJointLimits softLimits;
getJointLimits(joint.name, nh_, limits)
PositionJointSoftLimitsHandle jointLimitsHandle(jointPositionHandle, limits, softLimits);
positionJointSoftLimitsInterface.registerHandle(jointLimitsHandle);
position_joint_interface_.registerHandle(jointPositionHandle);

// Create effort joint interface
JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_[i]);
effort_joint_interface_.registerHandle(jointEffortHandle);
}

registerInterface(&joint_state_interface_);
registerInterface(&position_joint_interface_);
registerInterface(&effort_joint_interface_);
registerInterface(&positionJointSoftLimitsInterface);
}

void ROBOTHardwareInterface::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 ROBOTHardwareInterface::read() {
for (int i = 0; i < num_joints_; i++) {
joint_position_[i] = ROBOT.getJoint(joint_names_[i]).read();
}
}

void ROBOTHardwareInterface::write(ros::Duration elapsed_time) {
positionJointSoftLimitsInterface.enforceLimits(elapsed_time);
for (int i = 0; i < num_joints_; i++) {
ROBOT.getJoint(joint_names_[i]).actuate(joint_effort_command_[i]);
}
}
}

src/ROBOT_hardware_interface_node.cpp

#include <ROBOT_hardware_interface/ROBOT_hardware_interface.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "ROBOT_hardware_interface");
ros::CallbackQueue ros_queue;

ros::NodeHandle nh;
nh.setCallbackQueue(&ros_queue);
ROBOT_hardware_interface::ROBOTHardwareInterface rhi(nh);

ros::MultiThreadedSpinner spinner(0);
spinner.spin(&ros_queue);
return 0;
}

A word about ROBOTcpp

ROBOT.getJoint(joint_names_[i]).actuate(joint_effort_command_[i]);

What we’ve done in our implementation of ros_control on the TR1 is create a separate C++ library for simple hardware functionality called tr1cpp. This allows us to decouple the ros_control TR1_hardware_interface package from actual implementation of sending current to motors and reading bits of data from joint sensors.

Ultimately, something is sending a PWM signal to a motor driver that sends current to the motor. So, when you call write() and read() in your src/ROBOT_hardware_interface.cpp file, you need to connect the dots between the ros_control output/input and your actual hardware--something that synchronously executes the joint_effort_command_ variable on the hardware.

tr1cpp has an actuate() method on the joint class, so we just pass that value to that method. Of course, you can set this up anyway you like. This is just what we found to be a solution.

Conclusion

Best wishes!