From 7c00ac916eb80fa356e885bed9631506a1601077 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 13:22:30 +0000 Subject: [PATCH] added multi-dof gains (#163) --- .../ros2_control/lbr_controllers.yaml | 10 +- lbr_fri_ros2/include/lbr_fri_ros2/control.hpp | 42 ++++++--- lbr_fri_ros2/src/control.cpp | 69 ++++++++++---- .../src/controllers/admittance_controller.cpp | 94 +++++++++++++++++-- .../src/controllers/twist_controller.cpp | 36 ++++++- 5 files changed, 206 insertions(+), 45 deletions(-) diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml index 207113be..9a2a1a3d 100644 --- a/lbr_description/ros2_control/lbr_controllers.yaml +++ b/lbr_description/ros2_control/lbr_controllers.yaml @@ -97,15 +97,17 @@ ros__parameters: robot_name: lbr admittance: - mass: 0.01 - damping: 1.0 - stiffness: 0.0 + mass: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + damping: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + stiffness: [0.0, 0.0, 0.0, 0.0, 0.0, 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: 2.0 # maximum linear velocity max_angular_velocity: 2.0 # maximum linear acceleration + joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains + cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains /**/twist_controller: ros__parameters: @@ -117,4 +119,6 @@ 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 + joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains + cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains timeout: 0.2 # stop controller if no command is received within this time [s] diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp index bab95773..128f9b85 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -17,12 +17,14 @@ 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; + std::string chain_root = "lbr_link_0"; + std::string chain_tip = "lbr_link_ee"; + bool twist_in_tip_frame = true; + double damping = 0.2; + double max_linear_velocity = 0.1; + double max_angular_velocity = 0.1; + jnt_array_t joint_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + cart_array_t cartesian_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; }; class InvJacCtrlImpl { @@ -44,6 +46,7 @@ class InvJacCtrlImpl { protected: void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); + void set_all_zero_(); InvJacCtrlParameters parameters_; @@ -51,26 +54,30 @@ class InvJacCtrlImpl { std::unique_ptr kinematics_ptr_; Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; + Eigen::Matrix q_gains_; + Eigen::Matrix x_gains_; }; struct AdmittanceParameters { AdmittanceParameters() = delete; - AdmittanceParameters(const double &m = 1.0, const double &b = 0.1, const double &k = 0.0) + AdmittanceParameters(const_cart_array_t_ref m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}, + const_cart_array_t_ref b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + const_cart_array_t_ref k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) : m(m), b(b), k(k) { - if (m <= 0.0) { + if (std::any_of(m.cbegin(), m.cend(), [](const double &m_i) { return m_i <= 0.0; })) { throw std::runtime_error("Mass must be positive and greater zero."); } - if (b < 0.0) { + if (std::any_of(b.cbegin(), b.cend(), [](const double &b_i) { return b_i < 0.0; })) { throw std::runtime_error("Damping must be positive."); } - if (k < 0.0) { + if (std::any_of(k.cbegin(), k.cend(), [](const double &k_i) { return k_i < 0.0; })) { throw std::runtime_error("Stiffness must be positive."); } } - double m = 1.0; - double b = 0.1; - double k = 0.0; + cart_array_t m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; + cart_array_t b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; + cart_array_t k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; }; class AdmittanceImpl { @@ -78,7 +85,12 @@ class AdmittanceImpl { static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl"; public: - AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) {} + AdmittanceImpl() = delete; + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) { + m_ = Eigen::Map>(parameters_.m.data()); + b_ = Eigen::Map>(parameters_.b.data()); + k_ = Eigen::Map>(parameters_.k.data()); + } void compute(const Eigen::Matrix &f_ext, const Eigen::Matrix &x, @@ -89,6 +101,8 @@ class AdmittanceImpl { protected: AdmittanceParameters parameters_; + + Eigen::Matrix m_, b_, k_; }; } // 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 66ba5284..30e61cbb 100644 --- a/lbr_fri_ros2/src/control.cpp +++ b/lbr_fri_ros2/src/control.cpp @@ -6,6 +6,10 @@ InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description, : parameters_(parameters) { kinematics_ptr_ = std::make_unique(robot_description, parameters_.chain_root, parameters_.chain_tip); + set_all_zero_(); + q_gains_ = Eigen::Map>(parameters_.joint_gains.data()); + x_gains_ = + Eigen::Map>(parameters_.cartesian_gains.data()); } void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, @@ -56,6 +60,43 @@ void InvJacCtrlImpl::log_info() const { parameters_.max_linear_velocity); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f", parameters_.max_angular_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Joint gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.joint_gains[0], parameters_.joint_gains[1], parameters_.joint_gains[2], + parameters_.joint_gains[3], parameters_.joint_gains[4], parameters_.joint_gains[5], + parameters_.joint_gains[6]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Cartesian gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.cartesian_gains[0], parameters_.cartesian_gains[1], + parameters_.cartesian_gains[2], parameters_.cartesian_gains[3], + parameters_.cartesian_gains[4], parameters_.cartesian_gains[5]); +} + +void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + if ((m_.array() < 0.).any()) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + ddx = (f_ext - b_.asDiagonal() * dx - k_.asDiagonal() * x).array() / m_.array(); +} + +void AdmittanceImpl::log_info() const { + { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.m[0], parameters_.m[1], parameters_.m[2], parameters_.m[3], + parameters_.m[4], parameters_.m[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Damping: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.b[0], + parameters_.b[1], parameters_.b[2], parameters_.b[3], parameters_.b[4], + parameters_.b[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Stiffness: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.k[0], + parameters_.k[1], parameters_.k[2], parameters_.k[3], parameters_.k[4], + parameters_.k[5]); + } } void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { @@ -76,30 +117,22 @@ void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); } + twist_target_ = x_gains_.asDiagonal() * twist_target_; + // 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_; + Eigen::Map>(dq.data()) = + q_gains_.asDiagonal() * 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); - } +void InvJacCtrlImpl::set_all_zero_() { + std::fill(q_.begin(), q_.end(), 0.0); + jacobian_inv_.setZero(); + twist_target_.setZero(); + q_gains_.setZero(); + x_gains_.setZero(); } } // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 0178285c..bbe950b6 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -39,14 +39,21 @@ AdmittanceController::state_interface_configuration() const { 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("admittance.mass", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); + this->get_node()->declare_parameter("admittance.damping", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + this->get_node()->declare_parameter("admittance.stiffness", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 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); + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); configure_joint_names_(); configure_admittance_impl_(); configure_inv_jac_ctrl_impl_(); @@ -201,14 +208,82 @@ void AdmittanceController::configure_joint_names_() { } void AdmittanceController::configure_admittance_impl_() { - 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()}); + if (this->get_node()->get_parameter("admittance.mass").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of mass values (%ld) does not match the number of cartesian degrees of " + "freedom (%d).", + this->get_node()->get_parameter("admittance.mass").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + if (this->get_node()->get_parameter("admittance.damping").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of damping values (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("admittance.damping").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + if (this->get_node()->get_parameter("admittance.stiffness").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of stiffness values (%ld) does not match the number of cartesian degrees " + "of freedom " + "(%d).", + this->get_node()->get_parameter("admittance.stiffness").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + lbr_fri_ros2::cart_array_t mass_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + mass_array[i] = this->get_node()->get_parameter("admittance.mass").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t damping_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + damping_array[i] = this->get_node()->get_parameter("admittance.damping").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t stiffness_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + stiffness_array[i] = + this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i]; + } + admittance_impl_ptr_ = std::make_unique( + lbr_fri_ros2::AdmittanceParameters{mass_array, damping_array, stiffness_array}); } void AdmittanceController::configure_inv_jac_ctrl_impl_() { + if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() != + lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), + lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint gains."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure cartesian gains."); + } + lbr_fri_ros2::jnt_array_t joint_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t cartesian_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } inv_jac_ctrl_impl_ptr_ = std::make_unique( this->get_robot_description(), lbr_fri_ros2::InvJacCtrlParameters{ @@ -217,7 +292,8 @@ void AdmittanceController::configure_inv_jac_ctrl_impl_() { 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()}); + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); } void AdmittanceController::zero_all_values_() { diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 3b3befdf..d851f165 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -41,6 +41,10 @@ controller_interface::CallbackReturn TwistController::on_init() { 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("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); this->get_node()->declare_parameter("timeout", 0.2); configure_joint_names_(); configure_inv_jac_ctrl_impl_(); @@ -166,6 +170,35 @@ void TwistController::configure_joint_names_() { } void TwistController::configure_inv_jac_ctrl_impl_() { + if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() != + lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), + lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint gains."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure cartesian gains."); + } + lbr_fri_ros2::jnt_array_t joint_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t cartesian_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } inv_jac_ctrl_impl_ptr_ = std::make_unique( this->get_robot_description(), lbr_fri_ros2::InvJacCtrlParameters{ @@ -174,7 +207,8 @@ void TwistController::configure_inv_jac_ctrl_impl_() { 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()}); + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); } void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); }