Add cartesian admittance controller (beta version)

.docker/ci/Dockerfile

@@ -6,3 +6,5 @@ RUN apt update && apt upgrade -y
 RUN apt install -y python3-pip
 RUN apt install -y git tree clang-format-14 cppcheck
 RUN pip install pre-commit
+RUN cd ros2_dev/src && \
+    vcs import . < cartesian_controllers_ros2/cartesian_controllers_ros2.repos

.docker/run/Dockerfile

@@ -7,7 +7,8 @@ RUN apt install -y python3-pip
 RUN apt install -y git clang-format-14 cppcheck
 RUN pip install pre-commit
 RUN apt update && \
-    cd ros2_dev/src/cartesian_controllers_ros2
+    cd ros2_dev/src && \
+    vcs import . < cartesian_controllers_ros2/cartesian_controllers_ros2.repos
 RUN cd ros2_dev/ && \
     . /opt/ros/${ROS_DISTRO}/setup.sh && \
     rosdep install --ignore-src --from-paths . -y -r

.github/workflows/ci.yml

@@ -35,6 +35,7 @@ jobs:
           run: |
             cd /ros # structure = <...>/ros/src/cartesian_controllers_ros2/<...>
             . /opt/ros/humble/setup.sh
+            vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos
             rosdep install --ignore-src --from-paths . -y -r
             colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
 
@@ -46,9 +47,10 @@ jobs:
           run: |
             cd /ros
             . /opt/ros/humble/setup.sh
+            vcs import src < src/cartesian_controllers_ros2/cartesian_controllers_ros2.repos
             rosdep install --ignore-src --from-paths . -y -r
             colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
-            colcon test
+            colcon test --event-handlers console_direct+
             colcon test-result
 
       - name: Upload Tests to Artifacts

cartesian_admittance_controller/CMakeLists.txt

@@ -0,0 +1,117 @@
+cmake_minimum_required(VERSION 3.16)
+project(cartesian_admittance_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(DEPENDENCIES
+  angles
+  control_msgs
+  control_toolbox
+  controller_interface
+  Eigen3
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  joint_trajectory_controller
+  kinematics_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_kdl
+  tf2_ros
+  trajectory_msgs
+  cartesian_control_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${DEPENDENCIES})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(cartesian_admittance_controller_parameters
+  src/cartesian_admittance_controller_parameters.yaml
+)
+
+add_library(cartesian_admittance_controller SHARED
+  src/cartesian_admittance_controller.cpp
+  src/compliance_frame_trajectory.cpp
+  src/cartesian_admittance_rule.cpp
+  src/rules/vanilla_cartesian_admittance_rule.cpp
+)
+target_compile_features(cartesian_admittance_controller PUBLIC cxx_std_17)
+target_include_directories(cartesian_admittance_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/cartesian_admittance_controller>
+)
+target_link_libraries(cartesian_admittance_controller PUBLIC
+  cartesian_admittance_controller_parameters
+)
+ament_target_dependencies(cartesian_admittance_controller PUBLIC ${DEPENDENCIES})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(cartesian_admittance_controller PRIVATE "CARTESIAN_ADMITTANCE_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface cartesian_admittance_controller.xml)
+pluginlib_export_plugin_description_file(cartesian_admittance_controller cartesian_admittance_rules.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  # Dynamically loaded during test
+  find_package(kinematics_interface_kdl REQUIRED)
+
+  # test controller (loading only)
+  add_rostest_with_parameters_gmock(test_load_controller
+    test/test_load_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
+  )
+  ament_target_dependencies(test_load_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  # test controller
+  add_rostest_with_parameters_gmock(test_controller
+    test/test_cartesian_admittance_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml
+  )
+  target_link_libraries(test_controller cartesian_admittance_controller)
+  ament_target_dependencies(test_controller
+    control_msgs
+    controller_interface
+    hardware_interface
+    ros2_control_test_assets
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/cartesian_admittance_controller
+)
+
+install(TARGETS cartesian_admittance_controller cartesian_admittance_controller_parameters
+  EXPORT export_cartesian_admittance_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_targets(export_cartesian_admittance_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${DEPENDENCIES})
+ament_package()

cartesian_admittance_controller/README.md

@@ -0,0 +1,82 @@
+# cartesian_admittance_controller
+
+This is a cartesian version of the package `admittance_controller` from [ros-controls/ros2_controllers](https://github.com/ros-controls/ros2_controllers/tree/master).
+
+> **Warning**
+>
+> This package is currently under development and possibly unsafe if used to control an actual robot!
+
+## Control law
+
+The controller imposes the following cartesian behavior
+
+$$
+\begin{align}
+  M \ddot{e} + D \dot{e} + K e = - f_{ext}
+\end{align}
+$$
+
+where $e = p^d - p$, $M$ and $D$ are the (positive definite) desired inertia and damping matrix, respectively, and $K^d$ is the (positive semi-definite) desired stiffness matrix.
+
+The cartesian acceleration command $\ddot{p}^c$ is therefore computed as
+
+$$
+\begin{align}
+  \ddot{p}^c = \ddot{p}^d + M^{-1} \left( D \dot{e} + K \dot{e} + f_{ext} \right)
+\end{align}
+$$
+
+and integrated to obtain the cartesian velocity command $\dot{p}^c$.
+
+The joint velocity command $\dot{q}^c$ is computed from $\dot{p}^c$ using the `kinematics_interface`.
+
+In order to accommodate for both the `position` and `velocity` command interfaces, the joint velocity command $\dot{q}^c$ is integrated to provide a joint position command ${q}^c$ such that
+
+$$
+\begin{align}
+  {q}^c &= q + T_s \dot{q}^c
+\end{align}
+$$
+
+where $q$ is the **measured** joint position and $T_s$ is the controller sampling time.
+
+## Topics
+
+- `~/compliant_frame_reference` (input topic) [`cartesian_control_msgs::msg::CompliantFrameTrajectory`]
+
+  Target (cartesian) compliant frame containing at least a pose, a twist.
+
+  Additionally, a desired compliance (stiffness, damping, inertia) can be provided for each reference cartesian state in the trajectory. Otherwise, the ros parameters are used. For instance you could provide the following ros parameters:
+
+  ```yaml
+  <controller_name>:
+    admittance:
+      frame:
+        id: <admittance_reference_frame> # e.g., tool0 or world
+
+      inertia: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1]
+      damping_ratio: [1., 1., 1., 1., 1., 1.]
+      stiffness: [200., 200., 200., 50., 50., 50.]
+  ```
+
+- `~/state` (output topic) [`cartesian_control_msgs::msg::AdmittanceControllerState`]
+
+  Topic publishing controller internal states.
+
+  **TODO(tpoignonec)** Implements + use custom message `VicControllerState`
+
+
+## ros2_control interfaces
+
+### States
+
+The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``.
+The necessary state interfaces are ``position`` and ``velocity``.
+**Both are mandatory!**
+
+Additionally, the `Force Torque Sensor` semantic component is used and requires force state interfaces.
+
+### Commands
+
+Ideally, the control interface is a joint ``velocity`` group.
+The controller can also use a ``position`` command interface.

cartesian_admittance_controller/cartesian_admittance_controller.xml

@@ -0,0 +1,9 @@
+<library path="cartesian_admittance_controller">
+  <class name="cartesian_admittance_controller/CartesianAdmittanceController"
+         type="cartesian_admittance_controller::CartesianAdmittanceController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      CartesianAdmittanceController ros2_control controller.
+    </description>
+  </class>
+</library>

cartesian_admittance_controller/cartesian_admittance_rules.xml

@@ -0,0 +1,9 @@
+<library path="cartesian_admittance_controller">
+  <class name="cartesian_admittance_controller/VanillaCartesianAdmittanceRule"
+         type="cartesian_admittance_controller::VanillaCartesianAdmittanceRule"
+         base_class_type="cartesian_admittance_controller::CartesianAdmittanceRule">
+    <description>
+      Classical admittance control law.
+    </description>
+  </class>
+</library>

cartesian_admittance_controller/config/example_controller_config.yaml

@@ -0,0 +1,97 @@
+cartesian_admittance_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+      # - velocity
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+
+    filters:
+      state_filter_cuttoff_freq: 100.0
+      command_filter_cuttoff_freq: 100.0
+
+    kinematics:
+      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
+      plugin_package: kinematics_interface
+      base: base_link # Assumed to be stationary
+      tip: interaction_point
+      group_name: arm
+      alpha: 0.0005
+
+    ft_sensor:
+      name: ftsensor
+      frame:
+        id: ft_sensor  # Wrench measurements are in this frame
+        external: false # force torque frame exists within URDF kinematic chain
+      filter_coefficient: 0.1 # 1 - exp(-Ts/tau) = 1 - exp(-Ts*2*pi*f_cutoff)
+
+    control:
+      frame:
+        id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+        external: false # control frame exists within URDF kinematic chain
+
+    fixed_world_frame:   # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
+      frame:
+        id: world # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+        external: false # control frame exists within URDF kinematic chain
+
+    gravity_compensation:
+      frame:
+        id: ft_sensor
+        external: false
+
+      CoG: # specifies the center of gravity of the end effector (post FT sensor)
+        pos:
+          - 0.0015 # x
+          - 0.0001 # y
+          - 0.0371 # z
+        force: 0.5 # mass * 9.81
+
+    admittance:
+      frame:
+        id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+      # Admittance rule
+      plugin_package: cartesian_admittance_controller
+      plugin_name: cartesian_admittance_controller/VanillaCartesianAdmittanceRule  # Nominal controller
+
+      selected_axes:
+        - true # x
+        - true # y
+        - true # z
+        - true # rx
+        - true # ry
+        - true # rz
+
+      # Having ".0" at the end is MUST, otherwise there is a loading error
+      # F = M*a + D*v + S*(x - x_d)
+      inertia:
+        - 5.0
+        - 5.0
+        - 5.0
+        - 0.1
+        - 0.1
+        - 0.1
+
+      damping_ratio: # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
+        - 0.9 # x
+        - 0.9 # y
+        - 0.9 # z
+        - 0.99 # rx
+        - 0.99 # ry
+        - 0.99 # rz
+
+      stiffness:
+        - 200.0
+        - 200.0
+        - 200.0
+        - 2.0
+        - 2.0
+        - 2.0