Feature/fri wrench control

.github/workflows/industrial_ci.yml

@@ -33,7 +33,7 @@ jobs:
       BUILDER: colcon
       ANALYZER: sonarqube
       TEST_COVERAGE: true
-      UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#master github:kroshu/kuka-external-control-sdk#master'
+      UPSTREAM_WORKSPACE: 'github:kroshu/kuka_robot_descriptions#feature/fri_wrench_control github:kroshu/kuka-external-control-sdk#master'
       ROS_DISTRO: jazzy
       CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci)
       EVENT_NAME: ${{ github.event_name }}

controllers/kuka_controllers/package.xml

@@ -13,6 +13,7 @@
   <exec_depend>fri_configuration_controller</exec_depend>
   <exec_depend>fri_state_broadcaster</exec_depend>
   <exec_depend>joint_group_impedance_controller</exec_depend>
+  <exec_depend>wrench_controller</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

controllers/wrench_controller/CHANGELOG.rst

@@ -0,0 +1,16 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package wrench_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.9.2 (2024-07-10)
+------------------
+* Fix GCC warning causing unstable build
+
+0.9.1 (2024-07-08)
+------------------
+* Add missing test dependency
+
+0.9.0 (2024-07-08)
+------------------
+* Add controller for updating stiffness and damping command interfaces
+* Contributors: Aron Svastits

controllers/wrench_controller/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.5)
+project(wrench_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(forward_command_controller)
+find_package(kuka_drivers_core)
+find_package(generate_parameter_library)
+
+include_directories(include)
+
+generate_parameter_library(
+  wrench_controller_parameters
+  src/wrench_controller_parameters.yaml
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/wrench_controller.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE
+  include
+)
+
+ament_target_dependencies(${PROJECT_NAME} forward_command_controller kuka_drivers_core
+)
+target_link_libraries(${PROJECT_NAME} wrench_controller_parameters)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "WRENCH_CONTROLLER_BUILDING_LIBRARY")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES controller_plugins.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_include_directories(
+  include
+)
+
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

controllers/wrench_controller/controller_plugins.xml

@@ -0,0 +1,7 @@
+<library path="wrench_controller">
+  <class name="kuka_controllers/WrenchController" type="kuka_controllers::WrenchController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      This controller forwards the wrench commands in real time
+    </description>
+  </class>
+</library>

controllers/wrench_controller/include/wrench_controller/visibility_control.h

@@ -0,0 +1,49 @@
+//    Copyright 2024 Mihaly Kristofi
+//
+//    Licensed under the Apache License, Version 2.0 (the "License");
+//    you may not use this file except in compliance with the License.
+//    You may obtain a copy of the License at
+//
+//        http://www.apache.org/licenses/LICENSE-2.0
+//
+//    Unless required by applicable law or agreed to in writing, software
+//    distributed under the License is distributed on an "AS IS" BASIS,
+//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//    See the License for the specific language governing permissions and
+//    limitations under the License.
+
+#ifndef WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_
+#define WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define WRENCH_CONTROLLER_EXPORT __attribute__((dllexport))
+#define WRENCH_CONTROLLER_IMPORT __attribute__((dllimport))
+#else
+#define WRENCH_CONTROLLER_EXPORT __declspec(dllexport)
+#define WRENCH_CONTROLLER_IMPORT __declspec(dllimport)
+#endif
+#ifdef WRENCH_CONTROLLER_BUILDING_LIBRARY
+#define WRENCH_CONTROLLER_PUBLIC WRENCH_CONTROLLER_EXPORT
+#else
+#define WRENCH_CONTROLLER_PUBLIC WRENCH_CONTROLLER_IMPORT
+#endif
+#define WRENCH_CONTROLLER_PUBLIC_TYPE WRENCH_CONTROLLER_PUBLIC
+#define WRENCH_CONTROLLER_LOCAL
+#else
+#define WRENCH_CONTROLLER_EXPORT __attribute__((visibility("default")))
+#define WRENCH_CONTROLLER_IMPORT
+#if __GNUC__ >= 4
+#define WRENCH_CONTROLLER_PUBLIC __attribute__((visibility("default")))
+#define WRENCH_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define WRENCH_CONTROLLER_PUBLIC
+#define WRENCH_CONTROLLER_LOCAL
+#endif
+#define WRENCH_CONTROLLER_PUBLIC_TYPE
+#endif
+
+#endif  // WRENCH_CONTROLLER__VISIBILITY_CONTROL_H_

controllers/wrench_controller/include/wrench_controller/wrench_controller.hpp

@@ -0,0 +1,45 @@
+// Copyright 2024 Mihaly Kristofi
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_
+#define WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "forward_command_controller/multi_interface_forward_command_controller.hpp"
+
+#include "wrench_controller/visibility_control.h"
+#include "wrench_controller_parameters.hpp"
+
+namespace kuka_controllers
+{
+class WrenchController : public forward_command_controller::ForwardControllersBase
+{
+public:
+  WRENCH_CONTROLLER_PUBLIC WrenchController();
+
+private:
+  WRENCH_CONTROLLER_LOCAL void declare_parameters() override;
+  WRENCH_CONTROLLER_LOCAL controller_interface::CallbackReturn read_parameters() override;
+
+  using Params = wrench_controller::Params;
+  using ParamListener = wrench_controller::ParamListener;
+
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+};
+}  // namespace kuka_controllers
+#endif  // WRENCH_CONTROLLER__WRENCH_CONTROLLER_HPP_

controllers/wrench_controller/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wrench_controller</name>
+  <version>0.9.2</version>
+  <description>Controller for forwarding wrench commands</description>
+
+  <maintainer email="krmihaly42@gmail.com">Mihaly Kristofi</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>forward_command_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>kuka_drivers_core</depend>
+  <depend>generate_parameter_library</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

controllers/wrench_controller/src/wrench_controller.cpp

@@ -0,0 +1,66 @@
+// Copyright 2024 Mihaly Kristofi
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "pluginlib/class_list_macros.hpp"
+
+#include "kuka_drivers_core/hardware_interface_types.hpp"
+
+#include "wrench_controller/wrench_controller.hpp"
+
+namespace kuka_controllers
+{
+
+WrenchController::WrenchController() : ForwardControllersBase() {}
+
+void WrenchController::declare_parameters()
+{
+  param_listener_ = std::make_shared<ParamListener>(get_node());
+}
+
+controller_interface::CallbackReturn WrenchController::read_parameters()
+{
+  if (!param_listener_)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  params_ = param_listener_->get_params();
+
+  if (params_.joints.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter is empty");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  if (params_.interface_names.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  for (const auto & joint : params_.joints)
+  {
+    for (const auto & interface : params_.interface_names)
+    {
+      command_interface_types_.push_back(joint + "/" + interface);
+    }
+  }
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+}  // namespace kuka_controllers
+
+PLUGINLIB_EXPORT_CLASS(
+  kuka_controllers::WrenchController, controller_interface::ControllerInterface)

controllers/wrench_controller/src/wrench_controller_parameters.yaml

@@ -0,0 +1,11 @@
+wrench_controller:
+  joints: {
+    type: string_array,
+    default_value: [],
+    description: "Name of the joints to control",
+  }
+  interface_names: {
+    type: string_array,
+    default_value: [],
+    description: "Names of the interfaces to command",
+  }

doc/wiki/3_Sunrise_FRI.md

@@ -29,8 +29,10 @@ The parameters in the driver configuration file can be also changed during runti
 - `receive_multiplier` (integer): this parameter defines the answer rate factor (the client should sends commands in every `receive_multiplier`*`send_period_ms` milliseconds). It must be at least 1 and can be only changed in `inactive` and `configuring` states.
 - `control_mode`: The enum value of the control mode should be given, which updates the `ControlMode` and `ClientCommandMode` parameters of FRI. It cannot be changed in active state.
 - `joint_damping`, `joint_stiffness` (double vectors): these parameters change the stiffness and damping attributes of joint impedance control mode. The updated values are sent to the hardware interface using the `joint_group_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `joint_group_impedance_controller` is deactivated at driver activation.)
+- `cartesian_damping`, `cartesian_stiffness` (double vectors): these parameters change the stiffness and damping attributes of cartesian impedance control and wrench mode. The updated values are sent to the hardware interface using the `caretsian_impedance_controller` to adapt to conventions, but it is not possible to change them in active state due to the constraints of FRI. (Therefore the `cartesian_impedance_controller` is deactivated at driver activation.)
 - `position_controller_name`: The name of the controller (string) that controls the `position` interface of the robot. It can't be changed in active state.
 - `torque_controller_name`: The name of the controller (string) that controls the `effort` interface of the robot. It can't be changed in active state.
+- `wrench_controller_name`: The name of the controller (string) that controls the `wrench` interface of the robot. It can't be changed in active state.
 
 ### Usage
 
@@ -47,10 +49,13 @@ This starts the 3 core components of every driver (described in the [Non-real-ti
 - [`fri_configuration_controller`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_configuration_controller) (no configuration file)
 - [`fri_state_broadcaster`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#fri_state_broadcaster) (no configuration file)
 - `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml))
+- `cartesian_impedance_controller` (of type `JointGroupImpedanceController`,[configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml))
 - `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml))
 - [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file)
 - `external_torque_broadcaster` (of type `JointStateBroadcaster`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml), publishes a `JointState` message type on the topic `external_torque_broadcaster/joint_states` containing the measured external torques for every joint)
-3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`):
+- `wrench_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/wrench_controller_config.yaml))
+
+1. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`):
     ```
     ros2 lifecycle set robot_manager configure
     ros2 lifecycle set robot_manager activate
@@ -75,6 +80,8 @@ Both launch files support the following argument:
 - `jic_config`: the location of the configuration file for the `joint_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml`)
 - `ec_config`: the location of the configuration file for the `effort_controller` (defaults to `kuka_sunrise_fri_driver/config/effort_controller_config.yaml`)
 - `etb_config`: the location of the configuration file for the `external_torque_broadcaster` (defaults to `kuka_sunrise_fri_driver/config/external_torque_broadcaster._config.yaml`)
+- `cic_config`: the location of the configuration file for the `cartesian_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/cartesian_impedance_controller_config.yaml`)
+- `wc_config`: the location of the configuration file for the `wrench_controller` (defaults to `kuka_sunrise_fri_driver/config/wrench_controller_config.yaml`)
 
 
 The `startup_with_rviz.launch.py` additionally contains one argument:

doc/wiki/Home.md

@@ -83,7 +83,7 @@ The following table shows the supported features and control modes of each drive
 |OS | Joint position control | Joint impedance control | Joint velocity control | Joint torque control | Cartesian position control | Cartesian impedance control | Cartesian velocity control | Wrench control| I/O control|
 |---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
 |KSS| ✓ | ✗ | ✗ | ✗ | | ✗ | ✗ | ✗ | |
-|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ | | |
+|Sunrise| ✓ | ✓ | ✗ | ✓ | | | ✗ |✓| |
 |iiQKA| ✓ | ✓ | ✗ | ✓ | ✗ | ✗ | ✗ | ✗ | ✗ |
 
 

kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp

@@ -29,6 +29,8 @@ static constexpr char FRI_STATE_BROADCASTER[] = "fri_state_broadcaster";
 // Controller names with default values
 static constexpr char JOINT_TRAJECTORY_CONTROLLER[] = "joint_trajectory_controller";
 static constexpr char JOINT_GROUP_IMPEDANCE_CONTROLLER[] = "joint_group_impedance_controller";
+static constexpr char CARTESIAN_IMPEDANCE_CONTROLLER[] = "cartesian_impedance_controller";
+
 }  // namespace kuka_drivers_core
 
 #endif  // KUKA_DRIVERS_CORE__CONTROLLER_NAMES_HPP_

kuka_drivers_core/include/kuka_drivers_core/hardware_interface_types.hpp

@@ -25,6 +25,15 @@ static constexpr char HW_IF_DAMPING[] = "damping";
 // Constant defining external torque interface
 static constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque";
 
+// Constand defining cartesian coordinate interfaces
+static constexpr char HW_IF_X[] = "x";
+static constexpr char HW_IF_Y[] = "y";
+static constexpr char HW_IF_Z[] = "z";
+static constexpr char HW_IF_A[] = "a";
+static constexpr char HW_IF_B[] = "b";
+static constexpr char HW_IF_C[] = "c";
+static constexpr char HW_IF_CART_PREFIX[] = "dummy_cart_joint";
+
 /* Interface prefixes */
 // Constant defining prefix for I/O interfaces
 static constexpr char IO_PREFIX[] = "gpio";
@@ -34,7 +43,10 @@ static constexpr char CONFIG_PREFIX[] = "runtime_config";
 static constexpr char FRI_STATE_PREFIX[] = "fri_state";
 // Constant defining prefix for states
 static constexpr char STATE_PREFIX[] = "state";
-
+// Constant defining wrench interface prefixes
+static constexpr char HW_IF_WRENCH_PREFIX[] = "wrench";
+static constexpr char HW_IF_FORCE_PREFIX[] = "force";
+static constexpr char HW_IF_TORQUE_PREFIX[] = "torque";
 /* Configuration interfaces */
 // Constant defining control_mode configuration interface
 static constexpr char CONTROL_MODE[] = "control_mode";