Documentation for model-based controllers

doc/writing_new_controller.rst

@@ -139,9 +139,11 @@ That's it! Enjoy writing great controllers!
 
 
 Useful External References
----------------------------
+--------------------------
 
 - `Templates and scripts for generating controllers shell <https://rtw.b-robotized.com/master/use-cases/ros2_control/setup_controller.html>`_
 
 
   .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards.
+
+- :ref:`Writing a new model-based controller <effort_controllers_userdoc>`: when writing model-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin <https://index.ros.org/r/inverse_dynamics_solver/github-unisa-acg-inverse-dynamics-solver/>`_.

effort_controllers/doc/userdoc.rst

@@ -10,7 +10,7 @@ This is a collection of controllers that work using the "effort" joint command i
 The package contains the following controllers:
 
 effort_controllers/JointGroupEffortController
--------------------------------------------------
+---------------------------------------------
 
 This is specialization of the :ref:`forward_command_controller <forward_command_controller_userdoc>` that works using the "effort" joint interface.
 
@@ -19,14 +19,14 @@ ROS 2 interface of the controller
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 Topics
-,,,,,,,,,,,,,,,,,,
+,,,,,,
 
 ~/commands (input topic) [std_msgs::msg::Float64MultiArray]
   Joints' effort commands
 
 
 Parameters
-,,,,,,,,,,,,,,,,,,
+,,,,,,,,,,
 This controller overrides the interface parameter from :ref:`forward_command_controller <forward_command_controller_userdoc>`, and the
 ``joints`` parameter is the only one that is required.
 
@@ -45,3 +45,52 @@ An example parameter file is given here
     ros__parameters:
       joints:
         - slider_to_cart
+
+Model-based controllers
+--------------------------
+
+Model-based controllers' control laws often require computing the manipulator's dynamics terms, e.g. inertia, coriolis, friction, and gravity contributions.
+Notable examples are `gravity-compensation PD controllers or inverse dynamics controllers <https://doi.org/10.1007/978-1-84628-642-1>`_.
+
+Control laws
+^^^^^^^^^^^^
+
+For instance, the PD Control with Gravity Compensation law is the following
+
+.. math::
+
+  \boldsymbol\tau = \boldsymbol K_p \tilde{\boldsymbol q} - \boldsymbol K_d \dot{\boldsymbol q} + \boldsymbol g(\boldsymbol q)
+
+and can be implemented in C++ as
+
+.. code-block:: cpp
+
+  torque_output = K_p * position_error - K_d * velocity + g;
+
+Similarly, the inverse dynamics control law is the following
+
+.. math::
+
+  \boldsymbol\tau = \boldsymbol B(\boldsymbol q) \left( \boldsymbol K_p \tilde{\boldsymbol q} + \boldsymbol K_d \dot{\tilde{\boldsymbol q}} + \ddot{\boldsymbol q}_d \right) + \boldsymbol C(\boldsymbol q, \dot{\boldsymbol q}) \dot{\boldsymbol q} + \boldsymbol f(\dot{\boldsymbol q}) + \boldsymbol g(\boldsymbol q)
+
+and can be implemented in C++ as
+
+.. code-block:: cpp
+
+  torque_output = B * (K_p * position_error + K_d * velocity_error + desired_acceleration) + c + f + g;
+
+The role of the inverse dynamics solver
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+To facilitate the implementation of these controllers, the robot-agnostic `inverse dynamics solver <https://index.ros.org/r/inverse_dynamics_solver/github-unisa-acg-inverse-dynamics-solver/>`_ plugin can retrieve the dynamics terms with
+
+.. code-block:: cpp
+
+  Eigen::MatrixXd B = inverse_dynamics_solver_->getInertiaMatrix(position);
+  Eigen::VectorXd c = inverse_dynamics_solver_->getCoriolisVector(position, velocity);
+  Eigen::VectorXd f = inverse_dynamics_solver_->getFrictionVector(velocity);
+  Eigen::VectorXd g = inverse_dynamics_solver_->getGravityVector(position);
+
+where ``position``, ``velocity``, ``desired_acceleration``, ``position_error`` and ``velocity_error`` are given by the current robot state and reference, ``K_p`` and ``K_d`` are control gains, and ``torque_output`` shall be written on the command interfaces.
+
+For more information about the solver, please have a look at `this example <https://github.com/unisa-acg/inverse-dynamics-solver/tree/humble/kdl_inverse_dynamics_solver#configuration>`_ with KDL for simulated robots.