From 30af1b3795bfd05692b5bee39193cf02e27ee08a Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Wed, 16 Apr 2025 13:47:09 +0200 Subject: [PATCH 1/7] Update documentation to include dynamics-based controllers --- doc/writing_new_controller.rst | 7 +++- effort_controllers/doc/userdoc.rst | 55 ++++++++++++++++++++++++++++-- 2 files changed, 58 insertions(+), 4 deletions(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0874448461..2c8d4f08b2 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -139,9 +139,14 @@ That's it! Enjoy writing great controllers! Useful External References ---------------------------- +-------------------------- - `Templates and scripts for generating controllers shell `_ .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. + +- Writing a new dynamics-based controller + + When writing dynamics-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin `_. + For more information, please refer to the :ref:`effort-based controllers ` documentation. diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 62e98a75f9..4e483a570d 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/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 ` 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 `, 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 + +Dynamics-based controllers +-------------------------- + +Dynamics-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 `_. + +Control laws +^^^^^^^^^^^^ + +For instance, the PD Control with Gravity Compensation law is the following + +.. math:: + + \tau = K_p \tilde q + K_d \dot{\tilde q} + g(q) + +and can be implemented in C++ as + +.. code-block:: cpp + + torque_output = K_p * position_error + K_d * velocity_error + g; + +Similarly, the inverse dynamics control law is the following + +.. math:: + + \tau = B (K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d) + C(q, \dot q) \dot q + f(\dot q) + g(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 `_ 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 `_ with KDL for simulated robots. From 2fdae87842a68549778bfeab01fff8d5e7523e5b Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Wed, 16 Apr 2025 14:31:03 +0200 Subject: [PATCH 2/7] Avoid blank lines in bullet list --- doc/writing_new_controller.rst | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 2c8d4f08b2..22eb9a2b0e 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -146,7 +146,4 @@ Useful External References .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. -- Writing a new dynamics-based controller - - When writing dynamics-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin `_. - For more information, please refer to the :ref:`effort-based controllers ` documentation. +- :ref:`Writing a new dynamics-based controller `: when writing dynamics-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin `_. From ed4269b3462d238d4a51b1a6c491eaff515c5dca Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Wed, 16 Apr 2025 14:35:27 +0200 Subject: [PATCH 3/7] Add dependency on joint positions in inertia matrix --- effort_controllers/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 4e483a570d..4031006930 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -71,7 +71,7 @@ Similarly, the inverse dynamics control law is the following .. math:: - \tau = B (K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d) + C(q, \dot q) \dot q + f(\dot q) + g(q) + \tau = B(q) (K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d) + C(q, \dot q) \dot q + f(\dot q) + g(q) and can be implemented in C++ as From afba1da43a6c28156f544ba3db74c7341dbddac5 Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Wed, 16 Apr 2025 14:39:35 +0200 Subject: [PATCH 4/7] Update latex control law --- effort_controllers/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 4031006930..2380967512 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -71,7 +71,7 @@ Similarly, the inverse dynamics control law is the following .. math:: - \tau = B(q) (K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d) + C(q, \dot q) \dot q + f(\dot q) + g(q) + \tau = B(q) \left( K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d \right) + C(q, \dot q) \dot q + f(\dot q) + g(q) and can be implemented in C++ as From ff7dbe66650b3a3b8ae8b618bc2ce1a912f51356 Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Wed, 16 Apr 2025 14:39:49 +0200 Subject: [PATCH 5/7] Specify the inverse dynamics solver is a plugin --- effort_controllers/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 2380967512..fdaea7f8f8 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -82,7 +82,7 @@ and can be implemented in C++ as The role of the inverse dynamics solver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -To facilitate the implementation of these controllers, the robot-agnostic `inverse dynamics solver `_ can retrieve the dynamics terms with +To facilitate the implementation of these controllers, the robot-agnostic `inverse dynamics solver `_ plugin can retrieve the dynamics terms with .. code-block:: cpp From 2c4407c42b20e2c16987fb05868097d87042fc54 Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Thu, 17 Apr 2025 15:20:39 +0200 Subject: [PATCH 6/7] Use bold format to express arrays, and correct PD+g control law --- effort_controllers/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index fdaea7f8f8..7afc0f085d 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -59,19 +59,19 @@ For instance, the PD Control with Gravity Compensation law is the following .. math:: - \tau = K_p \tilde q + K_d \dot{\tilde q} + g(q) + \mathbf\tau = \mathbf K_p \tilde{\mathbf q} - \mathbf K_d \dot{\mathbf q} + \mathbf g(\mathbf q) and can be implemented in C++ as .. code-block:: cpp - torque_output = K_p * position_error + K_d * velocity_error + g; + torque_output = K_p * position_error - K_d * velocity + g; Similarly, the inverse dynamics control law is the following .. math:: - \tau = B(q) \left( K_p \tilde q + K_d \dot{\tilde q} + \ddot q_d \right) + C(q, \dot q) \dot q + f(\dot q) + g(q) + \mathbf\tau = \mathbf B(\mathbf q) \left( \mathbf K_p \tilde{\mathbf q} + \mathbf K_d \dot{\tilde{\mathbf q}} + \ddot{\mathbf q}_d \right) + \mathbf C(\mathbf q, \dot{\mathbf q}) \dot{\mathbf q} + \mathbf f(\dot{\mathbf q}) + \mathbf g(\mathbf q) and can be implemented in C++ as From 8683b75ae6b807403eb5dc0327acca3469d0c872 Mon Sep 17 00:00:00 2001 From: Vincenzo Petrone Date: Fri, 18 Apr 2025 08:48:00 +0200 Subject: [PATCH 7/7] Rename dynamics-based controllers to model-based controllers --- doc/writing_new_controller.rst | 2 +- effort_controllers/doc/userdoc.rst | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 22eb9a2b0e..ca30c55d53 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -146,4 +146,4 @@ Useful External References .. NOTE:: The script is currently only recommended to use with Humble, not compatible with the API from Jazzy and onwards. -- :ref:`Writing a new dynamics-based controller `: when writing dynamics-based controllers, e.g. gravity compensation or inverse dynamics controllers, the manipulator's dynamic matrices can be retrieved using the `inverse_dynamics_solver plugin `_. +- :ref:`Writing a new model-based controller `: 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 `_. diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst index 7afc0f085d..06bfe3f38c 100644 --- a/effort_controllers/doc/userdoc.rst +++ b/effort_controllers/doc/userdoc.rst @@ -46,10 +46,10 @@ An example parameter file is given here joints: - slider_to_cart -Dynamics-based controllers +Model-based controllers -------------------------- -Dynamics-based controllers' control laws often require computing the manipulator's dynamics terms, e.g. inertia, coriolis, friction, and gravity contributions. +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 `_. Control laws @@ -59,7 +59,7 @@ For instance, the PD Control with Gravity Compensation law is the following .. math:: - \mathbf\tau = \mathbf K_p \tilde{\mathbf q} - \mathbf K_d \dot{\mathbf q} + \mathbf g(\mathbf q) + \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 @@ -71,7 +71,7 @@ Similarly, the inverse dynamics control law is the following .. math:: - \mathbf\tau = \mathbf B(\mathbf q) \left( \mathbf K_p \tilde{\mathbf q} + \mathbf K_d \dot{\tilde{\mathbf q}} + \ddot{\mathbf q}_d \right) + \mathbf C(\mathbf q, \dot{\mathbf q}) \dot{\mathbf q} + \mathbf f(\dot{\mathbf q}) + \mathbf g(\mathbf q) + \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