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

Publish wrench state feedback #187

Open
wants to merge 1 commit into
base: ros1
Choose a base branch
from
Open

Publish wrench state feedback #187

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
17 changes: 17 additions & 0 deletions cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,14 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte
std::string m_new_ft_sensor_ref;
void setFtSensorReferenceFrame(const std::string& new_ref);

/**
* @brief Publish the controller's wrenches (sensor, gravity, target and net force)
*
* The data are w.r.t. the specified robot base link.
*/
void publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
ctrl::Vector6D& wrench);

private:
ctrl::Vector6D compensateGravity();

Expand Down Expand Up @@ -136,6 +144,15 @@ class CartesianForceController : public virtual cartesian_controller_base::Carte

std::shared_ptr<dynamic_reconfigure::Server<Config> > m_dyn_conf_server;
dynamic_reconfigure::Server<Config>::CallbackType m_callback_type;

realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
m_feedback_gravity_wrench_publisher;
realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
m_feedback_sensor_wrench_publisher;
realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
m_feedback_target_wrench_publisher;
realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>
m_feedback_net_force_wrench_publisher;
};

}
Expand Down
58 changes: 56 additions & 2 deletions cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,23 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
m_target_wrench.setZero();
m_ft_sensor_wrench.setZero();

// Controller-internal state publishing
m_feedback_gravity_wrench_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
nh, "current_gravity_wrench", 3);

m_feedback_sensor_wrench_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
nh, "current_sensor_wrench", 3);

m_feedback_target_wrench_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
nh, "current_target_wrench", 3);

m_feedback_net_force_wrench_publisher =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> >(
nh, "current_net_force_wrench", 3);

// Connect dynamic reconfigure and overwrite the default values with values
// on the parameter server. This is done automatically if parameters with
// the according names exist.
Expand Down Expand Up @@ -185,10 +202,28 @@ computeForceError()
target_wrench = m_target_wrench;
}

ctrl::Vector6D sensor_wrench;
sensor_wrench = Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref);

ctrl::Vector6D gravity_wrench;
gravity_wrench = compensateGravity();

// Superimpose target wrench and sensor wrench in base frame
return Base::displayInBaseLink(m_ft_sensor_wrench,m_new_ft_sensor_ref)
ctrl::Vector6D net_force_wrench;
net_force_wrench = sensor_wrench
+ target_wrench
+ compensateGravity();
+ gravity_wrench;

// Publish wrench state feedback
if (Base::m_publish_state_feedback)
{
publishStateWrenchFeedback(m_feedback_sensor_wrench_publisher, sensor_wrench);
publishStateWrenchFeedback(m_feedback_target_wrench_publisher, target_wrench);
publishStateWrenchFeedback(m_feedback_gravity_wrench_publisher, gravity_wrench);
publishStateWrenchFeedback(m_feedback_net_force_wrench_publisher, net_force_wrench);
}

return net_force_wrench;
}

template <class HardwareInterface>
Expand Down Expand Up @@ -295,6 +330,25 @@ signalTaringCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Respons
return true;
}

template <class HardwareInterface>
void CartesianForceController<HardwareInterface>::
publishStateWrenchFeedback(realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::WrenchStamped>& rt_publisher,
ctrl::Vector6D& wrench)
{
if (rt_publisher->trylock()){
rt_publisher->msg_.header.stamp = ros::Time::now();
rt_publisher->msg_.header.frame_id = Base::m_robot_base_link;
rt_publisher->msg_.wrench.force.x = wrench[0];
rt_publisher->msg_.wrench.force.y = wrench[1];
rt_publisher->msg_.wrench.force.z = wrench[2];
rt_publisher->msg_.wrench.torque.x = wrench[3];
rt_publisher->msg_.wrench.torque.y = wrench[4];
rt_publisher->msg_.wrench.torque.z = wrench[5];

rt_publisher->unlockAndPublish();
}
}

template <class HardwareInterface>
void CartesianForceController<HardwareInterface>::dynamicReconfigureCallback(Config& config,
uint32_t level)
Expand Down