diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..001f9359e7 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Subscription::SharedPtr + input_wrench_command_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; // admittance parameters @@ -144,6 +146,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; + realtime_tools::RealtimeBuffer input_wrench_command_; std::unique_ptr> state_publisher_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..dbe4c0b1d2 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -27,6 +27,23 @@ namespace admittance_controller { + +geometry_msgs::msg::Wrench add_wrenches( + const geometry_msgs::msg::Wrench & a, const geometry_msgs::msg::Wrench & b) +{ + geometry_msgs::msg::Wrench res; + + res.force.x = a.force.x + b.force.x; + res.force.y = a.force.y + b.force.y; + res.force.z = a.force.z + b.force.z; + + res.torque.x = a.torque.x + b.torque.x; + res.torque.y = a.torque.y + b.torque.y; + res.torque.z = a.torque.z + b.torque.z; + + return res; +} + controller_interface::CallbackReturn AdmittanceController::on_init() { // initialize controller config @@ -116,6 +133,7 @@ AdmittanceController::on_export_reference_interfaces() reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); position_reference_ = {}; velocity_reference_ = {}; + input_wrench_command_.reset(); // assign reference interfaces auto index = 0ul; @@ -265,6 +283,20 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + + input_wrench_command_subscriber_ = + get_node()->create_subscription( + "~/wrench_reference", rclcpp::SystemDefaultsQoS(), + [&](const geometry_msgs::msg::WrenchStamped & msg) + { + if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty()) + { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: " << admittance_->parameters_.ft_sensor.frame_id); + return; + } + input_wrench_command_.writeFromNonRT(msg); + }); s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -396,8 +428,10 @@ controller_interface::return_type AdmittanceController::update_and_write_command // get all controller inputs read_state_from_hardware(joint_state_, ft_values_); + auto offsetted_ft_values = add_wrenches(ft_values_, input_wrench_command_.readFromRT()->wrench); + // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + admittance_->update(joint_state_, offsetted_ft_values, reference_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_);