From 8b55297f840a8819601861de0c961f835d14aa5f Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 20 Nov 2023 20:03:46 +0000 Subject: [PATCH] added vft implementation --- lbr_ros2_control/CMakeLists.txt | 3 +- .../lbr_virtual_ft_broadcaster.hpp | 9 ++--- ...r_controllers.xml => lbr_broadcasters.xml} | 0 .../src/lbr_virtual_ft_broadcaster.cpp | 35 ++++++++++++------- 4 files changed, 29 insertions(+), 18 deletions(-) rename lbr_ros2_control/{lbr_controllers.xml => lbr_broadcasters.xml} (100%) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 18aa9124..62fec73f 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -37,6 +37,7 @@ add_library( SHARED src/lbr_state_broadcaster.cpp src/lbr_system_interface.cpp + src/lbr_virtual_ft_broadcaster.cpp ) # Add include directories @@ -69,7 +70,7 @@ target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient ) -pluginlib_export_plugin_description_file(controller_interface lbr_controllers.xml) +pluginlib_export_plugin_description_file(controller_interface lbr_broadcasters.xml) pluginlib_export_plugin_description_file(hardware_interface lbr_system_interface.xml) # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp index 574b964f..19f5acbb 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp @@ -1,6 +1,7 @@ #ifndef LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ #define LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_ +#include #include #include #include @@ -24,7 +25,7 @@ namespace lbr_ros2_control { class LBRVirtualFTBroadcaster : public controller_interface::ControllerInterface { public: - LBRVirtualFTBroadcaster() = default; + LBRVirtualFTBroadcaster(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -54,13 +55,13 @@ class LBRVirtualFTBroadcaster : public controller_interface::ControllerInterface kinematics_interface_kdl::KinematicsInterfaceKDL kinematics_interface_kdl_; - Eigen::Matrix jacobian_; - Eigen::Matrix jacobian_pinv_; + Eigen::Matrix jacobian_; + Eigen::Matrix jacobian_pinv_; Eigen::Matrix joint_positions_, external_joint_torques_; Eigen::Matrix virtual_ft_; - std::string link_name_ = "link_ee"; + std::string end_effector_link_ = "link_ee"; std::array joint_names_ = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; std::vector> diff --git a/lbr_ros2_control/lbr_controllers.xml b/lbr_ros2_control/lbr_broadcasters.xml similarity index 100% rename from lbr_ros2_control/lbr_controllers.xml rename to lbr_ros2_control/lbr_broadcasters.xml diff --git a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp index 91f35e60..806614aa 100644 --- a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp @@ -1,6 +1,10 @@ #include "lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp" namespace lbr_ros2_control { +LBRVirtualFTBroadcaster::LBRVirtualFTBroadcaster() + : jacobian_(6, KUKA::FRI::LBRState::NUMBER_OF_JOINTS), + jacobian_pinv_(KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 6) {} + controller_interface::InterfaceConfiguration LBRVirtualFTBroadcaster::command_interface_configuration() const { return controller_interface::InterfaceConfiguration{ @@ -12,7 +16,7 @@ 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 + "/" + hardware_interface::HW_IF_POSITION); interface_configuration.names.push_back(joint_name + "/" + HW_IF_EXTERNAL_TORQUE); } return interface_configuration; @@ -26,9 +30,10 @@ controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() { rt_wrench_stamped_publisher_ptr_ = std::make_shared>( 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) { + kinematics_interface_kdl_.initialize(this->get_node()->get_node_parameters_interface(), + end_effector_link_); + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { joint_position_interfaces_.emplace_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_EXTERNAL_TORQUE) { @@ -51,10 +56,9 @@ LBRVirtualFTBroadcaster::update(const rclcpp::Time & /*time*/, 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_); + kinematics_interface_kdl_.calculate_jacobian(joint_positions_, end_effector_link_, 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(); @@ -66,21 +70,22 @@ LBRVirtualFTBroadcaster::update(const rclcpp::Time & /*time*/, rt_wrench_stamped_publisher_ptr_->msg_.wrench.torque.z = virtual_ft_(5); rt_wrench_stamped_publisher_ptr_->unlockAndPublish(); } + return controller_interface::return_type::OK; } controller_interface::CallbackReturn -LBRVirtualFTBroadcaster::on_configure(const rclcpp_lifecycle::State &previous_state) { +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) { +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) { +LBRVirtualFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } @@ -100,9 +105,8 @@ void LBRVirtualFTBroadcaster::init_state_() { template Eigen::Matrix -LBRVirtualFTBroadcaster::damped_least_squares_( - const MatT &mat, - typename MatT::Scalar lambda = typename MatT::Scalar{2e-1}) // choose appropriately +LBRVirtualFTBroadcaster::damped_least_squares_(const MatT &mat, + typename MatT::Scalar lambda) // choose appropriately { typedef typename MatT::Scalar Scalar; auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -116,4 +120,9 @@ LBRVirtualFTBroadcaster::damped_least_squares_( } return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); } -} // end of namespace lbr_ros2_control \ No newline at end of file +} // end of namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRVirtualFTBroadcaster, + controller_interface::ControllerInterface)