added initial vft implementation

lbr_ros2_control/CMakeLists.txt

@@ -17,12 +17,16 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
 find_package(fri_vendor REQUIRED)
 find_package(FRIClient REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(kinematics_interface_kdl REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(lbr_fri_ros2 REQUIRED)
+find_package(kinematics_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(realtime_tools REQUIRED)
@@ -49,9 +53,11 @@ target_include_directories(
 ament_target_dependencies(
   ${PROJECT_NAME}
   controller_interface
+  Eigen3
   fri_vendor
   geometry_msgs
   hardware_interface
+  kinematics_interface_kdl
   lbr_fri_msgs
   lbr_fri_ros2
   pluginlib
@@ -73,10 +79,13 @@ ament_export_targets(
 
 ament_export_dependencies(
   controller_interface
+  eigen3_cmake_module
+  Eigen3
   fri_vendor
   FRIClient
   geometry_msgs
   hardware_interface
+  kinematics_interface_kdl
   lbr_fri_msgs
   lbr_fri_ros2
   pluginlib

lbr_ros2_control/config/lbr_controllers.yaml

@@ -11,6 +11,9 @@
     lbr_state_broadcaster:
       type: lbr_ros2_control/LBRStateBroadcaster
 
+    lbr_virtual_ft_broadcaster:
+      type: lbr_ros2_control/LBRVirtualFTBroadcaster
+
     # Controllers
     position_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController

lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp

@@ -4,14 +4,20 @@
 #include <limits>
 #include <memory>
 #include <stdexcept>
+#include <vector>
 
 #include "controller_interface/controller_interface.hpp"
+#include "eigen3/Eigen/Core"
+#include "eigen3/Eigen/SVD"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "kinematics_interface_kdl/kinematics_interface_kdl.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "realtime_tools/realtime_publisher.h"
 
 #include "friClientIf.h"
+#include "friLBRState.h"
 
 #include "lbr_ros2_control/lbr_system_interface_type_values.hpp"
 
@@ -39,6 +45,27 @@ class LBRVirtualFTBroadcaster : public controller_interface::ControllerInterface
   on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
 
 protected:
+  void init_state_();
+
+  template <class MatT>
+  Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
+  damped_least_squares_(const MatT &mat,
+                        typename MatT::Scalar lambda = typename MatT::Scalar{2e-1});
+
+  kinematics_interface_kdl::KinematicsInterfaceKDL kinematics_interface_kdl_;
+
+  Eigen::Matrix<double, 6, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> jacobian_;
+  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 6> jacobian_pinv_;
+  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1> joint_positions_,
+      external_joint_torques_;
+  Eigen::Matrix<double, 6, 1> virtual_ft_;
+
+  std::string link_name_ = "link_ee";
+  std::array<std::string, 7> joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      joint_position_interfaces_, external_joint_torque_interfaces_;
+
   rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_stamped_publisher_ptr_;
   std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>>
       rt_wrench_stamped_publisher_ptr_;

lbr_ros2_control/launch/system_interface.launch.py

@@ -31,13 +31,21 @@ def generate_launch_description() -> LaunchDescription:
     lbr_state_broadcaster = LBRSystemInterface.node_controller_spawner(
         controller="lbr_state_broadcaster"
     )
+    lbr_virtual_ft_broadcast = LBRSystemInterface.node_controller_spawner(
+        controller="lbr_virtual_ft_broadcaster"
+    )
     controller = LBRSystemInterface.node_controller_spawner(
         controller=LaunchConfiguration("ctrl")
     )
     controller_event_handler = RegisterEventHandler(
         OnProcessStart(
             target_action=ros2_control_node,
-            on_start=[joint_state_broadcaster, lbr_state_broadcaster, controller],
+            on_start=[
+                joint_state_broadcaster,
+                lbr_state_broadcaster,
+                lbr_virtual_ft_broadcast,
+                controller,
+            ],
         )
     )
     ld.add_action(controller_event_handler)

lbr_ros2_control/package.xml

@@ -9,9 +9,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>eigen</depend>
   <depend>fri_vendor</depend>
   <depend>geometry_msgs</depend>
+  <depend>kinematics_interface_kdl</depend>
   <depend>lbr_fri_msgs</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>pluginlib</depend>

lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp

@@ -2,22 +2,118 @@
 
 namespace lbr_ros2_control {
 controller_interface::InterfaceConfiguration
-LBRVirtualFTBroadcaster::command_interface_configuration() const override;
+LBRVirtualFTBroadcaster::command_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+}
 
 controller_interface::InterfaceConfiguration
-LBRVirtualFTBroadcaster::state_interface_configuration() const override;
+LBRVirtualFTBroadcaster::state_interface_configuration() const {
+  controller_interface::InterfaceConfiguration interface_configuration;
+  interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto &joint_name : joint_names_) {
+    interface_configuration.names.push_back(joint_name + "/" + HW_IF_POSITION);
+    interface_configuration.names.push_back(joint_name + "/" + HW_IF_EXTERNAL_TORQUE);
+  }
+  return interface_configuration;
+}
 
-controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() override;
+controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() {
+  try {
+    wrench_stamped_publisher_ptr_ =
+        this->get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
+            "wrench", rclcpp::SensorDataQoS());
+    rt_wrench_stamped_publisher_ptr_ =
+        std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>>(
+            wrench_stamped_publisher_ptr_);
+    kinematics_interface_kdl_.initialize(this->get_node(), link_name_);
+    for (const auto &state_interface : state_interfaces_) {
+      if (state_interface.get_interface_name() == HW_IF_POSITION) {
+        joint_position_interfaces_.emplace_back(std::ref(state_interface));
+      }
+      if (state_interface.get_interface_name() == HW_IF_EXTERNAL_TORQUE) {
+        external_joint_torque_interfaces_.emplace_back(std::ref(state_interface));
+      }
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR(this->get_node()->get_logger(),
+                 "Failed to initialize LBR virtual FT broadcaster with: %s.", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  return controller_interface::CallbackReturn::SUCCESS;
+}
 
 controller_interface::return_type
-LBRVirtualFTBroadcaster::update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+LBRVirtualFTBroadcaster::update(const rclcpp::Time & /*time*/,
+                                const rclcpp::Duration & /*period*/) {
+  for (std::size_t i = 0; i < joint_names_.size(); ++i) {
+    joint_positions_(i) = joint_position_interfaces_[i].get().get_value();
+    external_joint_torques_(i) = external_joint_torque_interfaces_[i].get().get_value();
+  }
+  // compute virtual FT given Jacobian and external joint torques
+  kinematics_interface_kdl_.calculate_jacobian(joint_positions_, link_name_, jacobian_);
+  jacobian_pinv_ = damped_least_squares_(jacobian_);
+  virtual_ft_ = jacobian_pinv_.transpose() * external_joint_torques_;
+
+  // publish
+  if (rt_wrench_stamped_publisher_ptr_->trylock()) {
+    rt_wrench_stamped_publisher_ptr_->msg_.header.stamp = this->get_node()->now();
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.x = virtual_ft_(0);
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = virtual_ft_(1);
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = virtual_ft_(2);
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = virtual_ft_(3);
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = virtual_ft_(4);
+    rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5);
+    rt_wrench_stamped_publisher_ptr_->unlockAndPublish();
+  }
+}
 
 controller_interface::CallbackReturn
-LBRVirtualFTBroadcaster::on_configure(const rclcpp_lifecycle::State &previous_state) override;
+LBRVirtualFTBroadcaster::on_configure(const rclcpp_lifecycle::State &previous_state) {
+  return controller_interface::CallbackReturn::SUCCESS;
+}
 
 controller_interface::CallbackReturn
-LBRVirtualFTBroadcaster::on_activate(const rclcpp_lifecycle::State &previous_state) override;
+LBRVirtualFTBroadcaster::on_activate(const rclcpp_lifecycle::State &previous_state) {
+  init_state_();
+  return controller_interface::CallbackReturn::SUCCESS;
+}
 
 controller_interface::CallbackReturn
-LBRVirtualFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+LBRVirtualFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State &previous_state) {
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+void LBRVirtualFTBroadcaster::init_state_() {
+  jacobian_.setConstant(std::numeric_limits<double>::quiet_NaN());
+  jacobian_pinv_.setConstant(std::numeric_limits<double>::quiet_NaN());
+  joint_positions_.setConstant(std::numeric_limits<double>::quiet_NaN());
+  external_joint_torques_.setConstant(std::numeric_limits<double>::quiet_NaN());
+  virtual_ft_.setConstant(std::numeric_limits<double>::quiet_NaN());
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.x = virtual_ft_(0);
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.y = virtual_ft_(1);
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.force.z = virtual_ft_(2);
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.x = virtual_ft_(3);
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.y = virtual_ft_(4);
+  rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5);
+}
+
+template <class MatT>
+Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
+LBRVirtualFTBroadcaster::damped_least_squares_(
+    const MatT &mat,
+    typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately
+{
+  typedef typename MatT::Scalar Scalar;
+  auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const auto &singularValues = svd.singularValues();
+  Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> dampedSingularValuesInv(
+      mat.cols(), mat.rows());
+  dampedSingularValuesInv.setZero();
+  for (unsigned int i = 0; i < singularValues.size(); ++i) {
+    dampedSingularValuesInv(i, i) =
+        singularValues(i) / (singularValues(i) * singularValues(i) + lambda * lambda);
+  }
+  return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint();
+}
 } // end of namespace lbr_ros2_control