expose complex data types through interfaces

[Karsten1987]

This PR is a first set of changes towards a state/command interface which allows complex data types.
In order to preserve a stable behavior across all controllers, all calls to get_value or the equivalent set_value are defaulting to double.
However, that introduces the following undefined behavior:

Assume we have a complex data type but try to access the interface through the standard getters

  // interface.get_value() is now undefined behavior.
  // EXPECT_DOUBLE_EQ(interface.get_value(), 0.0);

In a follow up PR, I propose to introduce a deprecation of the default template parameter and make it explicit for each controller to call get_value with a specific template argument. That way there is at least no ambiguity and the type safety concern is made explicit by forcing the caller to specify the template.
Additionally, I believe it makes sense to introduce an identifier in the ros2 control tags to expose the type of interface. I currently don't have a good way of having a type-safe yet type-erased way of getting around the void *.

[bmagyar]

This goes slightly against what we discussed at the relevant breakout session with allowing entirely custom types. I think that's something we should explicitly disallow.

At the implementation level I think this is a technically good solution but we need to add the right control for what we want to allow.

[bmagyar]

I'd really like to avoid this exact use case. If we start supporting random types, hardware manufacturers will be inclined to start baking their own custom structs into their system components which will force users to pair their controllers with that specific hardware. The type erasure technique is nice but I think the framework should have tight control over what exactly is supported and what isn't.

Parsing "custom_complex_pod" as interface type should fail upfront IMO and

 template<class DataT = double>
  DataT get_value() const
  {
    return ReadWriteHandle::get_value<DataT>();
  }

should only be allowed if DataT is in an approved set. Probably at the C++ level this means copying that code 4-5 times but that doesn't sound too bad.

[destogl]

In the breakout session a few weeks ago, we agree that the following types are the most interesting to have:

• bool
• int
• double
• vector<bool>
• vector<int>
• vector<double>

On the contrary to this, PR enables to use of anything as a data type. Would it be feasible/sensible to limit allowed values to those specific types? (maybe with explicit implementations of get_value and set_value?)

The purpose of this limiting is to force users to write explicit code and, by that, make debugging easier. Otherwise, using structures one can put in there anything, and there are not transparent anymore. At least not for the CLI tools, which are a significant improvement in debugging process that enables to "see inside" the framework.

Also, I am concerned about transparency of the void pointer. My main concern is that one can easily mix double and int values (both 4 bytes) without noticing something wrong during the compilation or runtime, exchanging false values between hardware and controllers.

To address maybe, it would be acceptable to have the std::string type field in the Handles provided in the constructor and in the constructor of "Loaned*Interface" to cross-check the types. These checks would be done during non-realtime execution of the code, so they should not
disturb the execution.

... Just a few ideas from looking at this implementation ...

[destogl]

I don't think we should allow to create such structures in the framework. I see potentially very-many problems is users can put anything as the interface values.

We could end up with zillion different structures for each industrial-manipulator realizing actually simple "position-velocity-acceleration" combination of interfaces.

This kind of complex structures (without strings) should be realized by using Semantic Components. This was we make them very visible and explicit.

[destogl]

We should probably limit this method to specific types (as described in overview). This means writing each one of those (it should be only +30 lines of code)

[destogl]

In the constructor of "Loaned*Interfaces" we should also provide a type as a string and cross-check it with the Handle type. That way we are sure there is no access violation (double - int example)

[destogl]

Why not also add "type" string as a member of handles so we can check when creating "Loaned*Interfaces" that the data types are not mixed. See also comment below.

[Karsten1987]

These are all valid comments and I can second them. However, the motivation here was slightly inverse.

I know we have a strict separation between the hardware interfaces and the controllers, and we'd like to keep that boundary alive as much as possible. This works well if we have full control over the hardware implementation.
However, in cases where things are not as trivial or we have to work with non-ROS compliant hardware (off the shelf robots, complex actuators and sensors), it might become cumbersome to fit a non-trivial datatype from the hardware into a set of vectors. Needless to say it provokes an extra copy in the implementation.
The Franka robot exposes roughly 50 various variables in its "robot state" and it begs the need to simply expose that data structure. One of them being a dynamics model and jacobian - fitting this into a vector is just not really feasible.
I see the idea of having semantic components tackling part of it, however the way I see this is that people come up with robot-specific semantic components, i.e. a semantic component for the Franka robot. That way, the controllers are still not robot agnostic.

[destogl]

@Karsten1987 I understand your points and they are legit. Should we then at least try to "lead" people into using standard "pos-viel-acc" interfaces for JTC and similar.

A possibility with semantic components could be a "standardization" similar to "SimpleRobotMessage" (or similar) in ROS1 proposed by ROS industrial. In that way the users would need to implement custom interface for their robot to be supported by standard controllers. I think we should rather tend to "standardization" of interfaces then supporting any random combination.

Could you show concrete example hoch data are relevant for you and where to use those?

[bmagyar]

Indeed it is a valid point but I'm still quite uneasy about it.

Why should we aim to support non-ROS-compliant robots?
Where does this stop? Should we also implement optional features supporting robot-specific hacky behaviours of non-compliant robots into controller_manager to support how RobotX's lifecycle works? And then the same for RobotY?

How can you devise robot state from custom interfaces? Or that's an exception that everyone will make the effort to properly implement? Why not the command interfaces then, it's typically not too much more.

I would not regard implementation/optimization details as valid arguments in this discussion, those are up to one's choice and abilities of implementing a non-ROS-compliant robot ROS driver.

Without something forcing standardization, people will be lazy and prefer non-standard drivers with non-standard controllers on top. Bad examples stick around... just look at how many people still use the C interface of OpenCV even though their application is in C++.
I understand that many chose to fork or imitate ros_control in ROS1 for this reason but I think we should think how to further lower the barrier of entry to support the standard interfaces so we can stay away from custom ones.

[marcbone]

I am currently working on the ROS2 integration for the Franka robots.

As @Karsten1987 already mentioned we have lots of stuff that we want to be able to use inside the controller (mass matrix, coriolis torques, end-effector pose, jacobians, ...).

It would be really tedious to put everything as a vector of doubles. Further, every controller the user is using would have to "deserialize" everything, which would break the compatibility with other robots.

From our point of view the ideal solution would be to allow any kind to data to be transmitted over the hardware interface. Then in a further step a "Generic7DofRobotState" could be introduced to allow the creation of more generic controllers.

[destogl]

As @Karsten1987 already mentioned we have lots of stuff that we want to be able to use inside the controller (mass matrix, coriolis torques, end-effector pose, jacobians, ...).

Can you share your -tag with definition of all interfaces you need? (Hint: if you use GPIO-tag things get much shorter).

It would be really tedious to put everything as a vector of doubles. Further, every controller the user is using would have to "deserialize" everything, which would break the compatibility with other robots.

If done properly, this is actually not needed. The idea is to have SemanticComponents that support this level of abstraction. The controller side examples are available for FTS and IMU sensors. I added a few notes in the roadmap repository to handle this on the hardware side.

From our point of view the ideal solution would be to allow any kind to data to be transmitted over the hardware interface.

For things you mentioned, it seems that "vector" would do the job. Is that right? Do you have any other example/need?

Then in a further step a "Generic7DofRobotState" could be introduced to allow the creation of more generic controllers.

What are you actually imagining with this? Can you please explain a bit more?

[marcbone]

I dont quite get what you mean with -tag definitions. At each time step we want to provide the joint positions, velocities and the measured torques. We can do that with the currently available interfaces. Further, we want to provide things like end-effector position and the current space and body jacobians. They all can be represented as double arrays. However, the representation of a jacobian matrix is not unambiguous. Therefore "serializing" them as double array has no benefit compared to sending the data directly as a meaningful type. It just adds complexity.

We would prefer if a controller written by a researcher does not have to convert lists of doubles into meaningful types for every controller he writes.

The idea with something like a Generic7DofRobotState was to define a common standard, for example to write a generic cartesian impedance controller. To implement one you basically need from the robot:

• current joint positions
• current joint velocities
• current joint torques
• current end-effector pose
• end-effector jacobian
• mass matrix
• corilois torques
• gravity torques

If this (+ maybe a few more for other use-cases) would be standardized in a Generic7DofRobotState struct, which would then be send from the hardware interface to the controller, the controller could work with any 7 DoF robot that provides this state interface. The state could have user-friendly data types like Eigen3::Matrix for the matrices, so it is easier to work with them.

Each robot vendor could implement their own specialized hardware interface to expose all functionality they offer and additionally implement a hardware interface with the generic state to allow using generic controllers.

[smihael]

I would like to give some thoughts as an end-user with some background with developing controllers based on custom ROS1 hardware interfaces provided by franka_ros (model, state.
Lack of support for anything than double data types in ROS2 blocks transition of any projects relying on these interfaces from ROS1 to ROS2.

While I agree with @bmagyar that allowing custom structs conflicts with generality and robot agnosticism, I also agree with @Karsten1987 and @marcbone that vector is not the user-friendliest format and that copying the same values is not exactly efficient. It seems that it will be hard to find consensus about being "as generic as possible" (with doubles as simple interfaces) vs. "as powerful as possible" (with any data type you’d want).

It might help to try to look at this from the perspective of what kind of controllers do we want to be able to implement. Calculations that are exposed by franka::Model and franka::State make it easy to implement controllers using torque interface such as joint-space and Cartesian-space impedance controllers with variable stiffness in arbitrary direction. So maybe, a general model interface with mass, gravity, Coriolis, and Jacobians should be available? The same for end-effector pose and velocity.

Then it would be still up to the manufacturers how they want to expose this to the interface. It is worth noting, that it makes little sense to calculate these results in the control loop (update() in ROS1's ros_control) if the results are already calculated in the robot internal software. So one option could be by using robot's API (e.g. libfranka is used in franka_hw). It would be also nice to ship some general solution based on one/combination of kinematics and dynamics libraries like KDL/RBDL/pinocchio/TSID/Trac-IK as part of the ros2_control framework for the robots that don't expose these calculations.

I actually always missed this feature on other comparable robots or when trying to simulate Franka's Panda in Gazebo. In fact I ended up implementing an interface which mimics Franka's to keep these features separate from the actual control code. Similar but more recent and finished approach for Franka Emika panda can be seen here.

It would be pity for ros2_control to stay too restrictive. Even without these limitations, it was often discussed whether it is worth to develop custom controllers using this framework or just stick with robot's manufacturer SDK, especially in research where you usually just want to demonstrate new algorithm in practice with the hardware you have in the lab.

[olivier-stasse]

Dear @smihael thanks for mentionning pinocchio and TSID.
FYI they are some on-going discussions to fix some technical problems preventing the release of pinocchio for foxy. This is a necessary condition to have TSID released too in foxy. For the latter, I discussed with some academic partners and we would be happy to work on an open version of TSID in a ros2_controller. The input would be the current state of the robot, the current instaneous desired feature values, and the output a control vector possibly torque, velocity, position. Note that the current state of the robot is often the result of an estimator. It sounds reasonnable to not include this estimator in the same ros2_controller.

[destogl]

Hi @smihael,

Thanks for your comment, especially all the details and real-world examples. Please check my answers/comments below.

It might help to try to look at this from the perspective of what kind of controllers do we want to be able to implement. Calculations that are exposed by franka::Model and franka::State make it easy to implement controllers using torque interface such as joint-space and Cartesian-space impedance controllers with variable stiffness in arbitrary direction. So maybe, a general model interface with mass, gravity, Coriolis, and Jacobians should be available? The same for end-effector pose and velocity.

Looking at those structures, it seems that using arrays/vectors we can achieve a lot already, and this is fairly easy to implement. TBH I don't see much need for custom structure in the examples you gave. For me, this "smells" more like: support for arrays + some semantic components (check here for clarification). This could bring us very, very far already. So, I suggest trying to do this (as planned).

It would be also nice to ship some general solution based on one/combination of kinematics and dynamics libraries like KDL/RBDL/pinocchio/TSID/Trac-IK as part of the ros2_control framework for the robots that don't expose these calculations.

Yes, this would be great, and we are actually thinking about that since it is needed for the admittance controller we have in the pipeline.
Any help on this is highly appreciated! We can not merge things for that PRs don't exist! :D

I actually always missed this feature on other comparable robots or when trying to simulate Franka's Panda in Gazebo. In fact I ended up implementing an interface which mimics Franka's to keep these features separate from the actual control code. Similar but more recent and finished approach for Franka Emika panda can be seen here.

Is there a possibility to make this more generic and add into gazebo_ros2_control or ign_ros2_control?

It would be pity for ros2_control to stay too restrictive. Even without these limitations, it was often discussed whether it is worth to develop custom controllers using this framework or just stick with robot's manufacturer SDK, especially in research where you usually just want to demonstrate new algorithm in practice with the hardware you have in the lab.

ATTENTION: this is a highly opinionated answer and does not reflect any “official” opinion of the maintainers

TBH I don't care much about those discussions. The framework doesn't target groups/people/users that “just want to demonstrate a new algorithm in practice”. We are more interested into general and well done solutions. That's why we are so slow and picky with merging the new features. I worked for seven years in research where many people had the same discussions and mainly used SDK's directly. For their career that was fine because they threw everything after a year or two. For me, on the other hand, I put effort to overcome the restrictions in ros_control which are now resulting in many code-reuse for actual industrial applications. Both ways are valid, the only questions are what are each person's goals…

[mergify]

This pull request is in conflict. Could you fix it @Karsten1987?

[smihael]

I actually always missed this feature on other comparable robots or when trying to simulate Franka's Panda in Gazebo. In fact I ended up implementing an interface which mimics Franka's to keep these features separate from the actual control code. Similar but more recent and finished approach for Franka Emika panda can be seen here.

Is there a possibility to make this more generic and add into gazebo_ros2_control or ign_ros2_control?

The code could be modified fairly easy. Unfortunately, I can't promise on doing that in the near future.

[tylerjw]

I would strongly discourage the use of any types that use dynamic allocation like vectors. It would be much better to use std::array with template arguments. I would also strongly discourage any sort of flag values like nullptr or NaN that have to be checked and instead use a Maybe type like std::optional that the type system can make sure you are validating. The year is 2022, we don't have to unsafe code that relies on pointers to globals like c programmers from the 90s, we can use the strong type system in c++ to make these interfaces much nicer to the user who is implementing against them.

[mergify]

This pull request is in conflict. Could you fix it @Karsten1987?

[mergify]

This pull request is in conflict. Could you fix it @Karsten1987?