Feature/gravity pd controller

doc/controllers_index.rst

@@ -59,6 +59,7 @@ The controllers are using `common hardware interface definitions`_, and may use
    Velocity Controllers <../velocity_controllers/doc/userdoc.rst>
    Gpio Command Controller <../gpio_controllers/doc/userdoc.rst>
    Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst>
+   Gravity Compensation Controller <../gravity_compensation_controller/doc/userdoc.rst>
 
 Broadcasters
 **********************

gravity_compensation_pd_controller/CMakeLists.txt

@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 3.16)
+project(gravity_compensation_pd_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX
+  OR CMAKE_CXX_COMPILER_ID
+    MATCHES
+    "Clang"
+)
+  add_compile_options(
+    -Wall
+    -Wextra
+    -Wpedantic
+  )
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+    controller_interface
+    Eigen3
+    generate_parameter_library
+    hardware_interface
+    inverse_dynamics_solver
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+    realtime_tools
+    urdf
+)
+
+find_package(ament_cmake REQUIRED)
+
+foreach(dependency IN
+        ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+  find_package(${dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(gravity_compensation_pd_controller_parameters
+                          src/gravity_compensation_pd_controller_parameters.yaml
+)
+
+add_library(gravity_compensation_pd_controller SHARED
+            src/gravity_compensation_pd_controller.cpp
+)
+
+target_compile_features(
+  gravity_compensation_pd_controller
+  PUBLIC cxx_std_17
+)
+
+target_include_directories(
+  gravity_compensation_pd_controller
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include/gravity_compensation_pd_controller>
+)
+
+target_link_libraries(
+  gravity_compensation_pd_controller
+  PUBLIC gravity_compensation_pd_controller_parameters
+)
+
+target_link_libraries(
+  gravity_compensation_pd_controller
+  PUBLIC controller_interface::controller_interface
+        hardware_interface::hardware_interface
+        hardware_interface::mock_components
+        inverse_dynamics_solver::inverse_dynamics_solver_lib
+        pluginlib::pluginlib
+        rclcpp::rclcpp
+        rclcpp_lifecycle::rclcpp_lifecycle
+        realtime_tools::realtime_tools
+        realtime_tools::thread_priority
+        urdf::urdf
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport, which is appropriate when
+# building the dll but not consuming it.
+target_compile_definitions(
+  gravity_compensation_pd_controller
+  PRIVATE "GRAVITY_COMPENSATION_PD_CONTROLLER_BUILDING_DLL"
+)
+
+pluginlib_export_plugin_description_file(
+  controller_interface gravity_compensation_pd_controller.xml
+)
+
+install(DIRECTORY include
+        DESTINATION include/${PROJECT_NAME}
+)
+
+install(TARGETS gravity_compensation_pd_controller gravity_compensation_pd_controller_parameters
+        EXPORT export_gravity_compensation_pd_controller
+        RUNTIME DESTINATION bin
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_gravity_compensation_pd_controller
+    test/test_gravity_compensation_pd_controller.cpp
+  )
+  target_link_libraries(test_gravity_compensation_pd_controller
+    gravity_compensation_pd_controller
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+  ament_add_gmock(test_load_gravity_compensation_pd_controller test/test_load_gravity_compensation_pd_controller.cpp)
+  target_link_libraries(test_load_gravity_compensation_pd_controller
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+endif()
+
+ament_export_targets(export_gravity_compensation_pd_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

gravity_compensation_pd_controller/doc/gravity_compensation_pd_controller_parameters.md

@@ -0,0 +1,118 @@
+# Gravity Compensation Pd Controller Parameters
+
+Default Config
+```yaml
+gravity_compensation_pd_controller:
+  ros__parameters:
+    command_interfaces_names_override:
+      effort: '{}'
+    compensate_gravity: true
+    d_gains: '{}'
+    dynamics_solver:
+      dynamics_solver_plugin: ''
+      root: ''
+      tip: ''
+    joint_space_reference_controller: ''
+    joints: '{}'
+    p_gains: '{}'
+    state_interfaces_names_override:
+      position: '{}'
+      velocity: '{}'
+
+```
+
+## joints
+
+Specifies which joints will be used by the controller.
+
+* Type: `string_array`
+* Default Value: {}
+
+## joint_space_reference_controller
+
+Name of the joint space reference controller.
+
+* Type: `string`
+* Default Value: ""
+* Read only: True
+
+## state_interfaces_names_override.position
+
+If this parameter is set, it will override the default names for the position interface of the state interfaces.
+
+* Type: `string_array`
+* Default Value: {}
+* Read only: True
+
+## state_interfaces_names_override.velocity
+
+If this parameter is set, it will override the default names for the velocity interface of the state interfaces.
+
+* Type: `string_array`
+* Default Value: {}
+* Read only: True
+
+## command_interfaces_names_override.effort
+
+If this parameter is set, it will override the default names for the effort interface of the command interfaces.
+
+* Type: `string_array`
+* Default Value: {}
+* Read only: True
+
+## p_gains
+
+* Type: `double_array`
+* Default Value: {}
+
+## d_gains
+
+* Type: `double_array`
+* Default Value: {}
+
+## compensate_gravity
+
+If true, the controller will compensate for gravity.
+
+* Type: `bool`
+* Default Value: true
+
+## dynamics_solver.dynamics_solver_plugin
+
+Name of the dynamics solver to use.
+
+* Type: `string`
+* Read only: True
+
+*Constraints:*
+ - parameter is not empty
+
+*Additional Constraints:*
+
+
+
+## dynamics_solver.root
+
+Specifies the base link of the robot description used by the inverse dynamics solver.
+
+* Type: `string`
+* Read only: True
+
+*Constraints:*
+ - parameter is not empty
+
+*Additional Constraints:*
+
+
+
+## dynamics_solver.tip
+
+Specifies the end effector link of the robot description used by the inverse dynamics solver.
+
+* Type: `string`
+* Read only: True
+
+*Constraints:*
+ - parameter is not empty
+
+*Additional Constraints:*

gravity_compensation_pd_controller/doc/userdoc.rst

@@ -0,0 +1,62 @@
+:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gravity_compensation_pd_controller/doc/userdoc.rst
+
+.. _gravity_compensation_pd_controller_userdoc:
+
+Gravity Compensation PD Controller
+==================================
+
+This controller provides a PD controller with gravity compensation that converts joint position references to joint effort commands for a robotic manipulator.
+It is a chainable controller; therefore, it requires another controller to provide joint position references via chaining.
+
+The controller uses the robot's dynamic model to compute the control action.
+In particular, it relies on the `InverseDynamicsSolver <https://index.ros.org/p/inverse_dynamics_solver/>`_ interface to estimate the gravity torque vector.
+
+Control Law
+-----------
+
+The implemented control law follows the scheme shown in the figure, based on the formulation presented in *B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, "Robotics: Modelling, Planning and Control"*.
+
+.. image:: ../doc/media/gravity_compensation_pd_controller_scheme.png
+   :alt: Gravity Compensation PD Controller Scheme
+
+
+Description of controller's interfaces
+--------------------------------------
+
+References (from a preceding controller)
+,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
+- ``<joint_name>/position`` [double]
+
+Commands
+,,,,,,,,
+- ``<joint_name>/effort`` [double]
+
+States
+,,,,,,
+- ``<joint_name>/position`` [double]
+- ``<joint_name>/velocity`` [double]
+
+
+Behavior in Edge Cases
+----------------------
+
+The controller is designed to handle the following edge cases:
+
+- **Computed torque exceeding joint limits**. If the computed torque exceeds the joint limits, the controller will saturate the output to remain within the range specified in the robot description.
+
+- **Invalid joint position reference**. If the joint position reference is ``NaN``, the controller will fall back to the last valid reference to compute the control action. Initially, the first valid reference is set to the robot's starting joint positions.
+
+- **Invalid joint position state**. If the controller detects a ``NaN`` value in the joint position state reported by the robot, it will return an error to the controller manager. In this case, the controller cannot compute a valid control action and will output ``NaN`` values. It is assumed that the hardware interface is capable of handling ``NaN`` values appropriately.
+
+
+Parameters
+----------
+
+This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters. The parameter `definition file located in the src folder <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gravity_compensation_pd_controller/src/gravity_compensation_pd_controller_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+
+.. generate_parameter_library_details:: ../src/gravity_compensation_pd_controller_parameters.yaml
+
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gravity_compensation_pd_controller/test/config/test_gravity_compensation_pd_controller.yaml>`_:
+
+.. literalinclude:: ../test/config/test_gravity_compensation_pd_controller.yaml
+   :language: yaml

gravity_compensation_pd_controller/gravity_compensation_pd_controller.xml

@@ -0,0 +1,9 @@
+<library path="gravity_compensation_pd_controller">
+  <class name="gravity_compensation_pd_controller/GravityCompensationPDController"
+         type="gravity_compensation_pd_controller::GravityCompensationPDController"
+         base_class_type="controller_interface::ChainableControllerInterface">
+    <description>
+      A gravity compensation PD controller developed by the Automatic Control Group of the University of Salerno, Italy.
+    </description>
+  </class>
+</library>