Add generic admittance controller for TCP wrenches (#370)

Co-authored-by: AndyZe <[email protected]>
Co-authored-by: Denis Štogl <[email protected]>

admittance_controller/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.5)
+project(admittance_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  angles
+  control_msgs
+  control_toolbox
+  controller_interface
+  kinematics_interface
+  Eigen3
+  generate_parameter_library
+  geometry_msgs
+  hardware_interface
+  joint_trajectory_controller
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_kdl
+  tf2_ros
+  trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+add_library(${PROJECT_NAME} SHARED src/admittance_controller.cpp)
+target_include_directories(${PROJECT_NAME} PRIVATE include)
+generate_parameter_library(${PROJECT_NAME}_parameters src/admittance_controller_parameters.yaml)
+target_link_libraries(${PROJECT_NAME} admittance_controller_parameters)
+ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# 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 "ADMITTANCE_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(control_msgs REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(controller_interface REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ## create custom test function to pass yaml file into test main
+  #function(add_test_with_yaml_input TARGET SOURCES YAML_FILE)
+    #add_executable(${TARGET} ${SOURCES})
+    #_ament_cmake_gmock_find_gmock()
+    #target_include_directories(${TARGET} PUBLIC "${GMOCK_INCLUDE_DIRS}")
+    #target_link_libraries(${TARGET} ${GMOCK_LIBRARIES})
+    #set(executable "$<TARGET_FILE:${TARGET}>")
+    #set(result_file "${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.gtest.xml")
+    #ament_add_test(
+      #${TARGET}
+      #COMMAND ${executable} --ros-args --params-file ${YAML_FILE}
+      #--gtest_output=xml:${result_file}
+      #OUTPUT_FILE ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TARGET}.txt
+      #RESULT_FILE ${result_file}
+    #)
+  #endfunction()
+
+  # test loading admittance controller
+  add_rostest_with_parameters_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml)
+  target_include_directories(test_load_admittance_controller PUBLIC ${GMOCK_INCLUDE_DIRS})
+  target_link_libraries(test_load_admittance_controller ${GMOCK_LIBRARIES})
+  ament_target_dependencies(
+    test_load_admittance_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+  # test admittance controller function
+  add_rostest_with_parameters_gmock(test_admittance_controller test/test_admittance_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml)
+  target_include_directories(test_admittance_controller PRIVATE include)
+  target_link_libraries(test_admittance_controller admittance_controller)
+  ament_target_dependencies(
+    test_admittance_controller
+    control_msgs
+    controller_interface
+    hardware_interface
+    ros2_control_test_assets
+  )
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_package()

admittance_controller/admittance_controller.xml

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

admittance_controller/doc/userdoc.rst

@@ -0,0 +1,58 @@
+.. _joint_trajectory_controller_userdoc:
+
+Admittance Controller
+======================
+
+Admittance controller enables you do zero-force control from a force measured on your TCP.
+The controller implements ``ChainedControllerInterface``, so it is possible to add another controllers in front of it, e.g., ``JointTrajectoryController``.
+
+The controller requires an external kinematics plugin to function. The `kinematics_interface <https://github.com/ros-controls/kinematics_interface>`_ repository provides an interface and an implementation that the admittance controller can use.
+
+Parameters:
+
+
+
+ROS2 interface of the controller
+---------------------------------
+
+Parameters
+^^^^^^^^^^^
+
+The admittance controller's 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/master/admittance_controller/src/admittance_controller_parameters.yaml>`_ contains descriptions for all the parameters used by the controller.
+An example parameter file can be found in the `test folder of the controller <https://github.com/ros-controls/ros2_controllers/blob/master/admittance_controller/test/test_params.yaml>`_
+
+
+Topics
+^^^^^^^
+
+~/joint_references (input topic) [trajectory_msgs::msg::JointTrajectoryPoint]
+  Target joint commands when controller is not in chained mode.
+
+~/state (output topic) [control_msgs::msg::AdmittanceControllerState]
+  Topic publishing internal states.
+
+
+ros2_control interfaces
+------------------------
+
+References
+^^^^^^^^^^^
+The controller has ``position`` and ``velocity`` reference interfaces exported in the format:
+``<controller_name>/<joint_name>/[position|velocity]``
+
+
+States
+^^^^^^^
+The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``.
+Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_.
+If some interface is not provided, the last commanded interface will be used for calculation.
+
+For handling TCP wrenches `*Force Torque Sensor* semantic component  (from package *controller_interface*) <https://github.com/ros-controls/ros2_control/blob/master/controller_interface/include/semantic_components/force_torque_sensor.hpp>`_ is used.
+The interfaces have prefix ``ft_sensor.name``, building the interfaces: ``<sensor_name>/[force.x|force.y|force.z|torque.x|torque.y|torque.z]``.
+
+
+Commands
+^^^^^^^^^
+The command interfaces are defined with ``joints`` and ``command_interfaces`` parameters as follows: ``<joint>/<command_interface>``.
+Supported state interfaces are ``position``, ``velocity``, and ``acceleration`` as defined in the `hardware_interface/hardware_interface_type_values.hpp <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp>`_.

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2022, PickNik, Inc.
+//
+// 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.
+//
+/// \authors: Denis Stogl, Andy Zelenak, Paul Gesel
+
+#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
+#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+// include generated parameter library
+#include "admittance_controller_parameters.hpp"
+
+#include "admittance_controller/admittance_rule.hpp"
+#include "admittance_controller/visibility_control.h"
+#include "control_msgs/msg/admittance_controller_state.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "semantic_components/force_torque_sensor.hpp"
+
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+namespace admittance_controller
+{
+using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState;
+
+class AdmittanceController : public controller_interface::ChainableControllerInterface
+{
+public:
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  /// Export configuration of required state interfaces.
+  /**
+   * Allowed types of state interfaces are \ref hardware_interface::POSITION,
+   * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION.
+   */
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  /// Export configuration of required state interfaces.
+  /**
+   * Allowed types of state interfaces are \ref hardware_interface::POSITION,
+   * \ref hardware_interface::VELOCITY, \ref hardware_interface::ACCELERATION.
+   */
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  controller_interface::return_type update_reference_from_subscribers() override;
+
+  size_t num_joints_ = 0;
+  std::vector<std::string> command_joint_names_;
+
+  // The interfaces are defined as the types in 'allowed_interface_types_' member.
+  // For convenience, for each type the interfaces are ordered so that i-th position
+  // matches i-th index in joint_names_
+  template <typename T>
+  using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
+
+  InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
+  InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
+
+  bool has_position_state_interface_ = false;
+  bool has_velocity_state_interface_ = false;
+  bool has_acceleration_state_interface_ = false;
+  bool has_position_command_interface_ = false;
+  bool has_velocity_command_interface_ = false;
+  bool has_acceleration_command_interface_ = false;
+  bool has_effort_command_interface_ = false;
+
+  // To reduce number of variables and to make the code shorter the interfaces are ordered in types
+  // as the following constants
+  const std::vector<std::string> allowed_interface_types_ = {
+    hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
+    hardware_interface::HW_IF_ACCELERATION};
+
+  // internal reference values
+  const std::vector<std::string> allowed_reference_interfaces_types_ = {
+    hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY};
+  std::vector<std::reference_wrapper<double>> position_reference_;
+  std::vector<std::reference_wrapper<double>> velocity_reference_;
+
+  // Admittance rule and dependent variables;
+  std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
+
+  // force torque sensor
+  std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
+
+  // ROS subscribers
+  rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr
+    input_joint_command_subscriber_;
+  rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_;
+
+  // admittance parameters
+  std::shared_ptr<admittance_controller::ParamListener> parameter_handler_;
+
+  // ROS messages
+  std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
+
+  // real-time buffer
+  realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
+    input_joint_command_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
+
+  trajectory_msgs::msg::JointTrajectoryPoint last_commanded_;
+  trajectory_msgs::msg::JointTrajectoryPoint last_reference_;
+
+  // control loop data
+  // reference_: reference value read by the controller
+  // joint_state_: current joint readings from the hardware
+  // reference_admittance_: reference value used by the controller after the admittance values are
+  // applied ft_values_: values read from the force torque sensor
+  trajectory_msgs::msg::JointTrajectoryPoint reference_, joint_state_, reference_admittance_;
+  geometry_msgs::msg::Wrench ft_values_;
+
+  /**
+  * @brief Read values from hardware interfaces and set corresponding fields of state_current and ft_values
+  */
+  void read_state_from_hardware(
+    trajectory_msgs::msg::JointTrajectoryPoint & state_current,
+    geometry_msgs::msg::Wrench & ft_values);
+
+  /**
+  * @brief Set fields of state_reference with values from controllers exported position and velocity references
+  */
+  void read_state_reference_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);
+
+  /**
+* @brief Write values from state_command to claimed hardware interfaces
+*/
+  void write_state_to_hardware(const trajectory_msgs::msg::JointTrajectoryPoint & state_command);
+};
+
+}  // namespace admittance_controller
+
+#endif  // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_