diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index b55b0aef..6e110244 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -19,6 +19,9 @@ hardware: open_loop: true # KUKA works the best in open_loop control mode estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation chain_root: lbr_link_0 chain_tip: lbr_link_ee damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index d078c1a9..5a31f0af 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -61,6 +61,9 @@ + ${system_config['estimated_ft_sensor']['enabled']} + ${system_config['estimated_ft_sensor']['update_rate']} + ${system_config['estimated_ft_sensor']['rt_prio']} ${system_config['estimated_ft_sensor']['chain_root']} ${system_config['estimated_ft_sensor']['chain_tip']} ${system_config['estimated_ft_sensor']['damping']} @@ -70,12 +73,14 @@ ${system_config['estimated_ft_sensor']['torque_x_th']} ${system_config['estimated_ft_sensor']['torque_y_th']} ${system_config['estimated_ft_sensor']['torque_z_th']} - - - - - - + + + + + + + + @@ -114,7 +119,8 @@ ${max_position} ${max_velocity} ${max_torque} - + diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 6e2f3f52..aea97da5 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -2,6 +2,7 @@ #define LBR_FRI_ROS2__APP_HPP_ #include +#include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" @@ -22,9 +23,6 @@ class App : public Worker { * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). * */ -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; - public: App(const std::shared_ptr async_client_ptr); ~App(); @@ -33,6 +31,8 @@ class App : public Worker { bool close_udp_socket(); void run_async(int rt_prio = 80) override; + inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::App"; }; + protected: void perform_work_() override; bool valid_port_(const int &port_id); 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 aa53d109..d7b93a0b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "eigen3/Eigen/Core" #include "rclcpp/logger.hpp" @@ -17,31 +18,74 @@ #include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" #include "lbr_fri_ros2/types.hpp" +#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class FTEstimator { +class FTEstimatorImpl { + /** + * @brief A class to estimate force-torques from external joint torque readings. Note that only + * forces beyond a specified threshold are returned. The specified threshold is removed from the + * estimated force-torque. + * + */ protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimatorImpl"; public: - FTEstimator(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", - const std::string &chain_tip = "lbr_link_ee", - const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}); - void compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque, - cart_array_t_ref f_ext, const double &damping = 0.2); + FTEstimatorImpl(const std::string &robot_description, + const std::string &chain_root = "lbr_link_0", + const std::string &chain_tip = "lbr_link_ee", + const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, + const double &damping = 0.2); + void compute(); void reset(); + inline void get_f_ext(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_; + } + inline void get_f_ext_tf(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_tf_; + } + inline void set_tau_ext(const_jnt_array_t_ref tau_ext) { + tau_ext_ = Eigen::Map>(tau_ext.data()); + } + inline void set_q(const_jnt_array_t_ref q) { q_ = q; } + protected: // force threshold cart_array_t f_ext_th_; + // damping for pseudo-inverse of Jacobian + double damping_; + + // joint positions and external joint torques + jnt_array_t q_; + // kinematics std::unique_ptr kinematics_ptr_; // force estimation Eigen::Matrix jacobian_inv_; Eigen::Matrix tau_ext_; - Eigen::Matrix f_ext_; + Eigen::Matrix f_ext_raw_, f_ext_, f_ext_tf_; +}; + +class FTEstimator : public Worker { + /** + * @brief A simple class to run the FTEstimatorImpl asynchronously at a specified update rate. + * + */ +public: + FTEstimator(const std::shared_ptr ft_estimator, + const std::uint16_t &update_rate = 100); + + inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::FTEstimator"; }; + +protected: + void perform_work_() override; + + std::shared_ptr ft_estimator_impl_ptr_; + std::uint16_t update_rate_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp index d1ff65ac..1eb42b8f 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -2,6 +2,7 @@ #define LBR_FRI_ROS2__WORKER_HPP_ #include +#include #include #include "rclcpp/logger.hpp" @@ -12,15 +13,13 @@ namespace lbr_fri_ros2 { class Worker { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Worker"; - public: Worker(); ~Worker(); virtual void run_async(int rt_prio = 80); void request_stop(); + inline virtual std::string LOGGER_NAME() const = 0; protected: virtual void perform_work_() = 0; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 45559466..9be75fe8 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -15,70 +15,70 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Opening UDP socket with port_id '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already open"); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Failed to open socket" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket opened successfully" << ColorScheme::ENDC); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } while (running_) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Waiting for run thread termination"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Waiting for run thread termination"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already closed"); return true; } connection_ptr_->close(); - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket closed successfully" << ColorScheme::ENDC); return true; } void App::run_async(int rt_prio) { if (!async_client_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not open" << ColorScheme::ENDC); return; } if (!app_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } @@ -95,7 +95,7 @@ void App::perform_work_() { // may never return running_ = true; if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "LBR in session state idle, exiting"); break; } } @@ -104,7 +104,7 @@ void App::perform_work_() { bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 8f6a9d73..e846ba6f 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -1,41 +1,54 @@ #include "lbr_fri_ros2/ft_estimator.hpp" 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) { +FTEstimatorImpl::FTEstimatorImpl(const std::string &robot_description, + const std::string &chain_root, const std::string &chain_tip, + const_cart_array_t_ref f_ext_th, const double &damping) + : f_ext_th_(f_ext_th), damping_(damping) { kinematics_ptr_ = std::make_unique(robot_description, chain_root, chain_tip); reset(); } -void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, - const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, - const double &damping) { - tau_ext_ = Eigen::Map>(external_torque.data()); - auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); - jacobian_inv_ = pinv(jacobian.data, damping); - f_ext_ = jacobian_inv_.transpose() * tau_ext_; +void FTEstimatorImpl::compute() { + auto jacobian = kinematics_ptr_->compute_jacobian(q_); + jacobian_inv_ = pinv(jacobian.data, damping_); + f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; + int i = -1; + f_ext_ = f_ext_raw_.unaryExpr([&](double v) { + ++i; + if (std::abs(v) < f_ext_th_[i]) { + return 0.; + } else { + return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); + } + }); // rotate into chain tip frame - 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); - - Eigen::Map>(f_ext.data()) = f_ext_; - - // threshold force-torque - std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), - [](const double &f_ext_i, const double &f_ext_th_i) { - if (std::abs(f_ext_i) < f_ext_th_i) { - return 0.; - } else { - return std::copysign(1., f_ext_i) * (std::abs(f_ext_i) - f_ext_th_i); - } - }); + auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); + f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); + f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); } -void FTEstimator::reset() { +void FTEstimatorImpl::reset() { + std::for_each(q_.begin(), q_.end(), [](double &q_i) { q_i = 0.; }); tau_ext_.setZero(); + f_ext_raw_.setZero(); f_ext_.setZero(); + f_ext_tf_.setZero(); + jacobian_inv_.setZero(); } + +FTEstimator::FTEstimator(const std::shared_ptr ft_estimator_impl_ptr, + const std::uint16_t &update_rate) + : ft_estimator_impl_ptr_(ft_estimator_impl_ptr), update_rate_(update_rate) {} + +void FTEstimator::perform_work_() { + running_ = true; + while (rclcpp::ok() && !should_stop_) { + auto start = std::chrono::high_resolution_clock::now(); + ft_estimator_impl_ptr_->compute(); + std::this_thread::sleep_until(start + std::chrono::nanoseconds(static_cast( + 1.e9 / static_cast(update_rate_)))); + } +}; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/worker.cpp b/lbr_fri_ros2/src/worker.cpp index ed8d0495..b42f55a0 100644 --- a/lbr_fri_ros2/src/worker.cpp +++ b/lbr_fri_ros2/src/worker.cpp @@ -12,26 +12,26 @@ Worker::~Worker() { void Worker::run_async(int rt_prio) { if (running_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); return; } run_thread_ = std::thread([this, rt_prio]() { if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::WARNING << "Failed to set FIFO realtime scheduling policy. Refer to " "[https://control.ros.org/master/doc/ros2_control/" "controller_manager/doc/userdoc.html]." << ColorScheme::ENDC); } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Realtime scheduling policy set to FIFO with priority '" << rt_prio << "'" << ColorScheme::ENDC); } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Starting run thread"); should_stop_ = false; // perform work in child-class @@ -39,7 +39,7 @@ void Worker::run_async(int rt_prio) { // perform work end running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Exiting run thread"); }); } @@ -47,7 +47,7 @@ void Worker::request_stop() { if (!running_) { return; } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Requesting run thread stop"); should_stop_ = true; } } // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 07052fbe..64c91616 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -101,3 +101,4 @@ 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 + timeout: 0.2 # stop controller if no command is received within this time [s] 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 index bd8b157c..a10a2ba0 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -81,7 +81,7 @@ class TwistController : public controller_interface::ControllerInterface { // some safety checks std::atomic updates_since_last_command_ = 0; - double max_time_without_command_ = 0.2; + double timeout_ = 0.2; // joint veloctiy computation std::unique_ptr twist_impl_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index ffa14fd7..9a43948b 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -56,6 +56,9 @@ struct SystemInterfaceParameters { }; struct EstimatedFTSensorParameters { + bool enabled{true}; + std::uint16_t update_rate{100}; + int32_t rt_prio{30}; std::string chain_root{"lbr_link_0"}; std::string chain_tip{"lbr_link_ee"}; double damping{0.2}; @@ -162,6 +165,7 @@ class SystemInterface : public hardware_interface::SystemInterface { // additional force-torque state interface lbr_fri_ros2::cart_array_t hw_ft_; + std::shared_ptr ft_estimator_impl_ptr_; std::unique_ptr ft_estimator_ptr_; // exposed command interfaces diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index fa827b8e..0483180c 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -73,8 +73,10 @@ controller_interface::CallbackReturn TwistController::on_init() { 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); + this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); configure_twist_impl_(); + timeout_ = this->get_node()->get_parameter("timeout").as_double(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", e.what()); @@ -98,11 +100,9 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } - if (updates_since_last_command_ > - static_cast(max_time_without_command_ / period.seconds())) { + if (updates_since_last_command_ > static_cast(timeout_ / period.seconds())) { RCLCPP_ERROR(this->get_node()->get_logger(), - "No twist command received within time %f. Stopping the controller.", - max_time_without_command_); + "No twist command received within %.3f s. Stopping the controller.", timeout_); return controller_interface::return_type::ERROR; } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 743f0061..91cbf710 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -61,6 +61,13 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { nan_last_hw_states_(); // setup force-torque estimator + std::transform(info_.sensors[1].parameters.at("enabled").begin(), + info_.sensors[1].parameters.at("enabled").end(), + info_.sensors[1].parameters.at("enabled").begin(), + ::tolower); // convert to lower case + ft_parameters_.enabled = info_.sensors[1].parameters.at("enabled") == "true"; + ft_parameters_.update_rate = std::stoul(info_.sensors[1].parameters.at("update_rate")); + ft_parameters_.rt_prio = std::stoi(info_.sensors[1].parameters.at("rt_prio")); ft_parameters_.chain_root = info_.sensors[1].parameters.at("chain_root"); ft_parameters_.chain_tip = info_.sensors[1].parameters.at("chain_tip"); ft_parameters_.damping = std::stod(info_.sensors[1].parameters.at("damping")); @@ -70,16 +77,20 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ft_parameters_.torque_x_th = std::stod(info_.sensors[1].parameters.at("torque_x_th")); ft_parameters_.torque_y_th = std::stod(info_.sensors[1].parameters.at("torque_y_th")); ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th")); - ft_estimator_ptr_ = std::make_unique( - info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, - lbr_fri_ros2::cart_array_t{ - ft_parameters_.force_x_th, - ft_parameters_.force_y_th, - ft_parameters_.force_z_th, - ft_parameters_.torque_x_th, - ft_parameters_.torque_y_th, - ft_parameters_.torque_z_th, - }); + if (ft_parameters_.enabled) { + ft_estimator_impl_ptr_ = std::make_shared( + info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, + lbr_fri_ros2::cart_array_t{ + ft_parameters_.force_x_th, + ft_parameters_.force_y_th, + ft_parameters_.force_z_th, + ft_parameters_.torque_x_th, + ft_parameters_.torque_y_th, + ft_parameters_.torque_z_th, + }); + ft_estimator_ptr_ = std::make_unique(ft_estimator_impl_ptr_, + ft_parameters_.update_rate); + } if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; @@ -155,14 +166,16 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_TIME_STAMP_NANO_SEC, &hw_time_stamp_nano_sec_); - // additional force-torque state interface - const auto &estimated_ft_sensor = info_.sensors[1]; - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + // additional force-torque state interface (if enabled) + if (ft_parameters_.enabled) { + const auto &estimated_ft_sensor = info_.sensors[1]; + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + } return state_interfaces; } @@ -250,6 +263,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } std::this_thread::sleep_for(std::chrono::seconds(1)); } + + // ft sensor + if (!ft_estimator_ptr_ && ft_parameters_.enabled) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Failed to instantiate FTEstimator despite user request." + << lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } + if (ft_estimator_ptr_) { + ft_estimator_ptr_->run_async(ft_parameters_.rt_prio); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -257,6 +282,9 @@ controller_interface::CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { app_ptr_->request_stop(); app_ptr_->close_udp_socket(); + if (ft_estimator_ptr_) { + ft_estimator_ptr_->request_stop(); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -285,6 +313,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim << lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->request_stop(); app_ptr_->close_udp_socket(); + ft_estimator_ptr_->request_stop(); return hardware_interface::return_type::ERROR; } @@ -305,8 +334,13 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim update_last_hw_states_(); // additional force-torque state interface - ft_estimator_ptr_->compute(hw_lbr_state_.measured_joint_position, hw_lbr_state_.external_torque, - hw_ft_, ft_parameters_.damping); + if (ft_parameters_.enabled) { + // note that (if enabled) the computation is performed asynchronously to not block the main + // thread + ft_estimator_impl_ptr_->set_q(hw_lbr_state_.measured_joint_position); + ft_estimator_impl_ptr_->set_tau_ext(hw_lbr_state_.external_torque); + ft_estimator_impl_ptr_->get_f_ext_tf(hw_ft_); + } return hardware_interface::return_type::OK; } @@ -526,8 +560,10 @@ bool SystemInterface::verify_sensors_() { if (!verify_auxiliary_sensor_()) { return false; } - if (!verify_estimated_ft_sensor_()) { - return false; + if (ft_parameters_.enabled) { + if (!verify_estimated_ft_sensor_()) { + return false; + } } return true; }