Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Best-practice for joint control access

Introduction

This tutorial provides a best-practice solution to access and control joints in the shadow hand through controllers/command topics. Meanwhile this will help you remove any use of the deprecated /sendupdate topics

We suppose that you have installed ROS and the shadow hand stack. Works in simulation and on the real hand.

Context

The same way as in the PR2, the Shadow etherCAT hand uses now host-side controllers sending torque demands to the low-level controllers inside the hand. For this reason, several different controllers can exist and can access the joints. Each controller has a different topic to send its command to.

For instance : Finger proximal joint FFJ3 could be controller by

• a velocity controller which awaits the command on sh_ffj3_velocity_controller/command
• a mixed controller which awaits the command on sh_ffj3_mixed_position_velocity_controller/command

The main question arises :

• How can my program know what topic to use to control a joint ?

Concept

The answer to the question is : controller_manager

The controller manager manages (loads, unloads, switch, start, stops) the controllers of a robot for each of its joints. Several controllers can be loaded at the same time for a same joint, but only one can be running at the same time.

The manager also offers a service (ListControllers) for your external software to know about the controllers and their status. This service is the main part of the solution proposed.

Indeed, by querying the running controllers, your software can directly know which topics it should publish to. But one thing is missing. The controller manager will not give you the association between a controller name and the joint it controls.

We will rely on the parameter server to query that association from the controller directly.

Solution

This part of code should be done in your initialization function to create the mapping

Cpp

   1 ros::NodeHandle nodehandler;
   2 std::map<std::string,std::string> jointControllerMap; 
   3 
   4 ros::service::waitForService("pr2_controller_manager/list_controllers");
   5 ros::ServiceClient controller_list_client = nodehandler.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");
   6 
   7 
   8 pr2_mechanism_msgs::ListControllers controller_list;
   9 std::string controlled_joint_name;
  10 
  11 controller_list_client.call(controller_list);
  12 
  13 for (unsigned int i=0;i<controller_list.response.controllers.size() ;i++ )
  14 {
  15   if(controller_list.response.state[i]=="running")
  16   {
  17     if (nodehandler.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name))
  18     {
  19       ROS_DEBUG("controller %d:%s controls joint %s\n",i,controller_list.response.controllers[i].c_str(),controlled_joint_name.c_str());
  20       jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ;
  21     }
  22    }
  23   }

Then, you can use your map to send data to the correct controller just by using the joint name

   1 std::string controller_name=jointControllerMap["FFJ3"];

Basically add /command to the controller name and voilà, you know the currently running controller's topic name for your joint.

Cpp code detailed

   1 ros::NodeHandle nodehandler;
   2 std::map<std::string,std::string> jointControllerMap; 

Prepare a mapping table between controller name and joint name

   1 ros::service::waitForService("pr2_controller_manager/list_controllers");
   2 ros::ServiceClient controller_list_client = nodehandler.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");

wait for controller manager service to be ready and prepare a service client to access the ListControllers service.

The next part will find the association between controller and joint names. It actually inits jointControllerMapping

   1 pr2_mechanism_msgs::ListControllers controller_list;
   2 std::string controlled_joint_name;
   3 
   4 controller_list_client.call(controller_list);

Query the controller for the controllers list

   1 for (unsigned int i=0;i<controller_list.response.controllers.size() ;i++ )
   2 {
   3   if(controller_list.response.state[i]=="running")
   4   {

For each controller name in the retrieved list, if this controller is running ...

   1     if (nodehandler.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name))
   2     {
   3       jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ;
   4     }
   5    }
   6   }

check the param server for the controlled joint name, if it exists, then add the controller name and the joint name to the mapping table.

Python

TO BE COMPLETED