added a twist controller

mhubii · mhubii · commit ffe3fac3dfd3 · 2024-10-20T17:09:48.000+01:00
diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py
@@ -35,6 +35,7 @@ def arg_ctrl() -> DeclareLaunchArgument:
                 "lbr_joint_position_command_controller",
                 "lbr_torque_command_controller",
                 "lbr_wrench_command_controller",
+                "twist_controller",
             ],
         )
 
diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp
@@ -44,7 +44,7 @@ class FTEstimator {
   cart_array_t f_ext_th_;
 
   // kinematics
-  std::unique_ptr<Kinematics> kinematics_;
+  std::unique_ptr<Kinematics> kinematics_ptr_;
 
   // force estimation
   Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, Kinematics::CARTESIAN_DOF>
diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp
@@ -4,7 +4,7 @@ namespace lbr_fri_ros2 {
 FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root,
                          const std::string &chain_tip, const_cart_array_t_ref f_ext_th)
     : f_ext_th_(f_ext_th) {
-  kinematics_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip);
+  kinematics_ptr_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip);
   reset();
 }
 
@@ -13,12 +13,12 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
                           const double &damping) {
   tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
       external_torque.data());
-  auto jacobian = kinematics_->compute_jacobian(measured_joint_position);
+  auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position);
   jacobian_inv_ = pinv(jacobian.data, damping);
   f_ext_ = jacobian_inv_.transpose() * tau_ext_;
 
   // rotate into chain tip frame
-  auto chain_tip_frame = kinematics_->compute_fk(measured_joint_position);
+  auto chain_tip_frame = kinematics_ptr_->compute_fk(measured_joint_position);
   f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
   f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3);
 
diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt
@@ -16,11 +16,13 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3)
 find_package(FRIClient REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(kinematics_interface REQUIRED)
 find_package(lbr_fri_idl 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)
@@ -33,6 +35,7 @@ add_library(
   src/controllers/lbr_torque_command_controller.cpp
   src/controllers/lbr_wrench_command_controller.cpp
   src/controllers/lbr_state_broadcaster.cpp
+  src/controllers/twist_controller.cpp
   src/system_interface.cpp
 )
 
@@ -47,16 +50,21 @@ target_include_directories(
 )
 
 # Link against dependencies
-ament_target_dependencies(
-  ${PROJECT_NAME}
+set(AMENT_DEPENDENCIES
   controller_interface
+  Eigen3
   hardware_interface
+  kinematics_interface
   lbr_fri_idl
   lbr_fri_ros2
   pluginlib
   rclcpp
   realtime_tools
 )
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  ${AMENT_DEPENDENCIES}
+)
 
 target_link_libraries(${PROJECT_NAME}
   FRIClient::FRIClient
@@ -71,16 +79,11 @@ ament_export_targets(
 )
 
 ament_export_dependencies(
-  controller_interface
   FRIClient
-  hardware_interface
-  lbr_fri_idl
-  lbr_fri_ros2
-  pluginlib
-  rclcpp
-  realtime_tools
+  ${AMENT_DEPENDENCIES}
+  eigen3_cmake_module
 )
-  
+
 install(
   DIRECTORY include/
   DESTINATION include
diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml
@@ -23,6 +23,9 @@
       type: position_controllers/JointGroupPositionController
 
     # LBR ROS 2 control controllers
+    admittance_controller:
+      type: lbr_ros2_control/AdmittanceController
+
     lbr_joint_position_command_controller:
       type: lbr_ros2_control/LBRJointPositionCommandController
 
@@ -32,8 +35,8 @@
     lbr_wrench_command_controller:
       type: lbr_ros2_control/LBRWrenchCommandController
 
-    admittance_controller:
-      type: lbr_ros2_control/AdmittanceController
+    twist_controller:
+      type: lbr_ros2_control/TwistController
 
 # ROS 2 control broadcasters
 /**/force_torque_broadcaster:
@@ -89,3 +92,12 @@
 /**/lbr_wrench_command_controller:
   ros__parameters:
     robot_name: lbr
+
+/**/twist_controller:
+  ros__parameters:
+    robot_name: lbr
+    chain_root: lbr_link_0
+    chain_tip: lbr_link_ee
+    damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian
+    max_linear_velocity: 0.1 # maximum linear velocity
+    max_angular_velocity: 0.1 # maximum linear acceleration
diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp
@@ -0,0 +1,101 @@
+#ifndef LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_
+#define LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "eigen3/Eigen/Core"
+#include "geometry_msgs/msg/twist.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_buffer.h"
+
+#include "friLBRState.h"
+
+#include "lbr_fri_ros2/kinematics.hpp"
+#include "lbr_fri_ros2/pinv.hpp"
+#include "lbr_ros2_control/system_interface_type_values.hpp"
+
+namespace lbr_ros2_control {
+struct TwistParameters {
+  std::string chain_root;
+  std::string chain_tip;
+  double damping;
+  double max_linear_velocity;
+  double max_angular_velocity;
+};
+
+class TwistImpl {
+public:
+  TwistImpl(const std::string &robot_description, const TwistParameters &parameters);
+
+  void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target,
+               lbr_fri_ros2::Kinematics::const_jnt_pos_array_t_ref q,
+               lbr_fri_ros2::Kinematics::jnt_pos_array_t &dq);
+
+protected:
+  TwistParameters parameters_;
+
+  lbr_fri_ros2::Kinematics::jnt_pos_array_t q_;
+  std::unique_ptr<lbr_fri_ros2::Kinematics> kinematics_ptr_;
+  Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS,
+                lbr_fri_ros2::Kinematics::CARTESIAN_DOF>
+      jacobian_inv_;
+  Eigen::Matrix<double, lbr_fri_ros2::Kinematics::CARTESIAN_DOF, 1> twist_target_;
+};
+
+class TwistController : public controller_interface::ControllerInterface {
+public:
+  TwistController();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::return_type update(const rclcpp::Time &time,
+                                           const rclcpp::Duration &period) override;
+
+  controller_interface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+protected:
+  bool reference_state_interfaces_();
+  void clear_state_interfaces_();
+  void reset_command_buffer_();
+  void configure_joint_names_();
+  void configure_twist_impl_();
+
+  // joint veloctiy computation
+  std::unique_ptr<TwistImpl> twist_impl_ptr_;
+  lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_;
+
+  // interfaces
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_;
+  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      joint_position_state_interfaces_;
+  std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      sample_time_state_interface_;
+  std::unique_ptr<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+      session_state_interface_;
+
+  // real-time twist command topic
+  realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist::SharedPtr> rt_twist_ptr_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_subscription_ptr_;
+};
+} // namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__LBR_TWIST_CONTROLLER_HPP_
diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml
@@ -8,18 +8,25 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
 
   <depend>fri_client_sdk</depend>
+  <depend>geometry_msgs</depend>
   <depend>lbr_fri_idl</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>realtime_tools</depend>
   <depend>ros2_control</depend>
-
+  
   <exec_depend>lbr_description</exec_depend>
   <exec_depend>ros2_controllers</exec_depend>
 
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+  <build_export_depend>eigen</build_export_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/lbr_ros2_control/plugin_description_files/controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml
@@ -1,9 +1,10 @@
 <?xml version="1.0"?>
 <library path="lbr_ros2_control">
-    <!-- LBR state broadcaster plugin -->
-    <class name="lbr_ros2_control/LBRStateBroadcaster" type="lbr_ros2_control::LBRStateBroadcaster"
+    <!-- Admittance controller plugin -->
+    <class name="lbr_ros2_control/AdmittanceController"
+        type="lbr_ros2_control::AdmittanceController"
         base_class_type="controller_interface::ControllerInterface">
-        <description>Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg.</description>
+        <description>A simple admittance controller.</description>
     </class>
 
     <!-- LBR forward position command controller plugin -->
@@ -14,6 +15,12 @@
             lbr_fri_idl/msg/LBRJointPositionCommand.msg.</description>
     </class>
 
+    <!-- LBR state broadcaster plugin -->
+    <class name="lbr_ros2_control/LBRStateBroadcaster" type="lbr_ros2_control::LBRStateBroadcaster"
+        base_class_type="controller_interface::ControllerInterface">
+        <description>Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg.</description>
+    </class>
+
     <!-- LBR forward torque command controller plugin -->
     <class name="lbr_ros2_control/LBRTorqueCommandController"
         type="lbr_ros2_control::LBRTorqueCommandController"
@@ -30,10 +37,10 @@
             lbr_fri_idl/msg/LBRWrenchCommand.msg.</description>
     </class>
 
-    <!-- Admittance controller plugin -->
-    <class name="lbr_ros2_control/AdmittanceController"
-        type="lbr_ros2_control::AdmittanceController"
+    <!-- Twist controller plugin -->
+    <class name="lbr_ros2_control/TwistController"
+        type="lbr_ros2_control::TwistController"
         base_class_type="controller_interface::ControllerInterface">
-        <description>A simple admittance controller.</description>
+        <description>A simple twist controller.</description>
     </class>
 </library>
diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp

Original file line number	Diff line number	Diff line change
`@@ -35,6 +35,7 @@ def arg_ctrl() -> DeclareLaunchArgument:`
`35`	`35`	`"lbr_joint_position_command_controller",`
`36`	`36`	`"lbr_torque_command_controller",`
`37`	`37`	`"lbr_wrench_command_controller",`
	`38`	`+ "twist_controller",`
`38`	`39`	`],`
`39`	`40`	`)`
`40`	`41`