From 5fdaed9630d89ca82cbecaf472595f28433b87bf Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 18 Nov 2024 13:18:37 +0000 Subject: [PATCH 1/7] tip frame twist command support (#163) --- lbr_ros2_control/config/lbr_controllers.yaml | 1 + .../lbr_ros2_control/controllers/twist_controller.hpp | 1 + lbr_ros2_control/src/controllers/twist_controller.cpp | 11 +++++++++++ 3 files changed, 13 insertions(+) diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 64c91616..4d858951 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -98,6 +98,7 @@ robot_name: lbr chain_root: lbr_link_0 chain_tip: lbr_link_ee + twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame 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 index a10a2ba0..477ba98f 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 @@ -29,6 +29,7 @@ namespace lbr_ros2_control { struct TwistParameters { std::string chain_root; std::string chain_tip; + bool twist_in_tip_frame; double damping; double max_linear_velocity; double max_angular_velocity; diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 0483180c..5cb7d7b2 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -25,6 +25,15 @@ void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); }); + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + // compute jacobian auto jacobian = kinematics_ptr_->compute_jacobian(q); jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); @@ -70,6 +79,7 @@ controller_interface::CallbackReturn TwistController::on_init() { 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("twist_in_tip_frame", true); 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); @@ -201,6 +211,7 @@ void TwistController::configure_twist_impl_() { 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("twist_in_tip_frame").as_bool(), 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()}); From 836f30cb7b6aadb37b1d3bb70a84610bcf301766 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 18 Nov 2024 18:03:53 +0000 Subject: [PATCH 2/7] initial admittance controller impl (#163) --- lbr_fri_ros2/CMakeLists.txt | 4 + lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 48 ++++ lbr_fri_ros2/package.xml | 1 + lbr_fri_ros2/src/control.cpp | 72 ++++++ lbr_ros2_control/CMakeLists.txt | 1 + lbr_ros2_control/config/lbr_controllers.yaml | 27 ++- .../controllers/admittance_controller.hpp | 57 ++++- .../controllers/twist_controller.hpp | 37 +-- .../src/controllers/admittance_controller.cpp | 211 +++++++++++++++--- .../src/controllers/twist_controller.cpp | 87 ++------ 10 files changed, 399 insertions(+), 146 deletions(-) create mode 100644 lbr_fri_ros2/include/lbr_fri_ros2/control.hpp create mode 100644 lbr_fri_ros2/src/control.cpp diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 04dff3fd..1dc8a943 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(lbr_fri_idl REQUIRED) find_package(orocos_kdl_vendor REQUIRED) @@ -33,6 +34,7 @@ add_library(lbr_fri_ros2 src/app.cpp src/async_client.cpp src/command_guard.cpp + src/control.cpp src/filters.cpp src/ft_estimator.cpp src/kinematics.cpp @@ -48,6 +50,7 @@ target_include_directories(lbr_fri_ros2 ament_target_dependencies(lbr_fri_ros2 control_toolbox Eigen3 + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor @@ -65,6 +68,7 @@ ament_export_dependencies( eigen3_cmake_module Eigen3 FRIClient + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp new file mode 100644 index 00000000..88297ae5 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -0,0 +1,48 @@ +#ifndef LBR_FRI_ROS2__CONTROL_HPP_ +#define LBR_FRI_ROS2__CONTROL_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/Core" +#include "geometry_msgs/msg/twist.hpp" + +#include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" + +namespace lbr_fri_ros2 { +struct InvJacCtrlParameters { + std::string chain_root; + std::string chain_tip; + bool twist_in_tip_frame; + double damping; + double max_linear_velocity; + double max_angular_velocity; +}; + +class InvJacCtrlImpl { +public: + InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters ¶meters); + + void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + void compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, jnt_array_t_ref dq); + void compute(const Eigen::Matrix &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + + inline const std::unique_ptr &get_kinematics_ptr() const { return kinematics_ptr_; }; + +protected: + void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); + + InvJacCtrlParameters parameters_; + + jnt_array_t q_; + std::unique_ptr kinematics_ptr_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix twist_target_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 62379bc1..d9cc4bf1 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -15,6 +15,7 @@ control_toolbox fri_client_sdk + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp new file mode 100644 index 00000000..7016c974 --- /dev/null +++ b/lbr_fri_ros2/src/control.cpp @@ -0,0 +1,72 @@ +#include "lbr_fri_ros2/control.hpp" + +namespace lbr_fri_ros2 { +InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description, + const InvJacCtrlParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique(robot_description, parameters_.chain_root, + parameters_.chain_tip); +} + +void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref 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; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq) { + // twist to Eigen + twist_target_[0] = twist_target[0]; + twist_target_[1] = twist_target[1]; + twist_target_[2] = twist_target[2]; + twist_target_[3] = twist_target[3]; + twist_target_[4] = twist_target[4]; + twist_target_[5] = twist_target[5]; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const Eigen::Matrix &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref dq) { + twist_target_ = twist_target; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { + // 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); + }); + + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; +} +} // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index cf019396..8621fa41 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED + src/controllers/admittance_controller.cpp src/controllers/lbr_joint_position_command_controller.cpp src/controllers/lbr_torque_command_controller.cpp src/controllers/lbr_wrench_command_controller.cpp diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 4d858951..aae3b6ea 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -93,13 +93,28 @@ ros__parameters: robot_name: lbr +/**/admittance_controller: + ros__parameters: + robot_name: lbr + admittance: + mass: 0.2 + damping: 0.1 + stiffness: 0.0 + inv_jac_ctrl: + 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 + /**/twist_controller: ros__parameters: robot_name: lbr - chain_root: lbr_link_0 - chain_tip: lbr_link_ee - twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame - 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 + inv_jac_ctrl: + chain_root: lbr_link_0 + chain_tip: lbr_link_ee + twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame + 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/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp index f6909e11..d110d2ba 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -9,22 +9,40 @@ #include #include "controller_interface/controller_interface.hpp" -#include "hardware_interface/loaned_command_interface.hpp" +#include "eigen3/Eigen/Core" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "semantic_components/force_torque_sensor.hpp" #include "friLBRState.h" +#include "lbr_fri_ros2/control.hpp" #include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -class Admittance { - // implement an addmittance... +struct AdmittanceParameters { + double m = 1.0; + double b = 0.1; + double k = 0.0; }; -class AdmittanceController : public controller_interface::ControllerInterface { - static constexpr uint8_t CARTESIAN_DOF = 6; +class AdmittanceImpl { +public: + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + + void compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; + } +protected: + AdmittanceParameters parameters_; +}; + +class AdmittanceController : public controller_interface::ControllerInterface { public: AdmittanceController(); @@ -47,18 +65,33 @@ class AdmittanceController : public controller_interface::ControllerInterface { on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_command_interfaces_(); bool reference_state_interfaces_(); - void clear_command_interfaces_(); void clear_state_interfaces_(); void configure_joint_names_(); - + void configure_admittance_impl_(); + void configure_inv_jac_ctrl_impl_(); + void zero_all_values_(); + + // admittance + bool initialized_ = false; + std::unique_ptr admittance_impl_ptr_; + Eigen::Matrix x_init_, x_prev_; + Eigen::Matrix f_ext_, x_, dx_, ddx_; + + // joint veloctiy computation + std::unique_ptr inv_jac_ctrl_impl_ptr_; + lbr_fri_ros2::jnt_array_t q_, dq_; + Eigen::Matrix twist_command_; + + // interfaces lbr_fri_ros2::jnt_name_array_t joint_names_; - - std::vector> - joint_position_command_interfaces_; std::vector> - estimated_ft_sensor_state_interface_; + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_ptr_; + std::unique_ptr> + session_state_interface_ptr_; + std::unique_ptr estimated_ft_sensor_ptr_; }; } // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ 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 477ba98f..b4d27780 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 @@ -4,14 +4,12 @@ #include #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" @@ -20,37 +18,12 @@ #include "friLBRState.h" +#include "lbr_fri_ros2/control.hpp" #include "lbr_fri_ros2/kinematics.hpp" -#include "lbr_fri_ros2/pinv.hpp" #include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -struct TwistParameters { - std::string chain_root; - std::string chain_tip; - bool twist_in_tip_frame; - 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::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq); - -protected: - TwistParameters parameters_; - - lbr_fri_ros2::jnt_array_t q_; - std::unique_ptr kinematics_ptr_; - Eigen::Matrix jacobian_inv_; - Eigen::Matrix twist_target_; -}; - class TwistController : public controller_interface::ControllerInterface { public: TwistController(); @@ -78,14 +51,14 @@ class TwistController : public controller_interface::ControllerInterface { void clear_state_interfaces_(); void reset_command_buffer_(); void configure_joint_names_(); - void configure_twist_impl_(); + void configure_inv_jac_ctrl_impl_(); // some safety checks std::atomic updates_since_last_command_ = 0; double timeout_ = 0.2; // joint veloctiy computation - std::unique_ptr twist_impl_ptr_; + std::unique_ptr inv_jac_ctrl_impl_ptr_; lbr_fri_ros2::jnt_array_t q_, dq_; // interfaces @@ -93,9 +66,9 @@ class TwistController : public controller_interface::ControllerInterface { std::vector> joint_position_state_interfaces_; std::unique_ptr> - sample_time_state_interface_; + sample_time_state_interface_ptr_; std::unique_ptr> - session_state_interface_; + session_state_interface_ptr_; // real-time twist command topic realtime_tools::RealtimeBuffer rt_twist_ptr_; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index f7b61c29..fe06c703 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -5,75 +5,182 @@ AdmittanceController::AdmittanceController() {} controller_interface::InterfaceConfiguration AdmittanceController::command_interface_configuration() const { - // reference joint position command interface + 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 AdmittanceController::state_interface_configuration() const { - // retrieve estimated ft state interface + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // joint position interface + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + + // estimated force-torque sensor interface + for (const auto &interface_name : estimated_ft_sensor_ptr_->get_state_interface_names()) { + interface_configuration.names.push_back(interface_name); + } + + // additional state interfaces + 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 AdmittanceController::on_init() {} +controller_interface::CallbackReturn AdmittanceController::on_init() { + try { + this->get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("admittance.mass", 1.0); + this->get_node()->declare_parameter("admittance.damping", 0.1); + this->get_node()->declare_parameter("admittance.stiffness", 0.0); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + configure_joint_names_(); + configure_admittance_impl_(); + configure_inv_jac_ctrl_impl_(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize admittance controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} -controller_interface::return_type AdmittanceController::update(const rclcpp::Time &time, +controller_interface::return_type AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) { + // get estimated force-torque sensor values + f_ext_.head(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_forces().data()); + f_ext_.tail(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_torques().data()); + + // get joint positions + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute forward kinematics + auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_); + x_.head(3) = Eigen::Map>(chain_tip_frame.p.data); + chain_tip_frame.M.GetRPY(x_init_[3], x_init_[4], x_init_[5]); + + // compute steady state position and orientation + if (!initialized_) { + x_init_ = x_; + x_prev_ = x_; + initialized_ = true; + } + + // compute velocity + dx_ = (x_ - x_prev_) / period.seconds(); + // compute admittance - // add warning for high force-torques and refer to load data calibration + admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_); + + // update previous position + x_prev_ = x_; + + // integrate ddx_ to command velocity + twist_command_ = ddx_ * period.seconds(); + + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_ptr_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + + // compute the joint velocity from the twist command target + inv_jac_ctrl_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_ptr_->get().get_value()); + ++i; + }); + + return controller_interface::return_type::OK; } controller_interface::CallbackReturn -AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { +AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + estimated_ft_sensor_ptr_ = std::make_unique( + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_X, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Y, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Z, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_X, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Y, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Z); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - if (!reference_command_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } +AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!reference_state_interfaces_()) { return controller_interface::CallbackReturn::ERROR; } + zero_all_values_(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - clear_command_interfaces_(); +AdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { clear_state_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool AdmittanceController::reference_command_interfaces_() { - for (auto &command_interface : command_interfaces_) { - if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); +bool AdmittanceController::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_ptr_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); } } - if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { + if (!estimated_ft_sensor_ptr_->assign_loaned_state_interfaces(state_interfaces_)) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to assign estimated force torque state interfaces."); + return false; + } + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint position command interfaces '%ld' does not match the number of joints " + "Number of joint position state interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } -} - -bool AdmittanceController::reference_state_interfaces_() { - for (auto &state_interface : state_interfaces_) { - if (state_interface.get_interface_name() == HW_IF_ESTIMATED_FT_PREFIX) { - estimated_ft_sensor_state_interface_.emplace_back(std::ref(state_interface)); - } - } -} - -void AdmittanceController::clear_command_interfaces_() { - joint_position_command_interfaces_.clear(); + return true; } void AdmittanceController::clear_state_interfaces_() { - estimated_ft_sensor_state_interface_.clear(); + joint_position_state_interfaces_.clear(); + estimated_ft_sensor_ptr_->release_interfaces(); } void AdmittanceController::configure_joint_names_() { @@ -89,4 +196,44 @@ void AdmittanceController::configure_joint_names_() { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } + +void AdmittanceController::configure_admittance_impl_() { + admittance_impl_ptr_ = std::make_unique( + AdmittanceParameters{this->get_node()->get_parameter("admittance.mass").as_double(), + this->get_node()->get_parameter("admittance.damping").as_double(), + this->get_node()->get_parameter("admittance.stiffness").as_double()}); + RCLCPP_INFO(this->get_node()->get_logger(), "Admittance controller initialized."); + RCLCPP_INFO(this->get_node()->get_logger(), "Mass: %f", + this->get_node()->get_parameter("admittance.mass").as_double()); + RCLCPP_INFO(this->get_node()->get_logger(), "Damping: %f", + this->get_node()->get_parameter("admittance.damping").as_double()); + RCLCPP_INFO(this->get_node()->get_logger(), "Stiffness: %f", + this->get_node()->get_parameter("admittance.stiffness").as_double()); +} + +void AdmittanceController::configure_inv_jac_ctrl_impl_() { + inv_jac_ctrl_impl_ptr_ = std::make_unique( + this->get_robot_description(), + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + true, // always assume twist in tip frame, since force-torque is estimated in tip frame + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); +} + +void AdmittanceController::zero_all_values_() { + f_ext_.setZero(); + x_.setZero(); + dx_.setZero(); + ddx_.setZero(); + std::fill(dq_.begin(), dq_.end(), 0.0); + twist_command_.setZero(); +} } // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::AdmittanceController, + controller_interface::ControllerInterface) \ 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 index 5cb7d7b2..9994f556 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -1,48 +1,6 @@ #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::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref 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); - }); - - // if desired, transform to tip frame - if (parameters_.twist_in_tip_frame) { - auto chain_tip_frame = kinematics_ptr_->compute_fk(q); - twist_target_.topRows(3) = - Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); - twist_target_.bottomRows(3) = - Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); - } - - // 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 @@ -77,15 +35,15 @@ controller_interface::CallbackReturn TwistController::on_init() { updates_since_last_command_ = 0; }); 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("twist_in_tip_frame", true); - 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("inv_jac_ctrl.chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true); + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); - configure_twist_impl_(); + configure_inv_jac_ctrl_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.", @@ -102,11 +60,11 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / 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."); + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); return controller_interface::return_type::ERROR; } - if (static_cast(session_state_interface_->get().get_value()) != + if (static_cast(session_state_interface_ptr_->get().get_value()) != KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { return controller_interface::return_type::OK; } @@ -123,12 +81,12 @@ controller_interface::return_type TwistController::update(const rclcpp::Time & / }); // compute the joint velocity from the twist command target - twist_impl_ptr_->compute(*twist_command, q_, dq_); + inv_jac_ctrl_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()); + q_i + dq_[i] * sample_time_state_interface_ptr_->get().get_value()); ++i; }); @@ -164,12 +122,12 @@ bool TwistController::reference_state_interfaces_() { joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { - sample_time_state_interface_ = + sample_time_state_interface_ptr_ = std::make_unique>( std::ref(state_interface)); } if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { - session_state_interface_ = + session_state_interface_ptr_ = std::make_unique>( std::ref(state_interface)); } @@ -206,15 +164,16 @@ void TwistController::configure_joint_names_() { } } -void TwistController::configure_twist_impl_() { - twist_impl_ptr_ = std::make_unique( +void TwistController::configure_inv_jac_ctrl_impl_() { + inv_jac_ctrl_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("twist_in_tip_frame").as_bool(), - 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()}); + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.twist_in_tip_frame").as_bool(), + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); } } // namespace lbr_ros2_control From ccab2db2806d0c305132c0f90b26d0f82bc7c4d7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 16:24:42 +0000 Subject: [PATCH 3/7] pid -> exp smooth + exp smooth with time constant (#163) --- .../ros2_control/lbr_system_config.yaml | 20 ++-- .../ros2_control/lbr_system_interface.xacro | 11 +- lbr_fri_ros2/CMakeLists.txt | 3 - .../include/lbr_fri_ros2/async_client.hpp | 5 +- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 113 ++++++++---------- .../lbr_fri_ros2/interfaces/base_command.hpp | 4 +- .../interfaces/position_command.hpp | 2 +- .../include/lbr_fri_ros2/interfaces/state.hpp | 8 +- .../interfaces/torque_command.hpp | 2 +- .../interfaces/wrench_command.hpp | 2 +- lbr_fri_ros2/package.xml | 1 - lbr_fri_ros2/src/async_client.cpp | 8 +- lbr_fri_ros2/src/filters.cpp | 90 ++++++-------- lbr_fri_ros2/src/interfaces/base_command.cpp | 6 +- .../src/interfaces/position_command.cpp | 14 +-- lbr_fri_ros2/src/interfaces/state.cpp | 32 ++--- .../src/interfaces/torque_command.cpp | 15 ++- .../src/interfaces/wrench_command.cpp | 15 ++- lbr_fri_ros2/test/test_command_interfaces.cpp | 15 ++- lbr_fri_ros2/test/test_position_command.cpp | 11 +- lbr_fri_ros2/test/test_torque_command.cpp | 11 +- lbr_fri_ros2/test/test_wrench_command.cpp | 11 +- .../lbr_ros2_control/system_interface.hpp | 12 +- lbr_ros2_control/src/system_interface.cpp | 28 +---- 24 files changed, 184 insertions(+), 255 deletions(-) diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 6e110244..8f96cf0d 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -7,15 +7,19 @@ hardware: port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop - pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) - pid_i: 0.0 # I gain for the joint position command - pid_d: 0.0 # D gain for the joint position command - pid_i_max: 0.0 # max integral value for the joint position command - pid_i_min: 0.0 # min integral value for the joint position command - pid_antiwindup: false # enable antiwindup for the joint position command + # exponential moving average time constant for joint position commands [s]: + # Set tau > robot sample time for more smoothing on the commands + # Set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.04 command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] - measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + # exponential moving average time constant for external joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for more smoothing on the external torque measurements + external_torque_tau: 0.04 + # exponential moving average time constant for joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for more smoothing on the raw torque measurements + measured_torque_tau: 0.04 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 diff --git a/lbr_description/ros2_control/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro index fbe30cc6..f4a4452f 100644 --- a/lbr_description/ros2_control/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -27,15 +27,10 @@ ${system_config['hardware']['port_id']} ${system_config['hardware']['remote_host']} ${system_config['hardware']['rt_prio']} - ${system_config['hardware']['pid_p']} - ${system_config['hardware']['pid_i']} - ${system_config['hardware']['pid_d']} - ${system_config['hardware']['pid_i_max']} - ${system_config['hardware']['pid_i_min']} - ${system_config['hardware']['pid_antiwindup']} + ${system_config['hardware']['joint_position_tau']} ${system_config['hardware']['command_guard_variant']} - ${system_config['hardware']['external_torque_cutoff_frequency']} - ${system_config['hardware']['measured_torque_cutoff_frequency']} + ${system_config['hardware']['external_torque_tau']} + ${system_config['hardware']['measured_torque_tau']} ${system_config['hardware']['open_loop']} diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 1dc8a943..a26f445b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -11,7 +11,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) @@ -48,7 +47,6 @@ target_include_directories(lbr_fri_ros2 ) ament_target_dependencies(lbr_fri_ros2 - control_toolbox Eigen3 geometry_msgs kdl_parser @@ -64,7 +62,6 @@ target_link_libraries(lbr_fri_ros2 ament_export_targets(lbr_fri_ros2_export HAS_LIBRARY_TARGET) ament_export_dependencies( - control_toolbox eigen3_cmake_module Eigen3 FRIClient diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index db194729..f44f33a8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,7 +11,6 @@ #include "friClientVersion.h" #include "friLBRClient.h" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" @@ -27,10 +26,10 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, - const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, + const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}, const bool &open_loop = true); inline std::shared_ptr get_command_interface() { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 6f62bdd6..3773be57 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -3,57 +3,71 @@ #include #include -#include #include +#include -#include "control_toolbox/filters.hpp" -#include "control_toolbox/pid_ros.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" -#include "friLBRClient.h" - -#include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { public: /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. * */ ExponentialFilter(); /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. + * + * @param[in] tau Time constant in seconds. + */ + ExponentialFilter(const double &tau); + + /** + * @brief Initialize the filter from members. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) * - * @param[in] cutoff_frequency Frequency in Hz. * @param[in] sample_time Sample time in seconds. */ - ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + void initialize(const double &sample_time); /** - * @brief Compute the exponential smoothing using the control_toolbox - * https://github.com/ros-controls/control_toolbox. + * @brief Initialize the filter. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) + * + * @param[in] tau Time constant in seconds. + * @param[in] sample_time Sample time in seconds. + */ + void initialize(const double &tau, const double &sample_time); + + /** + * @brief Compute the exponential smoothing. Internally computes the new smoothed value following + * smoothed = alpha * current + (1 - alpha) * previous. * * @param[in] current The current value. * @param[in] previous The previous smoothed value. * @return double The returned smoothed value. */ inline double compute(const double ¤t, const double &previous) { - return filters::exponentialSmoothing(current, previous, alpha_); + return alpha_ * current + (1.0 - alpha_) * previous; }; /** - * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * @brief Get the time constant #tau_. * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. + * @return const double& */ - void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + inline const double &get_tau() const { return tau_; }; /** * @brief Get #sample_time_. @@ -70,16 +84,6 @@ class ExponentialFilter { inline const double &get_alpha() const { return alpha_; }; protected: - /** - * @brief Compute alpha given the cutoff frequency and the sample time. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - * @return double Alpha based on - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. - */ - double compute_alpha_(const double &cutoff_frequency, const double &sample_time); - /** * @brief Validate alpha in [0, 1]. * @@ -89,53 +93,30 @@ class ExponentialFilter { */ bool validate_alpha_(const double &alpha); - double cutoff_frequency_; /**< Frequency in Hz.*/ - double sample_time_; /**< Sample time in seconds.*/ - double - alpha_; /**< Alpha parameter based on - https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ + double tau_; /**< Time constant in seconds.*/ + double sample_time_; /**< Sample time in seconds.*/ + double alpha_; /**< Smoothing parameter in [0, 1].*/ }; class JointExponentialFilterArray { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointExponentialFilterArray"; + public: JointExponentialFilterArray() = default; + JointExponentialFilterArray(const double &tau); void compute(const double *const current, jnt_array_t_ref previous); - void initialize(const double &cutoff_frequency, const double &sample_time); + void compute(const_jnt_array_t_ref current, jnt_array_t_ref previous); + void initialize(const double &sample_time); + void initialize(const double &tau, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; -protected: - bool initialized_{false}; /**< True if initialized.*/ - ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ -}; - -struct PIDParameters { - double p{0.0}; /**< Proportional gain.*/ - double i{0.0}; /**< Integral gain.*/ - double d{0.0}; /**< Derivative gain.*/ - double i_max{0.0}; /**< Maximum integral value.*/ - double i_min{0.0}; /**< Minimum integral value.*/ - bool antiwindup{false}; /**< Antiwindup enabled.*/ -}; - -class JointPIDArray { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using pid_array_t = std::array; - -public: - JointPIDArray() = delete; - JointPIDArray(const PIDParameters &pid_parameters); - - void compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); - void compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command); void log_info() const; protected: - PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + bool initialized_{false}; /**< True if initialized.*/ + ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index bb46d0ae..4aa3c582 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -24,7 +24,7 @@ class BaseCommandInterface { public: BaseCommandInterface() = delete; - BaseCommandInterface(const PIDParameters &pid_parameters, + BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); @@ -39,7 +39,7 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - JointPIDArray joint_position_pid_; + JointExponentialFilterArray joint_position_filter_; idl_command_t command_, command_target_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index 24e8a324..399beed7 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -12,7 +12,7 @@ class PositionCommandInterface : public BaseCommandInterface { public: PositionCommandInterface() = delete; - PositionCommandInterface(const PIDParameters &pid_parameters, + PositionCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 23457637..ca0a6068 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -1,6 +1,8 @@ #ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ #define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ + #include +#include #include #include "rclcpp/logger.hpp" @@ -14,8 +16,8 @@ namespace lbr_fri_ros2 { struct StateInterfaceParameters { - double external_torque_cutoff_frequency; /*Hz*/ - double measured_torque_cutoff_frequency; /*Hz*/ + double external_torque_tau; /*seconds*/ + double measured_torque_tau; /*seconds*/ }; class StateInterface { @@ -24,7 +26,7 @@ class StateInterface { public: StateInterface() = delete; - StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); + StateInterface(const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}); inline const_idl_state_t_ref get_state() const { return state_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index e4ec7874..70536bee 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -12,7 +12,7 @@ class TorqueCommandInterface : public BaseCommandInterface { public: TorqueCommandInterface() = delete; - TorqueCommandInterface(const PIDParameters &pid_parameters, + TorqueCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index 0c846ee4..4e68fae0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -12,7 +12,7 @@ class WrenchCommandInterface : public BaseCommandInterface { public: WrenchCommandInterface() = delete; - WrenchCommandInterface(const PIDParameters &pid_parameters, + WrenchCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index d9cc4bf1..a13fe491 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -13,7 +13,6 @@ eigen - control_toolbox fri_client_sdk geometry_msgs kdl_parser diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 14ad96bf..7d11414c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters, @@ -26,16 +26,16 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #endif { command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; } case KUKA::FRI::EClientCommandMode::TORQUE: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; case KUKA::FRI::EClientCommandMode::WRENCH: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; default: std::string err = "Unsupported client command mode."; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index eb842ccc..2bf0ed2c 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -1,34 +1,41 @@ #include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { -ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} +ExponentialFilter::ExponentialFilter() + : tau_(std::numeric_limits::quiet_NaN()), + sample_time_(std::numeric_limits::quiet_NaN()), + alpha_(std::numeric_limits::quiet_NaN()) {} -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { - set_cutoff_frequency(cutoff_frequency, sample_time); +ExponentialFilter::ExponentialFilter(const double &tau) : tau_(tau) {} + +void ExponentialFilter::initialize(const double &sample_time) { + if (std::isnan(tau_)) { + throw std::runtime_error("Time constant must be set before initializing."); + } + return initialize(tau_, sample_time); } -void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, - const double &sample_time) { - cutoff_frequency_ = cutoff_frequency; - if (cutoff_frequency_ > (1. / sample_time)) { - cutoff_frequency_ = (1. / sample_time); +void ExponentialFilter::initialize(const double &tau, const double &sample_time) { + if (tau <= 0.0) { + throw std::runtime_error("Time constant must be positive and greater zero."); } - sample_time_ = sample_time; - alpha_ = compute_alpha_(cutoff_frequency, sample_time); - if (!validate_alpha_(alpha_)) { + if (sample_time < 0.0) { + throw std::runtime_error("Sample time must be positive."); + } + double alpha = 1.0 - std::exp(-sample_time / tau); + if (!validate_alpha_(alpha)) { throw std::runtime_error("Alpha is not within [0, 1]"); } -} - -double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, - const double &sample_time) { - double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; - return std::cos(omega_3db) - 1 + - std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); + tau_ = tau; + sample_time_ = sample_time; + alpha_ = alpha; } bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } +JointExponentialFilterArray::JointExponentialFilterArray(const double &tau) + : exponential_filter_(tau) {} + void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { previous[i] = exponential_filter_.compute(current_i, previous[i]); @@ -36,47 +43,22 @@ void JointExponentialFilterArray::compute(const double *const current, jnt_array }); } -void JointExponentialFilterArray::initialize(const double &cutoff_frequency, - const double &sample_time) { - exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); - initialized_ = true; +void JointExponentialFilterArray::compute(const_jnt_array_t_ref current, jnt_array_t_ref previous) { + compute(current.data(), previous); } -JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) - : pid_parameters_(pid_parameters) // keep local copy of parameters since - // controller_toolbox::Pid::getGains is not const correct - // (i.e. can't be called in this->log_info) -{ - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, - pid_parameters_.i_min, pid_parameters_.antiwindup); - }); -} - -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &sample_time) { + exponential_filter_.initialize(sample_time); + initialized_ = true; } -void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state, - const std::chrono::nanoseconds &dt, jnt_array_t_ref command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &tau, const double &sample_time) { + exponential_filter_.initialize(tau, sample_time); + initialized_ = true; } -void JointPIDArray::log_info() const { +void JointExponentialFilterArray::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); -}; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* tau: %.5f s", exponential_filter_.get_tau()); +} } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index f53bddeb..e43111ea 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -2,10 +2,10 @@ namespace lbr_fri_ros2 { -BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, +BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : joint_position_pid_(pid_parameters) { + : joint_position_filter_(joint_position_tau) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,6 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - joint_position_pid_.log_info(); + joint_position_filter_.log_info(); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index b80530f3..95533981 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { PositionCommandInterface::PositionCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -40,11 +40,11 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 3054bf55..d34b457e 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -15,7 +15,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -23,7 +22,6 @@ void StateInterface::set_state(const_fri_state_t_ref state) { } std::memcpy(state_.measured_joint_position.data(), state.getMeasuredJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -34,9 +32,14 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; }; @@ -52,7 +55,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), @@ -60,7 +62,6 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, } std::memcpy(state_.measured_joint_position.data(), joint_position.data(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -71,24 +72,27 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; } void StateInterface::init_filters_() { - external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, - state_.sample_time); - measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, - state_.sample_time); + external_torque_filter_.initialize(parameters_.external_torque_tau, state_.sample_time); + measured_torque_filter_.initialize(parameters_.measured_torque_tau, state_.sample_time); } void StateInterface::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %.1f Hz", - parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f Hz", - parameters_.measured_torque_cutoff_frequency); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_tau: %.5f s", + parameters_.external_torque_tau); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_tau: %.5f s", + parameters_.measured_torque_tau); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b658b266..54f53a2b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { TorqueCommandInterface::TorqueCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -30,12 +30,11 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.torque = command_target_.torque; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9dd9618a..1a1c0ab7 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { WrenchCommandInterface::WrenchCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -28,12 +28,11 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.wrench = command_target_.wrench; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 127fd2ee..d662643a 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -18,7 +18,6 @@ class TestCommandInterfaces : public ::testing::Test { public: TestCommandInterfaces() : random_engine_(std::random_device{}()) { - pid_params_ = lbr_fri_ros2::PIDParameters(); cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); state_interface_params_ = lbr_fri_ros2::StateInterfaceParameters(); @@ -40,17 +39,17 @@ class TestCommandInterfaces : public ::testing::Test { case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; } case KUKA::FRI::EClientCommandMode::TORQUE: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; case KUKA::FRI::EClientCommandMode::WRENCH: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; default: throw std::runtime_error("Unsupported client command mode."); @@ -129,7 +128,7 @@ class TestCommandInterfaces : public ::testing::Test { std::default_random_engine random_engine_; std::uniform_real_distribution uniform_real_dist_{-1.0, 1.0}; - lbr_fri_ros2::PIDParameters pid_params_; + double joint_position_tau_{0.04}; lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; lbr_fri_ros2::StateInterfaceParameters state_interface_params_; diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index e2f7830d..7da064aa 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 1.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::POSITION, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::POSITION, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 9c580828..45472578 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::TORQUE, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::TORQUE, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index 9ad8b315..e353476b 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,8 +21,6 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., @@ -39,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::WRENCH, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::WRENCH, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); 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 9a43948b..a7448a3c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -23,7 +23,6 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" @@ -44,15 +43,10 @@ struct SystemInterfaceParameters { const char *remote_host{nullptr}; int32_t rt_prio{80}; bool open_loop{true}; - double pid_p{0.0}; - double pid_i{0.0}; - double pid_d{0.0}; - double pid_i_max{0.0}; - double pid_i_min{0.0}; - double pid_antiwindup{0.0}; + double joint_position_tau{0.04}; std::string command_guard_variant{"default"}; - double external_torque_cutoff_frequency{10.0}; - double measured_torque_cutoff_frequency{10.0}; + double external_torque_tau{0.04}; + double measured_torque_tau{0.04}; }; struct EstimatedFTSensorParameters { diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 91cbf710..ebab71e4 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -18,15 +18,8 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { } // setup driver - lbr_fri_ros2::PIDParameters pid_parameters; lbr_fri_ros2::CommandGuardParameters command_guard_parameters; lbr_fri_ros2::StateInterfaceParameters state_interface_parameters; - pid_parameters.p = parameters_.pid_p; - pid_parameters.i = parameters_.pid_i; - pid_parameters.d = parameters_.pid_d; - pid_parameters.i_max = parameters_.pid_i_max; - pid_parameters.i_min = parameters_.pid_i_min; - pid_parameters.antiwindup = parameters_.pid_antiwindup; for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) { command_guard_parameters.joint_names[idx] = system_info.joints[idx].name; command_guard_parameters.max_positions[idx] = @@ -38,14 +31,12 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { command_guard_parameters.max_torques[idx] = std::stod(system_info.joints[idx].parameters.at("max_torque")); } - state_interface_parameters.external_torque_cutoff_frequency = - parameters_.external_torque_cutoff_frequency; - state_interface_parameters.measured_torque_cutoff_frequency = - parameters_.measured_torque_cutoff_frequency; + state_interface_parameters.external_torque_tau = parameters_.external_torque_tau; + state_interface_parameters.measured_torque_tau = parameters_.measured_torque_tau; try { async_client_ptr_ = std::make_shared( - parameters_.client_command_mode, pid_parameters, command_guard_parameters, + parameters_.client_command_mode, parameters_.joint_position_tau, command_guard_parameters, parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { @@ -412,17 +403,10 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & info_.hardware_parameters["pid_antiwindup"].end(), info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); // convert to lower case - parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); - parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); - parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); - parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); - parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); - parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; + parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); - parameters_.external_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); - parameters_.measured_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); + parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]); + parameters_.measured_torque_tau = std::stod(info_.hardware_parameters["measured_torque_tau"]); } catch (const std::out_of_range &e) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR From 78462d4833d8237ee537b0a460258eb001e7aa44 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 18:48:32 +0000 Subject: [PATCH 4/7] addressed review (https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/220#pullrequestreview-2443348238) --- lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 46 +++++++++++++++++++ lbr_fri_ros2/src/control.cpp | 33 +++++++++++++ .../controllers/admittance_controller.hpp | 24 +--------- .../controllers/twist_controller.hpp | 1 + .../src/controllers/admittance_controller.cpp | 36 ++++++++------- .../src/controllers/twist_controller.cpp | 3 ++ 6 files changed, 104 insertions(+), 39 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp index 88297ae5..bab95773 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -3,10 +3,13 @@ #include #include +#include #include #include "eigen3/Eigen/Core" #include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" @@ -23,6 +26,9 @@ struct InvJacCtrlParameters { }; class InvJacCtrlImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::InvJacCtrlImpl"; + public: InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters ¶meters); @@ -34,6 +40,8 @@ class InvJacCtrlImpl { inline const std::unique_ptr &get_kinematics_ptr() const { return kinematics_ptr_; }; + void log_info() const; + protected: void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); @@ -44,5 +52,43 @@ class InvJacCtrlImpl { Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; }; + +struct AdmittanceParameters { + AdmittanceParameters() = delete; + AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0) + : m(m), b(b), k(k) { + if (m <= 0.0) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + if (b < 0.0) { + throw std::runtime_error("Damping must be positive."); + } + if (k < 0.0) { + throw std::runtime_error("Stiffness must be positive."); + } + } + + double m = 1.0; + double b = 0.1; + double k = 0.0; +}; + +class AdmittanceImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl"; + +public: + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + + void compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx); + + void log_info() const; + +protected: + AdmittanceParameters parameters_; +}; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp index 7016c974..66ba5284 100644 --- a/lbr_fri_ros2/src/control.cpp +++ b/lbr_fri_ros2/src/control.cpp @@ -44,6 +44,20 @@ void InvJacCtrlImpl::compute(const Eigen::Matrix &twis compute_impl_(q, dq); } +void InvJacCtrlImpl::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain root: %s", + parameters_.chain_root.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain tip: %s", parameters_.chain_tip.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Twist in tip frame: %s", + parameters_.twist_in_tip_frame ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.damping); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max linear velocity: %.3f", + parameters_.max_linear_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f", + parameters_.max_angular_velocity); +} + void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { // clip velocity twist_target_.head(3).unaryExpr([&](double v) { @@ -69,4 +83,23 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) // compute target joint veloctiy and map it to dq Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; } + +void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + if (parameters_.m <= 0.0) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; +} + +void AdmittanceImpl::log_info() const { + { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: %.3f", parameters_.m); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.b); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Stiffness: %.3f", parameters_.k); + } +} } // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp index d110d2ba..17143903 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -21,27 +21,6 @@ #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -struct AdmittanceParameters { - double m = 1.0; - double b = 0.1; - double k = 0.0; -}; - -class AdmittanceImpl { -public: - AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} - - void compute(const Eigen::Matrix &f_ext, - const Eigen::Matrix &x, - const Eigen::Matrix &dx, - Eigen::Matrix &ddx) { - ddx = (f_ext - parameters_.b * dx - parameters_.k * x) / parameters_.m; - } - -protected: - AdmittanceParameters parameters_; -}; - class AdmittanceController : public controller_interface::ControllerInterface { public: AdmittanceController(); @@ -71,10 +50,11 @@ class AdmittanceController : public controller_interface::ControllerInterface { void configure_admittance_impl_(); void configure_inv_jac_ctrl_impl_(); void zero_all_values_(); + void log_info_() const; // admittance bool initialized_ = false; - std::unique_ptr admittance_impl_ptr_; + std::unique_ptr admittance_impl_ptr_; Eigen::Matrix x_init_, x_prev_; Eigen::Matrix f_ext_, x_, dx_, ddx_; 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 b4d27780..f0a1fea4 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 @@ -52,6 +52,7 @@ class TwistController : public controller_interface::ControllerInterface { void reset_command_buffer_(); void configure_joint_names_(); void configure_inv_jac_ctrl_impl_(); + void log_info_() const; // some safety checks std::atomic updates_since_last_command_ = 0; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index fe06c703..0178285c 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -50,6 +50,7 @@ controller_interface::CallbackReturn AdmittanceController::on_init() { configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); + log_info_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize admittance controller with: %s.", e.what()); @@ -76,7 +77,7 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim // compute forward kinematics auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_); x_.head(3) = Eigen::Map>(chain_tip_frame.p.data); - chain_tip_frame.M.GetRPY(x_init_[3], x_init_[4], x_init_[5]); + x_.tail(3) = Eigen::Map>(chain_tip_frame.M.GetRot().data); // compute steady state position and orientation if (!initialized_) { @@ -85,15 +86,17 @@ controller_interface::return_type AdmittanceController::update(const rclcpp::Tim initialized_ = true; } - // compute velocity + // compute velocity & update previous position dx_ = (x_ - x_prev_) / period.seconds(); + x_prev_ = x_; + + // convert f_ext_ back to root frame + f_ext_.head(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.head(3); + f_ext_.tail(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.tail(3); // compute admittance admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_); - // update previous position - x_prev_ = x_; - // integrate ddx_ to command velocity twist_command_ = ddx_ * period.seconds(); @@ -198,17 +201,11 @@ void AdmittanceController::configure_joint_names_() { } void AdmittanceController::configure_admittance_impl_() { - admittance_impl_ptr_ = std::make_unique( - AdmittanceParameters{this->get_node()->get_parameter("admittance.mass").as_double(), - this->get_node()->get_parameter("admittance.damping").as_double(), - this->get_node()->get_parameter("admittance.stiffness").as_double()}); - RCLCPP_INFO(this->get_node()->get_logger(), "Admittance controller initialized."); - RCLCPP_INFO(this->get_node()->get_logger(), "Mass: %f", - this->get_node()->get_parameter("admittance.mass").as_double()); - RCLCPP_INFO(this->get_node()->get_logger(), "Damping: %f", - this->get_node()->get_parameter("admittance.damping").as_double()); - RCLCPP_INFO(this->get_node()->get_logger(), "Stiffness: %f", - this->get_node()->get_parameter("admittance.stiffness").as_double()); + admittance_impl_ptr_ = + std::make_unique(lbr_fri_ros2::AdmittanceParameters{ + this->get_node()->get_parameter("admittance.mass").as_double(), + this->get_node()->get_parameter("admittance.damping").as_double(), + this->get_node()->get_parameter("admittance.stiffness").as_double()}); } void AdmittanceController::configure_inv_jac_ctrl_impl_() { @@ -217,7 +214,7 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { lbr_fri_ros2::InvJacCtrlParameters{ this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), - true, // always assume twist in tip frame, since force-torque is estimated in tip frame + false, // always assume twist in root frame this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); @@ -231,6 +228,11 @@ void AdmittanceController::zero_all_values_() { std::fill(dq_.begin(), dq_.end(), 0.0); twist_command_.setZero(); } + +void AdmittanceController::log_info_() const { + admittance_impl_ptr_->log_info(); + inv_jac_ctrl_impl_ptr_->log_info(); +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 9994f556..3b3befdf 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -44,6 +44,7 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); configure_inv_jac_ctrl_impl_(); + log_info_(); 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.", @@ -175,6 +176,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() { this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double()}); } + +void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); } } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" From 412922e1f08cdff8ae620f725f808783fa6ce16f Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 19:03:24 +0000 Subject: [PATCH 5/7] moved controller config for standalone URDF use --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_bringup/launch/gazebo.launch.py | 2 +- lbr_bringup/lbr_bringup/ros2_control.py | 2 +- .../lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst | 8 ++++++-- .../lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst | 4 ++-- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 ++++---- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 ++++---- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 ++-- lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 2 +- .../ros2_control}/lbr_controllers.yaml | 0 lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 +- 13 files changed, 25 insertions(+), 21 deletions(-) rename {lbr_ros2_control/config => lbr_description/ros2_control}/lbr_controllers.yaml (100%) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index b0395b29..c55bcb01 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -183,5 +183,5 @@ Troubleshooting --------------- Noisy Execution ~~~~~~~~~~~~~~~ -- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_:octicon:`link-external`. +- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_:octicon:`link-external`. - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username. diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index 34da3615..b6505ab4 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/config/lbr_controllers.yaml # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index e45a5fb9..3083ba10 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -11,7 +11,7 @@ class LBRROS2ControlMixin: def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg_pkg", - default_value="lbr_ros2_control", + default_value="lbr_description", description="Controller configuration package. The package containing the ctrl_cfg.", ) diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index f8426012..22e46da4 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -8,6 +8,10 @@ lbr_demos_advanced_cpp :local: :backlinks: none +Twist Controller +---------------- +TODO + Admittance Controller --------------------- This demo implements a simple admittance controller. @@ -15,7 +19,7 @@ This demo implements a simple admittance controller. #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -56,7 +60,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 616751d0..8e23245f 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -15,7 +15,7 @@ This demo implements a simple admittance controller. #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ This demo implements an admittance controller with a remote center of motion (RC #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 652f5aef..530e82f4 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index 2eeb8dd6..df04a2e6 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index ccab9103..02a35fbe 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -63,7 +63,7 @@ MoveIt Servo - Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -123,7 +123,7 @@ MoveIt via RViz - Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index 0816829a..8856e511 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -42,7 +42,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 78a833fc..637fc8bb 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_ros2_control)/config/lbr_controllers.yaml + $(find lbr_description)/config/lbr_controllers.yaml /${robot_name} diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml similarity index 100% rename from lbr_ros2_control/config/lbr_controllers.yaml rename to lbr_description/ros2_control/lbr_controllers.yaml diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 52e70858..f4fba85c 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -90,7 +90,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. In the `move_group.launch.py `_:octicon:`link-external` use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` + #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 07bc8c7c..986895ae 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -30,7 +30,7 @@ Hardware components and controllers are loaded as plugins (components) by the `` The ``controller_manager::ControllerManager`` has a node, the `controller_manager `_:octicon:`link-external`. - Hardware plugins are read from the ``robot_descritption`` parameter of the ``robot_state_publisher`` node and loaded at runtime. -- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. +- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. Hardware Plugin --------------- From afda9c9a34ef750bd8d9231e57e23b6ae754b2bb Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 19:04:12 +0000 Subject: [PATCH 6/7] moved controller config for standalone URDF use --- lbr_ros2_control/CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 8621fa41..837382d2 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -98,9 +98,4 @@ install( INCLUDES DESTINATION include ) -install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - ament_package() From 26f502233aa10b2986b4c2851fa4722bb38f2083 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 19 Nov 2024 20:22:54 +0000 Subject: [PATCH 7/7] updated documentation (#163) --- lbr_bringup/doc/lbr_bringup.rst | 2 +- lbr_bringup/launch/gazebo.launch.py | 2 +- lbr_bringup/lbr_bringup/ros2_control.py | 7 ++- .../config/admittance_control.yaml | 8 --- .../config/lbr_system_config.yaml | 37 ++++++++++++ .../doc/lbr_demos_advanced_cpp.rst | 58 +++++++++++++------ .../doc/lbr_demos_advanced_py.rst | 4 +- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 8 +-- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 8 +-- lbr_demos/lbr_moveit/doc/lbr_moveit.rst | 4 +- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- lbr_description/gazebo/lbr_gazebo.xacro | 2 +- .../ros2_control/lbr_controllers.yaml | 8 +-- lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- lbr_ros2_control/doc/lbr_ros2_control.rst | 2 +- 15 files changed, 103 insertions(+), 51 deletions(-) delete mode 100644 lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml create mode 100644 lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index c55bcb01..4fab2142 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -183,5 +183,5 @@ Troubleshooting --------------- Noisy Execution ~~~~~~~~~~~~~~~ -- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_:octicon:`link-external`. +- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml `_:octicon:`link-external`. - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username. diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index b6505ab4..d9f68126 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -13,7 +13,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/config/lbr_controllers.yaml + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index 3083ba10..f98ef127 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -19,7 +19,7 @@ def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: def arg_ctrl_cfg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg", - default_value="config/lbr_controllers.yaml", + default_value="ros2_control/lbr_controllers.yaml", description="Relative path from ctrl_cfg_pkg to the controllers.", ) @@ -30,6 +30,7 @@ def arg_ctrl() -> DeclareLaunchArgument: default_value="joint_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", choices=[ + "admittance_controller", "joint_trajectory_controller", "forward_position_controller", "lbr_joint_position_command_controller", @@ -82,11 +83,11 @@ def node_ros2_control( [ FindPackageShare( LaunchConfiguration( - "ctrl_cfg_pkg", default="lbr_ros2_control" + "ctrl_cfg_pkg", default="lbr_description" ) ), LaunchConfiguration( - "ctrl_cfg", default="config/lbr_controllers.yaml" + "ctrl_cfg", default="ros2_control/lbr_controllers.yaml" ), ] ), diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml deleted file mode 100644 index b356195d..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**/admittance_control: - ros__parameters: - base_link: "lbr_link_0" - end_effector_link: "lbr_link_ee" - f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] - dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - exp_smooth: 0.95 diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml new file mode 100644 index 00000000..165055de --- /dev/null +++ b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml @@ -0,0 +1,37 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + # exponential moving average time constant for joint position commands [s]: + # Set tau > robot sample time for more smoothing on the commands + # Set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.2 + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + # exponential moving average time constant for external joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for more smoothing on the external torque measurements + external_torque_tau: 0.4 + # exponential moving average time constant for joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for more smoothing on the raw torque measurements + measured_torque_tau: 0.4 + 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 + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 22e46da4..0b6fea60 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -10,16 +10,7 @@ lbr_demos_advanced_cpp Twist Controller ---------------- -TODO - -Admittance Controller ---------------------- -This demo implements a simple admittance controller. - -#. Client side configurations: - - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` +This demo uses the twist controller. #. Remote side configurations: @@ -39,16 +30,47 @@ This demo implements a simple admittance controller. .. code-block:: bash ros2 launch lbr_bringup hardware.launch.py \ - ctrl:=lbr_joint_position_command_controller \ + ctrl:=twist_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Launch the `admittance_control `_:octicon:`link-external`: +#. Next, publish to the ``/lbr/command/twist`` topic: - .. code-block:: bash - - ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml + .. code-block:: bash + + ros2 topic pub \ + --rate 100 \ + /lbr/command/twist \ + geometry_msgs/msg/Twist \ + "{linear: {x: 0.0, y: 0.0, z: 0.05}, angular: {x: 0.0, y: 0.0, z: 0.0}}" + +#. If you ``Ctrl+C`` the publisher, the ``twist_controller`` throws an error as it expects a continuous stream of twist commands. + +Admittance Controller +--------------------- +This demo uses the admittance controller. + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Launch the robot driver (please note that a different system configuration file is used with heavier smoothing!): + + .. code-block:: bash + + ros2 launch lbr_bringup hardware.launch.py \ + ctrl:=admittance_controller \ + sys_cfg_pkg:=lbr_demos_advanced_cpp \ + sys_cfg:=config/lbr_system_config.yaml \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Now gently move the robot at the end-effector. @@ -60,7 +82,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 8e23245f..2284202f 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -15,7 +15,7 @@ This demo implements a simple admittance controller. #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -55,7 +55,7 @@ This demo implements an admittance controller with a remote center of motion (RC #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 530e82f4..8f7b834d 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index df04a2e6..4d1258d4 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -15,7 +15,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -73,7 +73,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -97,7 +97,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -135,7 +135,7 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 02a35fbe..cdfdbb49 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -63,7 +63,7 @@ MoveIt Servo - Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -123,7 +123,7 @@ MoveIt via RViz - Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index 8856e511..f65da254 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -42,7 +42,7 @@ Hardware #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 637fc8bb..3c9fa1ab 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_description)/config/lbr_controllers.yaml + $(find lbr_description)/ros2_control/lbr_controllers.yaml /${robot_name} diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml index aae3b6ea..207113be 100644 --- a/lbr_description/ros2_control/lbr_controllers.yaml +++ b/lbr_description/ros2_control/lbr_controllers.yaml @@ -97,15 +97,15 @@ ros__parameters: robot_name: lbr admittance: - mass: 0.2 - damping: 0.1 + mass: 0.01 + damping: 1.0 stiffness: 0.0 inv_jac_ctrl: 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 + max_linear_velocity: 2.0 # maximum linear velocity + max_angular_velocity: 2.0 # maximum linear acceleration /**/twist_controller: ros__parameters: diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index f4fba85c..50d95fb3 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -90,7 +90,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. In the `move_group.launch.py `_:octicon:`link-external` use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` + #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 986895ae..7e74b5b3 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -30,7 +30,7 @@ Hardware components and controllers are loaded as plugins (components) by the `` The ``controller_manager::ControllerManager`` has a node, the `controller_manager `_:octicon:`link-external`. - Hardware plugins are read from the ``robot_descritption`` parameter of the ``robot_state_publisher`` node and loaded at runtime. -- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. +- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. Hardware Plugin ---------------