[gazebo_ros_control] Plugin parses all transmissions in robot_description param

[carlosjoserg]

You might think this is ok at frist glance. However, in a multi-robot configuration where robots are chained (to differentiate from several mobile robots) and each robot with potentially different HWSim interfaces and transmisioins, this is not good.

Let me put as example my usecase. I have dual-arm robot with two hands as end-effectors. I have each component (1 arm, 1 hand) as a xacro model with a ros_control plugin, and, particularly, the hand uses a custom HWSim interface and transmission. So, to spawn the robot correctly in Gazebo, I need one single URDF. Thus, I have a xacro of the dual-arm robot including the models, i.e. twice per arm and hand. This implies that the plugin is loaded four times, that's fine, but it leads to the transmissions being parsed four times and being used in all HW interfaces. And the problem extends when different HWSim interfaces exist.

I'm using Indigo/Gazebo4, and couldn't find another way to do it. The way I solved it was by adding a robotNamespace tag in the transmission XML defintion, with an optional parsing, and an additional field in the TransmissionInfo struct. This allow to filter the transmissions by namespace before passing them to the HWSim interface. This can be done by adding to the parseTransmissionsFromURDF function something like

  std::vector<transmission_interface::TransmissionInfo>::iterator it = transmissions_.begin();
  for(; it != transmissions_.end(); ) 
  {
    if (robot_namespace_.compare(it->robot_namespace_) != 0)
    {
      ROS_WARN_STREAM_NAMED("gazebo_ros_control", "ignoring transmission "
        << it->name_
        << " because it is not in the plugin namespace.");
      it = transmissions_.erase(it);
    }
    else
    {
      ++it;
    }
  }

I believe these changes are beneficial given some posts in ROS and Gazebo answers, since you end-up with a single robot_description param that can be spawn in gazebo correctly and be used by MoveIt! for instance, and very important, one single ROS master. You can see the result in our setup. If you think this is useful I can write a PR, in here and and in the ros_control repository for the corresponding branches I'm working on.

[adolfo-rt]

To be sure I understand what your setup is, you have:

• One robot which is the aggregation of multiple components (arms, hands, head).
• Each component has its own RobotHW implementation, and a xacro file with its description.
• There is a single URDF that gets loaded into the ROS parameter server, that represents the entire robot, and contains the xacro expansions of its components
• This single URDF is used to spawn the Gazebo simulation, as well as to parse the transmissions (hence it getting parsed multiple times).

Let me know if this is correct.

[carlosjoserg]

That is correct.

To be more specific on the issue: The arm uses the DefaultSoftHandHWSim interface and SimpleTransmission, though the latter not being used by the interface, as known), and the hand a custom RobotHW and a custom transmission that it is used within the custom interface. Since the issue exists, e.g. using a single URDF makes that the custom hand transmission is passed to the DefaultSoftHandHWSim of the arm, and this is not good for resource handling by controllers. Several robot_description parameters would solve that. However, one looses the correct Gazebo spawn, since there is no way to spawn a chain by parts AFAIK. Adding the namespace to the transmission is a way to do it, but perhaps there is a more general/elegant solution.

[fmessmer]

I think this issue is related to something we solved in our cob_gazebo_ros_control plugin here

We added a joint_filtering feature to allow a plugin to be only responsible for a subset of joints of the whole robot! Checkout the README.md for more information.

In the end, you will be using a separate plugin for each of your components as is done e.g. for our Care-O-bot here

[carlosjoserg]

Yes, it is, and yes, I agree that the best practice is to have a plugin for each component to allow using them alone or in a multi-robot configuration, specially if you have different hardware interfaces.

I didn't look thoroughly at all your packages, but I see you use <filterJointsParam>joint_names</filterJointsParam>, where I presume joint_names is a parameter that is written and loaded using yaml by hand, means that you need to look at the URDF and see what the joint names will result after xacroing your model, right?

Our solution to this issue for the plugin and the transmission is minimal, promotes the use of the transmission_interface package and it does not require an extra file/parameter loading, since the names are parsed from the transmission definition, and the namespace for transmissions can be propagated with arguments readily, e.g. as usual, the name of the robot, see this example for instance. For the real hardware interface we do use the joint names file as you, but I haven't though about it until now, I believe this change could be useful in that case as well.

In any case, the issue refers to the fact that each RobotHW, real or simulated, should receive the transmissions/joints related to that hardware.

[adolfo-rt]

So you have multiple, induvidual RobotHWs, all of which parse the same URDF.

I understand the need of using a single, unified URDF, as you often have multiple instances of the same component (hand or arm) with different resource names (l_elbow vs r_elbow). The final name of a resource is not known by inspecting the xacro file alone, but needs to be instantiated with specific parameters to make sense. We're OK here.

I find the transmission namespacing feature somewhat of an odd fit. We're trying to solve in the transmission spec an issue of robot composability. I get the feeling of not properly separated concerns.

Idea

I think that the information we want should be already there, and we can get the desired behavior by adapting the code, and not the user-facing configuration / specification.

Please note that correct transmission parsing has not been implemented in gazebo_ros_control. Multiple hardware interfaces and different transmission types are unsupported. This has been implemented only for hardware (non-simulated) setups but has not been ported to gazebo_ros_control. The following comments do not currently apply to the Gazebo backend, but to setups leveraging the TransmissionInterfaceLoader class.

Currently, when we load transmissions from a URDF, we assume that a single RobotHW instance will provide all required resources, and we error out if this is not the case. We could extend the TransmissionLoderInterface API to support loading only transmissions involving resources in the RobotHW passed on construction, for example, and ignore all others. This would be a pretty modest effort.

You might be thinking, yeah, but this does not currently apply to Gazebo. While it is true, what it means is that the current shortcoming is on the implementation, and not the specification. All the information required to do what you are requesting should already be there, or at least I think so. It's just a matter of using it correctly. The main challenge I see here is that we should unify how transmissions are parsed in simulated setups.

[carlosjoserg]

You might be thinking, yeah

Yes, you are right. I was struggling our packages dependencies to the released ROS packages (as I usually do), but I couldn't make it work in several setups. Then, I read this line, and that's when the yeah came out. Maybe the title of the issue is not clear enough. I'm still parsing that additional tag of the transmission spec I'm proposing on the transmission_interface package, see here, only to fill the additional field in the TransmissionInfo struc, which then is used by the plugin. But neverming that, and move on with what follows.

We could extend the TransmissionLoderInterface API to support loading only transmissions involving resources in the RobotHW passed on construction, for example, and ignore all others. This would be a pretty modest effort.

I would expect that as the default behavior, but I understand that's how open source software must evolve.

As far as I have seen around, typical RobotHW (real) implementatios require a yaml file defining which resources the interface handles, like above, the joint_names parameter in @ipa-fxm's solution. I believe this might be due to none is using the tranmission_interface package within the `RobotHW implementation, which contains already that specification. So, I think you are right, perhaps It's just a matter of using it correctly. In the simulated scenario nothing should be different from this, I believe. I will give it a second try, thinking of that API extension, and come back with an answer, hopefully closing the issue.

Thanks for the hints.

[adolfo-rt]

As far as I have seen around, typical RobotHW (real) implementatios require a yaml file defining which resources the interface handles...

That's really up to the RobotHW implementation. If you don't actually have to deal with transmissions on software, you might skip them altogether, but then the default Gazebo plugin would not work. This is why some people just put simple reducers with ratio 1.

We do use transmissions when working with hardware. And for simulation, we currently don't use them at all. We query the Gazebo model directly for joints (we have a custom backend). If the Gazebo backend supported transmissions better, we'd use it.

[fmessmer]

The reason why we use the joint_filtering is not so much about using a particular transmission, but for our hardware we need a separate controller_manager for each component (because we have to use different CAN-Buses for each component)! Still all components are modeled in just one URDF which is loaded to robot_description parameter. Thus, the challenge is how to map/assign the joints of a component to its corresponding controller_manager/plugin. For now, we achieved this by using a joint_names parameter (we are using the same parameter for other things, too....) How would that be done using the transmission interface?

[mathias-luedtke]

PS: The different CAN buses are not the main issue, but they are connected to different ROS machines..

[carlosjoserg]

...for our hardware we need a separate controller_manager for each component ...components are modeled in just one URDF which is loaded to robot_description parameter.

Same here, ethernet, USB, etc. each component with its own controller_manager properly namespaced.

but they are connected to different ROS machines

You mean with different master's or ROS-networked with a single master?

How would that be done using the transmission interface?

This is our setup. We are using it in both real and simulation.

In simulation, we are already using the transmissions to keep the joints corresponding to that particular HW interface, but I'm using the additional namespace tag I mentioned above (which I agree is kind of odd, and I'm trying to figure how to do it without that). And I don't see how it can be different in the real scenario.

We are running one single master. We have to use 2 PCs because of the KUKA LWR data exchange rate requirements, so we use 1 PC for drivers (2 arms and 2 hands) and the other for high- and low-level planning.

I'm just trying to find the best-practice way, so others can benefit, that's why I'm asking before trying, and the boilerplate template from ros-controls/ros_control#198 is an excellent idea for this.

[mathias-luedtke]

You mean with different master's or ROS-networked with a single master?

Distributed ROS system (1 to 6 PCs) with single ROS master.

Every component has its namespace with separated services, parameters, controllers and manager. Same for real hardware and simulation (WIP). Hardware drivers spawn their controller_manager instances, in simulation we use multiple instances of https://github.com/ipa320/cob_gazebo_plugins/tree/indigo_dev/cob_gazebo_ros_control

[fmessmer]

An updated version of the hwi_switch_gazebo_ros_control plugin that uses the hwi_switch API proposed here can be found here.

In case there is an interest for having this extension of the default gazebo_ros_control plugin back within gazebo_ros_pkgs (as a separate plugin beside the default plugin), I'd be happy to provide our plugin to the greater gazebo_ros_pkgs community by sending a PR.

Comments and suggestions are very welcome!

[carlosjoserg]

Hi, I finally made up my mind on this issue, maybe not a generic solution, but I really like it. So, in case you find it useful, I give you a short-long description of it:

I have an abstract RobotHW-derived class of my robot (arm, hand, anything) that goes together with an URDF model of it containing transmissions, plugin specs, and a relevant joint naming convention (is the RobotHW of that particular robot anyway).

Any object of this derived class is created using an URDF string (it can be any robot description containing my robot) and the robot name. No dependencies on anything external but urdf and ros-controls (and sometimes any other useful library like KDL) up to here.

Then, a key part, I do the opposite to what's done in the plugin: I first parse the joints that belong to my robot from the URDF string using the name and the joint naming convention, and then load their corresponding transmissions, and ignore all others in the robot description.

The abstraction is enforced with three pure virtual functions namely init(), read() and write() that are implemented in derived classes depending on the actual interface: Gazebo, V-rep, different real robot interfaces, etc.

Therefore, I can't use the gazebo_ros_control class loader plugin structure, because the base class name these functions initSim(), readSim() and writeSim() and the signature of initSim() is very large, which in my case I use it to match the joints from the URDF to that of the actual interface only. So it'd be nice to have these three functions general to any interface, i.e. I think the Sim part is not needed.

In any case, thanks for your ideas @adolfo-rt, @ipa-fxm and @ipa-mdl

Closing...