diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index b8d6bffb..e45a5fb9 100644 --- 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 index 48f1b04e..816bdd77 100644 --- 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_; + std::unique_ptr kinematics_ptr_; // force estimation Eigen::Matrix diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index c8378892..93ec4037 100644 --- 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(robot_description, chain_root, chain_tip); + kinematics_ptr_ = std::make_unique(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>( 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 index 6dc55918..cf019396 100644 --- 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 index 35878f5d..07052fbe 100644 --- 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 new file mode 100644 index 00000000..ed364a25 --- /dev/null +++ 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 +#include +#include +#include +#include +#include +#include + +#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 ¶meters); + + 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 kinematics_ptr_; + Eigen::Matrix + jacobian_inv_; + Eigen::Matrix 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 twist_impl_ptr_; + lbr_fri_ros2::Kinematics::jnt_pos_array_t q_, dq_; + + // interfaces + std::array joint_names_; + std::vector> + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_; + std::unique_ptr> + session_state_interface_; + + // real-time twist command topic + realtime_tools::RealtimeBuffer rt_twist_ptr_; + rclcpp::Subscription::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 index f6f5b81a..932c667c 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -8,18 +8,25 @@ Apache-2.0 ament_cmake + eigen3_cmake_module + + eigen fri_client_sdk + geometry_msgs lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp realtime_tools ros2_control - + lbr_description ros2_controllers + eigen3_cmake_module + eigen + ament_cmake diff --git a/lbr_ros2_control/plugin_description_files/controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml index 82c4ae8e..10694639 100644 --- a/lbr_ros2_control/plugin_description_files/controllers.xml +++ b/lbr_ros2_control/plugin_description_files/controllers.xml @@ -1,9 +1,10 @@ - - + - Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + A simple admittance controller. @@ -14,6 +15,12 @@ lbr_fri_idl/msg/LBRJointPositionCommand.msg. + + + Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + + - - + - A simple admittance controller. + A simple twist controller. \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp new file mode 100644 index 00000000..e01d03e7 --- /dev/null +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -0,0 +1,203 @@ +#include "lbr_ros2_control/controllers/twist_controller.hpp" + +namespace lbr_ros2_control { +TwistImpl::TwistImpl(const std::string &robot_description, const TwistParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique( + robot_description, parameters_.chain_root, parameters_.chain_tip); +} + +void TwistImpl::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) { + // twist to Eigen + twist_target_[0] = twist_target->linear.x; + twist_target_[1] = twist_target->linear.y; + twist_target_[2] = twist_target->linear.z; + twist_target_[3] = twist_target->angular.x; + twist_target_[4] = twist_target->angular.y; + twist_target_[5] = twist_target->angular.z; + + // clip velocity + twist_target_.head(3).unaryExpr([&](double v) { + return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity); + }); + twist_target_.tail(3).unaryExpr([&](double v) { + return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); + }); + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = + jacobian_inv_ * twist_target_; +} + +TwistController::TwistController() : rt_twist_ptr_(nullptr), twist_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +TwistController::command_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 + "/" + hardware_interface::HW_IF_POSITION); + } + return interface_configuration; +} + +controller_interface::InterfaceConfiguration +TwistController::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 + "/" + hardware_interface::HW_IF_POSITION); + } + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SAMPLE_TIME); + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SESSION_STATE); + return interface_configuration; +} + +controller_interface::CallbackReturn TwistController::on_init() { + try { + twist_subscription_ptr_ = this->get_node()->create_subscription( + "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { + rt_twist_ptr_.writeFromNonRT(msg); + }); + this->get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("damping", 0.2); + this->get_node()->declare_parameter("max_linear_velocity", 0.1); + this->get_node()->declare_parameter("max_angular_velocity", 0.1); + configure_joint_names_(); + configure_twist_impl_(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + auto twist_command = rt_twist_ptr_.readFromRT(); + if (!twist_command || !(*twist_command)) { + return controller_interface::return_type::OK; + } + if (!twist_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Twist controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + + // pass joint positions to q_ + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute the joint velocity from the twist command target + twist_impl_ptr_->compute(*twist_command, q_, dq_); + + // pass joint positions to hardware + std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { + this->command_interfaces_[i].set_value( + q_i + dq_[i] * sample_time_state_interface_->get().get_value()); + ++i; + }); + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!reference_state_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + clear_state_interfaces_(); + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TwistController::reference_state_interfaces_() { + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { + sample_time_state_interface_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ = + std::make_unique>( + std::ref(state_interface)); + } + } + if (joint_position_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint position state interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", + joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return false; + } + return true; +} + +void TwistController::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); } + +void TwistController::reset_command_buffer_() { + rt_twist_ptr_ = + realtime_tools::RealtimeBuffer>(nullptr); +}; + +void TwistController::configure_joint_names_() { + if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} + +void TwistController::configure_twist_impl_() { + twist_impl_ptr_ = std::make_unique( + this->get_robot_description(), + TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), + this->get_node()->get_parameter("chain_tip").as_string(), + this->get_node()->get_parameter("damping").as_double(), + this->get_node()->get_parameter("max_linear_velocity").as_double(), + this->get_node()->get_parameter("max_angular_velocity").as_double()}); +} +} // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::TwistController, controller_interface::ControllerInterface)