Dynamics controllers for Computed Torque Control / Gravity Compensation

[francofusco]

Hello! During the last year I worked on some Computed Torque Controllers and I finally thought that this might be a nice addition to ROS control. In this PR, I added a new package, dynamics_controllers whose aim is to take into account the inverse dynamics model.

Two controllers are added: one works on serial chains, while the other on whole trees. KDL is used to evaluate the efforts via the Recursive Newton-Euler algorithm. What I think is really interesting is that these controllers work only at a "high-level". In practice, they allow to instanciate a "sub-controller" (operating on an EffortJointInterface) to obtain an acceleration command - technically speaking, the produced command should be an effort, but is interpreted in a different way inside the package.

One important remark: to compute the dynamics of kinematic trees it is necessary to use the latest version of KDL.

Let me know what you think about it and what should be improved!

[bmagyar]

First of all, we need to either get someone to update the ROS Melodic upstream KDL (tricky) or we ship a custom KDL internally with this package (not ideal).
How did you deploy this to robots locally?

[francofusco]

In my case I have a local copy of KDL - I wrote the solver for trees so it was convenient for me to have it locally.

As far as I can tell, the only "extra" files not already in Melodic's KDL are those of the tree solver (it's just one header + the associated cpp). So as a quick and easy solution I could simply copy these files locally until the upstream is updated.

[gavanderhoorn]

So as a quick and easy solution I could simply copy these files locally until the upstream is updated.

If this approach is chosen, I would recommend to either only include them in the compilation units that need them (ie: only include in the .cpp) or appropriately namespace them, to avoid polluting the include path of dependants.

[francofusco]

I manually added the sources for the Tree solver: they are all inside the src directory, so that they cannot be included in another package by mistake. In principle, once the solver becomes available, it will be necessary to simply delete the files and edit CMakeLists.txt to have everything working.

[bmagyar]

Couldn't this be detected via parsing the KDL tree?

[francofusco]

If you mean that it is necessary to make sure that all joints are handled by the sub-controller, then yes, it can be checked from the parsed tree and is addressed in the code here. My comment in the readme is meant to warn users that it is not possible to run dynamics controllers if the sub-controller evaluates the efforts just for a subset of the joints.

[bmagyar]

Could you please add some pointers on how one would go about this?

[francofusco]

That is supposed to be a warning about the fact that model-based computed torque controls require a sufficiently good identification of the parameters. Do you want me to add references about possible identification strategies?

[bmagyar]

I left a few quick points from my phone, excuse my brevity.

[francofusco]

Thanks for all the feedback, I will try to address these points in few more commits!

[francofusco]

In the last two commits I added support for multiple sub-controllers (so that it is not strictly necessary to use a JointGroup-like controller) and also made sure that efforts are saturated according to URDF's limits.

I was considering to handle safety limits, however as far as I understand they should be enforced at the very end of the control flow directly by the RobotHW, isn't it?

[carlosjoserg]

My two-cents from a long-time-user point of view.... and facing new-comers having issues with naming conventions in ros-controls.....

Since the proposed controllers send torque to the hardware_interface::EffortJointInterface, their namespace should be effort_controllers instead of dynamic_controllers, for consistency with the rest of controllers here.

For example, JointTrajectoryController can be used as effort_controllers/JointTrajectoryControllers which already gives the idea the the output is effort, and a PID is in the middle, if you are already familiar with the naming.

To me effort_controllers/GravityCompensation would make more sense.

---

Great work btw! It remided me of an very old work here ... https://github.com/CentroEPiaggio/kuka-lwr/tree/master/lwr_controllers

[francofusco]

their namespace should be effort_controllers instead of dynamic_controllers

I see your point, and for me it would not be a big deal moving from dynamics_controllers to effort_controllers. However, I believe that I should then update also the name of the controllers themselves, .eg., from dynamics_controllers::KdlChainController to something like effort_controllers::KdlDynamicsChainController, since KdlChainController would not give much information otherwise. What do you think? KdlDynamicsChainController seems a little ugly perhaps, if you have a better suggestion it will be more than welcome! 😄

Great work btw!

Thanks! 🎉