Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add wrench offset for admittance controller #1249

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

Add wrench offset for admittance controller #1249

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
30b4407
wired in a simple version of the wrench target
Aug 14, 2024
8335398
formatting
Aug 15, 2024
64eaea4
Apply suggestions from code review
firesurfer Aug 16, 2024
e260c69
Merge branch 'master' into feature/admittance_wrench_target_rolling
firesurfer Aug 21, 2024
9bb9435
Apply suggestions from code review
firesurfer Aug 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions admittance_controller/include/admittance_controller/admittance_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// ROS subscribers
rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr
input_joint_command_subscriber_;
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
input_wrench_command_subscriber_;
rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_;

// admittance parameters
Expand All @@ -144,6 +146,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
// real-time buffer
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
input_joint_command_;
realtime_tools::RealtimeBuffer<geometry_msgs::msg::WrenchStamped> input_wrench_command_;
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;

trajectory_msgs::msg::JointTrajectoryPoint last_commanded_;
Expand Down
36 changes: 35 additions & 1 deletion admittance_controller/src/admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,23 @@

namespace admittance_controller
{

static geometry_msgs::msg::Wrench add_wrenches(
firesurfer marked this conversation as resolved.
Show resolved Hide resolved
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;
}

firesurfer marked this conversation as resolved.
Show resolved Hide resolved
controller_interface::CallbackReturn AdmittanceController::on_init()
{
// initialize controller config
Expand Down Expand Up @@ -116,6 +133,7 @@ AdmittanceController::on_export_reference_interfaces()
reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits<double>::quiet_NaN());
position_reference_ = {};
velocity_reference_ = {};
input_wrench_command_.reset();

// assign reference interfaces
auto index = 0ul;
Expand Down Expand Up @@ -265,6 +283,20 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
input_joint_command_subscriber_ =
get_node()->create_subscription<trajectory_msgs::msg::JointTrajectoryPoint>(
"~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback);

input_wrench_command_subscriber_ =
get_node()->create_subscription<geometry_msgs::msg::WrenchStamped>(
"~/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(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why don't we transform the wrench if it is in another frame to the sensor frame?

Copy link
Contributor Author

@firesurfer firesurfer Sep 3, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I had some very weird results in the past when transforming wrenches with TF (e.g. non zero torques afterwards when only forces where given). Additionally I don't think we have a tf buffer/listener in the controller at the moment and why add this additional overhead?

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);
firesurfer marked this conversation as resolved.
Show resolved Hide resolved
return;
firesurfer marked this conversation as resolved.
Show resolved Hide resolved
}
input_wrench_command_.writeFromNonRT(msg);
});
s_publisher_ = get_node()->create_publisher<control_msgs::msg::AdmittanceControllerState>(
"~/status", rclcpp::SystemDefaultsQoS());
state_publisher_ =
Expand Down Expand Up @@ -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_);
Expand Down
Loading