Add admittance controller

admittance_controller/CMakeLists.txt

@@ -0,0 +1,134 @@
+cmake_minimum_required(VERSION 3.5)
+project(admittance_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(control_toolbox REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(filters REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(joint_limits REQUIRED)
+find_package(moveit_ros_move_group REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_library(
+  admittance_controller
+  SHARED
+  src/admittance_controller.cpp
+  src/moveit_kinematics.cpp
+)
+target_include_directories(
+  admittance_controller
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  admittance_controller
+  control_msgs
+  control_toolbox
+  controller_interface
+  ${Eigen_LIBRARIES}
+  filters
+  geometry_msgs
+  hardware_interface
+  joint_limits
+  moveit_ros_move_group
+  moveit_ros_planning_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_ros
+)
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(admittance_controller PRIVATE "ADMITTANCE_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface admittance_controller.xml)
+
+install(
+  TARGETS admittance_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp)
+  target_include_directories(test_load_admittance_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_admittance_controller
+    controller_manager
+    hardware_interface
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_admittance_controller test/test_admittance_controller.cpp)
+  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_libraries(
+  admittance_controller
+)
+ament_export_dependencies(
+  control_msgs
+  control_toolbox
+  controller_interface
+  filters
+  geometry_msgs
+  hardware_interface
+  joint_limits
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_ros
+)
+
+ament_package()

admittance_controller/README.md

@@ -0,0 +1,7 @@
+admittance_controller
+==========================================
+
+Implemenation of admittance controllers for different input and output interface.
+
+Pluginlib-Library: admittance_controller
+Plugin: admittance_controller/AdmittanceController (controller_interface::ControllerInterface)

admittance_controller/admittance_controller.xml

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

admittance_controller/include/admittance_controller/admittance_controller.hpp

@@ -0,0 +1,160 @@
+// Copyright (c) 2021, 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
+
+#ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
+#define ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "admittance_controller/admittance_rule.hpp"
+#include "admittance_controller/visibility_control.h"
+#include "control_msgs/msg/admittance_controller_state.hpp"
+#include "controller_interface/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 "joint_limits/joint_limiter_interface.hpp"
+#include "joint_limits/joint_limits.hpp"
+#include "pluginlib/class_loader.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"
+// TODO(destogl): this is only temporary to work with servo. It should be either trajectory_msgs/msg/JointTrajectoryPoint or std_msgs/msg/Float64MultiArray
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+namespace admittance_controller
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class AdmittanceController : public controller_interface::ControllerInterface
+{
+public:
+  ADMITTANCE_CONTROLLER_PUBLIC
+  AdmittanceController();
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  CallbackReturn on_init() override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ADMITTANCE_CONTROLLER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::vector<std::string> joint_names_;
+  std::vector<std::string> command_interface_types_;
+  std::vector<std::string> state_interface_types_;
+  std::string ft_sensor_name_;
+  bool use_joint_commands_as_input_;
+  std::string joint_limiter_type_;
+
+  bool hardware_state_has_offset_;
+  trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
+
+  // joint limiter
+  using JointLimiter = joint_limits::JointLimiterInterface<joint_limits::JointLimits>;
+  std::shared_ptr<pluginlib::ClassLoader<JointLimiter>> joint_limiter_loader_;
+  std::unique_ptr<JointLimiter> joint_limiter_;
+
+  // Internal variables
+  std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
+
+  // Admittance rule and dependent variables;
+  std::unique_ptr<admittance_controller::AdmittanceRule> admittance_;
+  rclcpp::Time previous_time_;
+
+  // Callback for updating dynamic parameters
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_callback_handle_;
+
+  // Command subscribers and Controller State publisher
+  using ControllerCommandWrenchMsg = geometry_msgs::msg::WrenchStamped;
+  using ControllerCommandPoseMsg = geometry_msgs::msg::PoseStamped;
+  using ControllerCommandJointMsg = trajectory_msgs::msg::JointTrajectory;
+
+  rclcpp::Subscription<ControllerCommandWrenchMsg>::SharedPtr
+  input_wrench_command_subscriber_ = nullptr;
+  rclcpp::Subscription<ControllerCommandPoseMsg>::SharedPtr
+  input_pose_command_subscriber_ = nullptr;
+  rclcpp::Subscription<ControllerCommandJointMsg>::SharedPtr input_joint_command_subscriber_ = nullptr;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerCommandWrenchMsg>>
+  input_wrench_command_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerCommandPoseMsg>>
+  input_pose_command_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerCommandJointMsg>>
+  input_joint_command_;
+
+  using ControllerStateMsg = control_msgs::msg::AdmittanceControllerState;
+  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+  std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+  // Internal access to sorted interfaces
+
+  // 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,
+  };
+
+  // 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_velocity_state_interface_ = false;
+  bool has_position_command_interface_ = false;
+  bool has_velocity_command_interface_ = false;
+
+  void read_state_from_hardware(trajectory_msgs::msg::JointTrajectoryPoint & state);
+
+  /// Use values on command interfaces as states when robot should be controller in open-loop.
+  /**
+   * If velocities and positions are both available from the joint command interface, set
+   * output_state equal to them.
+   * If velocities or positions are unknown, output_state is unchanged and the function returns false.
+   */
+  bool read_state_from_command_interfaces(trajectory_msgs::msg::JointTrajectoryPoint & state);
+};
+
+}  // namespace admittance_controller
+
+#endif  // ADMITTANCE_CONTROLLER__ADMITTANCE_CONTROLLER_HPP_

admittance_controller/include/admittance_controller/admittance_controller_cartesian_commands.hpp

@@ -0,0 +1,41 @@
+
+// Header!!
+std::string ik_tip_frame_;
+
+// Frame which position should be controlled
+std::string endeffector_frame_;
+
+tf2::Transform ik_tip_to_control_frame_tf_;
+tf2::Transform control_frame_to_ik_tip_tf_;
+
+// CPP!!
+controller_interface::return_type AdmittanceRule::reset()
+{
+  // Initialize ik_tip and tool_frame transformations - those are fixed transformations
+  tf2::Stamped<tf2::Transform> tf2_transform;
+  try {
+    auto transform = tf_buffer_->lookupTransform(ik_tip_frame_, control_frame_, tf2::TimePointZero);
+    tf2::fromMsg(transform, tf2_transform);
+    ik_tip_to_control_frame_tf_ = tf2_transform;
+    control_frame_to_ik_tip_tf_ = tf2_transform.inverse();
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_SKIPFIRST_THROTTLE(
+      rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what());
+    return controller_interface::return_type::ERROR;
+  }
+}
+
+
+controller_interface::return_type AdmittanceRule::get_controller_state(
+  control_msgs::msg::AdmittanceControllerState & state_message)
+{
+  // FIXME(destogl): Something is wrong with this transformation - check frames...
+  try {
+    auto transform = tf_buffer_->lookupTransform(endeffector_frame_, control_frame_, tf2::TimePointZero);
+    tf2::doTransform(measured_wrench_ik_base_frame_, measured_wrench_endeffector_frame_, transform);
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_SKIPFIRST_THROTTLE(
+      rclcpp::get_logger("AdmittanceRule"), *clock_, 5000, "%s", e.what());
+  }
+  state_message.measured_wrench_endeffector_frame = measured_wrench_endeffector_frame_;
+}