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"