add basic CI tests (backport #26) (#29)

* add basic CI tests (#26) * add example configs * test load * rename test src files * refractor existing tests * fix jazzy-specific problem when instanciating th ctrl manager * add ft_sensor in URDF * fix ft_sensor frame * update mandatory parameters list * make vic plugin desc. param mandatory * fix tests (basic tests only...) * linting * fix "ft_sensor:is_enabled" param issues * source in CI before test * add dynamics_interface_kdl to pkg XML (cherry picked from commit f8fccc4) * Update test_load_controller.cpp for humble compatibility * Update test_cartesian_vic_controller.hpp for humble compatibility --------- Co-authored-by: Thibault Poignonec <[email protected]>
ICube-Robotics · Aug 8, 2024 · 04dd1d4 · 04dd1d4
1 parent 397ea45
commit 04dd1d4
Show file tree

Hide file tree

Showing 10 changed files with 336 additions and 173 deletions.
diff --git a/cartesian_vic_controller/CMakeLists.txt b/cartesian_vic_controller/CMakeLists.txt
@@ -69,14 +69,15 @@ target_compile_definitions(cartesian_vic_controller PRIVATE "CARTESIAN_VIC_CONTR
 pluginlib_export_plugin_description_file(controller_interface cartesian_vic_controller.xml)
 pluginlib_export_plugin_description_file(cartesian_vic_controller cartesian_vic_rules.xml)
 
-if(0) # (BUILD_TESTING)
+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
@@ -87,6 +88,7 @@ if(0) # (BUILD_TESTING)
     hardware_interface
     ros2_control_test_assets
   )
+  #]]
 
   # test controller
   add_rostest_with_parameters_gmock(test_controller

diff --git a/cartesian_vic_controller/config/example_admittance_controller_config.yaml b/cartesian_vic_controller/config/example_admittance_controller_config.yaml
@@ -1,8 +1,7 @@
-cartesian_vic_controller:
+cartesian_impedance_controller:
   ros__parameters:
     command_interfaces:
-      - position
-      # - velocity
+      - velocity
     state_interfaces:
       - position
       - velocity
@@ -13,17 +12,21 @@ cartesian_vic_controller:
       - joint_a4
       - joint_a5
       - joint_a6
+      - joint_a7
+
+    # Parameters for the controller
+    # Note: having ".0" at the end of float is a MUST, otherwise there is a loading error
 
     filters:
-      state_position_filter_cuttoff_freq: 100.0
+      state_position_filter_cuttoff_freq: -1.0
       state_velocity_filter_cuttoff_freq: 100.0
       command_filter_cuttoff_freq: -1.0
       ft_sensor_filter_cuttoff_freq: 30.0
 
     dynamics:
-      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
-      plugin_package: kinematics_interface
-      base: base_link # Assumed to be stationary
+      plugin_name: dynamics_interface_kdl/DynamicsInterfaceKDL
+      plugin_package: dynamics_interface
+      base: iiwa_base # Assumed to be stationary
       tip: interaction_point
       alpha: 0.0005
       gravity: [0.0, 0.0, -9.81]
@@ -41,33 +44,37 @@ cartesian_vic_controller:
 
     control:
       frame:
-        id: interaction_point # Vic calcs (displacement etc) are done in this frame. Usually the tool or end-effector
+        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 # Vic calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-        external: false # control frame exists within URDF kinematic chain
+        id: iiwa_base
+        external: false
 
     gravity_compensation:
       frame:
         id: ft_sensor
         external: false
 
-      CoG: # specifies the center of gravity of the end effector (post FT sensor)
+      CoG:
         pos:
-          - 0.0015 # x
-          - 0.0001 # y
-          - 0.0371 # z
-        force: 0.5 # mass * 9.81
+          - 0.01
+          - 0.01
+          - 0.01
+        force: 0.0  # mass * 9.81
 
     vic:
       frame:
-        id: interaction_point # Vic calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-      # Vic rule
+        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
+      # VIC rule
       plugin_package: cartesian_vic_controller
-      plugin_name: cartesian_vic_controller/VanillaCartesianAdmittanceRule  # Nominal admittance controller
-      # plugin_name: cartesian_vic_controller/VanillaCartesianImpedanceRule  # Nominal impedance controller
+      plugin_name: cartesian_vic_controller/VanillaCartesianAdmittanceRule  # As nominal controller
+
+      # Misc. control parameters
+      activate_nullspace_control: false
+      activate_gravity_compensation: false
 
       selected_axes:
         - true # x
@@ -77,8 +84,6 @@ cartesian_vic_controller:
         - 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
@@ -91,9 +96,9 @@ cartesian_vic_controller:
         - 0.9 # x
         - 0.9 # y
         - 0.9 # z
-        - 0.9 # rx
-        - 0.9 # ry
-        - 0.9 # rz
+        - 0.99 # rx
+        - 0.99 # ry
+        - 0.99 # rz
 
       stiffness:
         - 200.0
@@ -102,5 +107,3 @@ cartesian_vic_controller:
         - 2.0
         - 2.0
         - 2.0
-
-      joint_damping: 0.5
diff --git a/cartesian_vic_controller/config/example_impedance_controller_config.yaml b/cartesian_vic_controller/config/example_impedance_controller_config.yaml
@@ -0,0 +1,116 @@
+cartesian_impedance_controller:
+  ros__parameters:
+    command_interfaces:
+      - force
+    state_interfaces:
+      - position
+      - velocity
+    joints:
+      - joint_a1
+      - joint_a2
+      - joint_a3
+      - joint_a4
+      - joint_a5
+      - joint_a6
+      - joint_a7
+
+    # Parameters for the controller
+    # Note: having ".0" at the end of float is a MUST, otherwise there is a loading error
+
+    filters:
+      state_position_filter_cuttoff_freq: -1.0
+      state_velocity_filter_cuttoff_freq: 100.0
+      command_filter_cuttoff_freq: -1.0
+      ft_sensor_filter_cuttoff_freq: 30.0
+
+    dynamics:
+      plugin_name: dynamics_interface_kdl/DynamicsInterfaceKDL
+      plugin_package: dynamics_interface
+      base: iiwa_base # Assumed to be stationary
+      tip: interaction_point
+      alpha: 0.0005
+      gravity: [0.0, 0.0, -9.81]
+
+    ft_sensor:
+      name: ft_sensor
+      frame:
+        id: ft_sensor  # Wrench measurements are in this frame
+        external: false # force torque frame exists within URDF kinematic chain
+
+    external_torque_sensor:
+      is_enabled: false
+      name: ""
+
+    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: iiwa_base
+        external: false
+
+    gravity_compensation:
+      frame:
+        id: ft_sensor
+        external: false
+
+      CoG:
+        pos:
+          - 0.01
+          - 0.01
+          - 0.01
+        force: 0.0  # mass * 9.81
+
+    vic:
+      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
+      # VIC rule
+      plugin_package: cartesian_vic_controller
+      plugin_name: cartesian_vic_controller/VanillaCartesianImpedanceRule  # As nominal controller
+
+      # Misc. control parameters
+      activate_nullspace_control: true
+      activate_gravity_compensation: true
+
+      selected_axes:
+        - true # x
+        - true # y
+        - true # z
+        - true # rx
+        - true # ry
+        - true # rz
+
+      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
+
+    nullspace_control:
+      # desired_joint_positions: [...]  # if empty, defaults to initial joint positions
+      # desired_joint_positions: [0.0, -0.7854, 0.0, 1.3962, 0.0, 0.6109, 0.0]
+      joint_inertia: [25.0]
+      joint_stiffness: [100.0]
+      joint_damping: [90.0]  # e.g., 2 * 0.9 * sqrt (m * k)
+      # Example values for {M, K, D} : {25, 10, 28}, {25, 100, 90}, {25, 200, 128}
diff --git a/cartesian_vic_controller/package.xml b/cartesian_vic_controller/package.xml
@@ -11,6 +11,7 @@
 
   <depend>cartesian_control_msgs</depend>
   <depend>dynamics_interface</depend>
+  <depend>dynamics_interface_kdl</depend>
 
   <depend>backward_ros</depend>
   <depend>control_msgs</depend>

diff --git a/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml b/cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml
@@ -148,12 +148,10 @@ cartesian_vic_controller:
       }
     plugin_name: {
       type: string,
-      default_value: "cartesian_vic_controller/VanillaCartesianAdmittanceRule",
       description: "Specifies the name of the vic rule plugin to load."
     }
     plugin_package: {
       type: string,
-      default_value: "cartesian_vic_controller",
       description: "Specifies the package name that contains the admittance rule plugin."
     }
     selected_axes:
@@ -196,7 +194,7 @@ cartesian_vic_controller:
       The damping ratio is defined as: zeta = D / (2 * sqrt( M * S )).",
       validation: {
         fixed_size<>: 6,
-        element_bounds<>: [ 0.0, 1.2 ]
+        element_bounds<>: [ 0.0, 5.0 ]
       }
     }
     stiffness: {

diff --git a/cartesian_vic_controller/test/test_asset_6d_robot_description.hpp b/cartesian_vic_controller/test/test_asset_6d_robot_description.hpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright 2024 ICUBE Laboratory, University of Strasbourg
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -258,6 +258,32 @@ const auto valid_6d_robot_urdf =
       </command_interface>
       <state_interface name="position"/>
     </joint>
+    <!-- F/T sensor -->
+    <sensor name="tool0">
+      <state_interface name="force.x">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <state_interface name="force.y">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <state_interface name="force.z">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <state_interface name="torque.x">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <state_interface name="torque.y">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+
+      <state_interface name="torque.z">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+    </sensor>
   </ros2_control>
 </robot>
 )";