diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 0874448461..ca30c55d53 100644 --- a/doc/writing_new_controller.rst +++ b/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 `_ .. 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 `: 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 62e98a75f9..06bfe3f38c 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 + +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 `_. + +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 `_ 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 `_ with KDL for simulated robots.