Add admittance controller

admittance_controller/CMakeLists.txt

@@ -0,0 +1,120 @@
+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_test_with_yaml_input(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_test_with_yaml_input(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_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_package()

admittance_controller/README.md

@@ -0,0 +1,10 @@
+admittance_controller
+==========================================
+
+The admittance controller operates in one of two modes: chained and non-chained.
+In the non-chained mode, the controller listens to joint commands on the `~/joint_commands` topic to get a reference. In chained mode, the admittance controller receives its reference from another controller. In both modes, the controller applies the calculated admittance values to the received reference and write the modified reference to its command interfaces.
+
+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:
+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 [here](../admittance_controller/src/admittance_controller_parameters.yaml) contains descriptions for all the parameters used by the controller.

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/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_