Make the update loop in ros_control to wait until controller "A" or "B" has obtained a goal

asked 2020-04-18 07:06:26 -0500

azerila gravatar image

updated 2020-04-18 07:11:16 -0500

I have written a hardware interface with ros_control for an already implemented controller. With the simulation that is connected to this interface, when I launch the controller manager and other nodes, the robot falls on the ground since my controller "A" (a gravitycompensated...) is either not enabled or has not yet received a goal because of which the command_efforts are near zero.

But the moment I proceed to a flexbe-state where controller "A" receives a goal, the correct joint effort commands become available and the robot gets up from the ground.

Is there a way that I make the update look in the hardware interface to wait until controller "A" has received a goal, so that it doesn't continue with the loop and send zero efforts to the robot? or any other way, perhaps with the controller manager and etc.

My controller interface in the cpp file looks like the following.

#include <muj_hw.h>
#include <iostream>
#include <string>
#include <fstream>

ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
    controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
    //ac("muj_simread_joint_state", true);

    ros::Duration update_freq = ros::Duration(1.0/loop_hz_);

    read_client  = nh_.serviceClient<amira_mujoco_py_sim::JointsState>("read_joint_state");
    write_client = nh_.serviceClient<amira_mujoco_py_sim::JointsState>("write_joint_state");    
    non_realtime_loop_ = nh_.createTimer(update_freq, &ROBOTHardwareInterface::update, this);


ROBOTHardwareInterface::~ROBOTHardwareInterface() {

void ROBOTHardwareInterface::init() {

    //ac(server_name, spinn);

    for (int i = 0; i < num_joints_; ++i) {
        // Create joint state interface
        hardware_interface::JointStateHandle    jointStateHandle (joint_names_    [i],
                                                                 &joint_position_ [i],
                                                                 &joint_velocity_ [i],
                                                                 &joint_effort_   [i]);
        // Create position joint interface
        hardware_interface::JointHandle    jointPositionHandle (jointStateHandle, &joint_position_command_[i]);
        hardware_interface::JointHandle    jointVelocityHandle (jointStateHandle, &joint_velocity_command_[i]);
        hardware_interface::JointHandle    jointEffortHandle   (jointStateHandle, &joint_effort_command_  [i]);


    // Registering all joints interfaces    

void ROBOTHardwareInterface::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    controller_manager_->update(ros::Time::now(), elapsed_time_);

void ROBOTHardwareInterface::read() {   
    bool success =;
    for (int i = 0; i < num_joints_; ++i) {
        joint_position_[i] = j_state_srv_read.response.j_state_res.position[i];
        joint_velocity_[i] = j_state_srv_read.response.j_state_res.velocity[i];
        joint_effort_  [i] = j_state_srv_read.response.j_state_res.effort  [i];


void ROBOTHardwareInterface::write(ros::Duration elapsed_time) {

    j_state_srv_write.request.j_state_req.effort  = {(float) joint_effort_command_[0],
                                                     (float) joint_effort_command_[1],
                                                     (float) joint_effort_command_[2],
                                                     (float) joint_effort_command_[3],
                                                     (float) joint_effort_command_[4],
                                                     (float) joint_effort_command_[5],
                                                     (float) joint_effort_command_[6]
    bool success =;


int main(int argc, char** argv) 
    ros::init(argc, argv, "mujoco_py_hardware_interface");
    ros::NodeHandle nh;
    ROBOTHardwareInterface ROBOT(nh);
    ros::AsyncSpinner spinner(4);
    return 0;
edit retag flag offensive close merge delete



Hi @azerila,

Have you considered the possibility of using a Service client to listen to the controller_manager node with controller_manager/list_controllers?

You can iterate though the list of controllers in the callback, search your controller name and check its state as "running". Just place this loop in a while true loop that only stops when the state is "running". With this set up you will block the execution until the controller you want is up and running.

Weasfas gravatar image Weasfas  ( 2020-04-20 08:55:24 -0500 )edit

Hi @Weasfas , The problem is even if one of the controller's are active, when it has not yet recieved any goal from a higher level interface, it does not generate torques that would compensate for gravity. If there is an easy way that I keep one controller always running which would only compensate for gravity and superpose its torques with other controllers, it would have also been good.

azerila gravatar image azerila  ( 2020-04-20 12:20:32 -0500 )edit

Mm, well it depends on how you fetch your goals in the interface. I mean you can have the low level control interface provided by ROS_control and implement the logic you want in a high level control node that provide a good effort value before all other controllers start working. Besides that, have you considered using position controllers instead of an effort one?

Weasfas gravatar image Weasfas  ( 2020-04-20 14:09:58 -0500 )edit