Feature/gravity pd controller

[my-rice]

This PR introduces a new controller package, gravity_pd_controller, which implements a joint space PD controller with gravity compensation. The control law is described in detail in the package’s userdoc.rst.
The controller uses the inverse_dynamics_solver plugin interface to compute the gravity torque vector, g(q), and combines it with PD control terms.

IMPORTANT
The goal is to provide a robot-agnostic, model-based controller example within ros2_controllers, as discussed in ros2_controllers#1651. That PR was closed because no software modules were available that actually made use of the inverse_dynamics_solver.
Here, I am proposing a controller that explicitly depends on it. This raises the need for a strategy/decision on how to handle this dependency: since inverse_dynamics_solver is not yet part of ros2_controllers or ros2_control, the solver should be integrated before this controller.

Furthermore, since the inverse_dynamics_solver interface and its concrete implementations (e.g., kdl_inverse_dynamics_solver, etc.) are not yet part of ros2_controllers or ros2_control packages, we developed a demo repository that provides both, along with detailed usage instructions.

NOTES
The gravity_pd_controller uses the chaining technique to receive its input references. This means it relies on an upstream controller (e.g., joint_trajectory_controller) to provide the joint targets. With this approach, this package contains only the control logic, while reusing the existing reference generation logic (in the demo, provided by the joint_trajectory_controller). This avoids duplicating code and keeps the implementation more modular and maintainable.

[christophfroehlich]

Where do you think inverse_dynamics_solver should be published? It does not fit to ros2_controllers, either you include this in kinematics_interface plugins, or we look for a different place..?

[v8p1197]

Hi @christophfroehlich, I am the original proposer of #1651, main contributor of the inverse dynamics solver and a colleague of @my-rice.

As explained in the ROS discourse thread, the idea of the solver was to be the equivalent of kinematics-interface, but for dynamics components.

Our idea was to keep the solver outside the ros2_controllers repo as you already mentioned (just like kinematics-interface).
If you agree with the controller implementation in this PR, is there a chance to transfer the solver in a new repo in the ros-controls organization?
Alternatively, we can keep it in our lab's organization and maintain it.
We have currently released (under apt) the humble and jazzy versions.
Do you need a rolling version released?
We can take care of it in the next week(s).

Let us know :)

[christophfroehlich]

I haven't followed the discourse post (I was not tagged there :D), let me have a closer look. Yes we would need a release on rolling to go on this with PR, but if we decide to move it into ros-controls, then we will do that first..

[v8p1197]

@christophfroehlich thank you so much for your interest :)
We can release the rolling version ourselves, in the meantime you and others can check out the code and the demo to know how the modules are glued together ;)

[saikishor]

IMO kdl_inverse_dynamics_solver is pretty old. I think we should move on to more maintained libraries like Pinocchio or Drake etc. it's for the own good of this package.

Moreover, the API requirements of a dynamics solver is going to be huge, we would need to take a closer look before proceeding.

[v8p1197]

@saikishor thank you for your suggestion. We can implement a Pinocchio-based solver.

As mentioned here, I'll join the next WG meeting this Wednesday to discuss other aspects.

[saikishor]

@saikishor thank you for your suggestion. We can implement a Pinocchio-based solver.

As mentioned here, I'll join the next WG meeting this Wednesday to discuss other aspects.

Sure, we can discuss that part.