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